C++ CMSG_DATA函数代码示例

bool Connection::sendOutgoingMessage(std::unique_ptr<MessageEncoder> encoder)
{
    COMPILE_ASSERT(sizeof(MessageInfo) + attachmentMaxAmount * sizeof(size_t) <= messageMaxSize, AttachmentsFitToMessageInline);

    Vector<Attachment> attachments = encoder->releaseAttachments();
    if (attachments.size() > (attachmentMaxAmount - 1)) {
        ASSERT_NOT_REACHED();
        return false;
    }

    MessageInfo messageInfo(encoder->bufferSize(), attachments.size());
    size_t messageSizeWithBodyInline = sizeof(messageInfo) + (attachments.size() * sizeof(AttachmentInfo)) + encoder->bufferSize();
    if (messageSizeWithBodyInline > messageMaxSize && encoder->bufferSize()) {
        RefPtr<WebKit::SharedMemory> oolMessageBody = WebKit::SharedMemory::allocate(encoder->bufferSize());
        if (!oolMessageBody)
            return false;

        WebKit::SharedMemory::Handle handle;
        if (!oolMessageBody->createHandle(handle, WebKit::SharedMemory::Protection::ReadOnly))
            return false;

        messageInfo.setMessageBodyIsOutOfLine();

        memcpy(oolMessageBody->data(), encoder->buffer(), encoder->bufferSize());

        attachments.append(handle.releaseAttachment());
    }

    struct msghdr message;
    memset(&message, 0, sizeof(message));

    struct iovec iov[3];
    memset(&iov, 0, sizeof(iov));

    message.msg_iov = iov;
    int iovLength = 1;

    iov[0].iov_base = reinterpret_cast<void*>(&messageInfo);
    iov[0].iov_len = sizeof(messageInfo);

    std::unique_ptr<AttachmentInfo[]> attachmentInfo;
    MallocPtr<char> attachmentFDBuffer;

    if (!attachments.isEmpty()) {
        int* fdPtr = 0;

        size_t attachmentFDBufferLength = std::count_if(attachments.begin(), attachments.end(),
            [](const Attachment& attachment) {
                return attachment.fileDescriptor() != -1;
            });

        if (attachmentFDBufferLength) {
            attachmentFDBuffer = MallocPtr<char>::malloc(sizeof(char) * CMSG_SPACE(sizeof(int) * attachmentFDBufferLength));

            message.msg_control = attachmentFDBuffer.get();
            message.msg_controllen = CMSG_SPACE(sizeof(int) * attachmentFDBufferLength);
            memset(message.msg_control, 0, message.msg_controllen);

            struct cmsghdr* cmsg = CMSG_FIRSTHDR(&message);
            cmsg->cmsg_level = SOL_SOCKET;
            cmsg->cmsg_type = SCM_RIGHTS;
            cmsg->cmsg_len = CMSG_LEN(sizeof(int) * attachmentFDBufferLength);

            fdPtr = reinterpret_cast<int*>(CMSG_DATA(cmsg));
        }

        attachmentInfo = std::make_unique<AttachmentInfo[]>(attachments.size());
        int fdIndex = 0;
        for (size_t i = 0; i < attachments.size(); ++i) {
            attachmentInfo[i].setType(attachments[i].type());

            switch (attachments[i].type()) {
            case Attachment::MappedMemoryType:
                attachmentInfo[i].setSize(attachments[i].size());
                // Fall trhough, set file descriptor or null.
            case Attachment::SocketType:
                if (attachments[i].fileDescriptor() != -1) {
                    ASSERT(fdPtr);
                    fdPtr[fdIndex++] = attachments[i].fileDescriptor();
                } else
                    attachmentInfo[i].setNull();
                break;
            case Attachment::Uninitialized:
            default:
                break;
            }
        }

        iov[iovLength].iov_base = attachmentInfo.get();
        iov[iovLength].iov_len = sizeof(AttachmentInfo) * attachments.size();
        ++iovLength;
    }

    if (!messageInfo.isMessageBodyIsOutOfLine() && encoder->bufferSize()) {
        iov[iovLength].iov_base = reinterpret_cast<void*>(encoder->buffer());
        iov[iovLength].iov_len = encoder->bufferSize();
        ++iovLength;
    }

    message.msg_iovlen = iovLength;
//.........这里部分代码省略.........

//RY: start here
void udp_xmit6 (struct buffer6 *buf, struct tunnel6 *t)
{
	struct cmsghdr *cmsg;
    char cbuf[CMSG_SPACE(sizeof (unsigned int))];
    unsigned int *refp;
    struct msghdr msgh;
    int err;
    struct iovec iov;
//RY: start, for testing
    struct sockaddr_in6 clientaddr;
    struct iovec iov1[1];
    char a[10];
//RY: end
    /*
     * OKAY, now send a packet with the right SAref values.
     */
    memset(&msgh, 0, sizeof(struct msghdr));

    msgh.msg_control = cbuf;
    msgh.msg_controllen = 0;
//TODO: need to take decision on IPSEC
    if(gconfig.ipsecsaref && t->refhim != IPSEC_SAREF_NULL) {
	msgh.msg_controllen = sizeof(cbuf);

	cmsg = CMSG_FIRSTHDR(&msgh);
	cmsg->cmsg_level = SOL_IP;
	//RY: start
	//cmsg->cmsg_type  = IP_IPSEC_REFINFO;
	cmsg->cmsg_type  = IPV6_DSTOPTS;	//RY: updated for IPV6 com.
	//RY: end
	cmsg->cmsg_len   = CMSG_LEN(sizeof(unsigned int));

	if(gconfig.debug_network) {
		l2tp_log(LOG_DEBUG,"sending with saref=%d\n", t->refhim);
	}
	refp = (unsigned int *)CMSG_DATA(cmsg);
	*refp = t->refhim;

	msgh.msg_controllen = cmsg->cmsg_len;
    }

    iov.iov_base = buf->start;
    iov.iov_len  = buf->len;

    /* return packet from whence it came */
    //RY: start
    inet_pton(AF_INET6, "2003::1", &buf->peer.sin6_addr);
    buf->peer.sin6_family = AF_INET6;
    buf->peer.sin6_port = htons(1701);
    //RY: end
    msgh.msg_name    = &buf->peer;
    msgh.msg_namelen = sizeof(buf->peer);

    msgh.msg_iov  = &iov;
    msgh.msg_iovlen = 1;
    msgh.msg_flags = 0;

#if 0
    //RY: start
    inet_pton(AF_INET6, "2005::1", &buf->peer.sin6_addr);

    printf("dest addr:%s\n", IPADDY6(buf->peer.sin6_addr));
    buf->peer.sin6_family = AF_INET6;
    buf->peer.sin6_port = htons(23156);

    msgh.msg_name = &buf->peer;

    iov.iov_base = a;
    iov.iov_len = sizeof(a);
    msgh.msg_iov->iov_base = &iov;
    msgh.msg_iovlen = 1;
    msgh.msg_control = NULL;
    msgh.msg_controllen = 0;
    msgh.msg_flags = 0;

//#ifdef RY:
//    memset(&clientaddr, 0, sizeof(clientaddr));
//    	inet_pton(AF_INET6,"2005::1", &clientaddr.sin6_addr);
//    	clientaddr.sin6_port = htons(1701);
//    	clientaddr.sin6_family = AF_INET6;
//    memset(a, 'a', sizeof(a));
//        for(;;)
//          sendto(server_socket6, a, sizeof(a), 0,
//  							&clientaddr, sizeof(clientaddr));
 //   for(;;)
#endif
    //for(;;)
    if((err = sendmsg(server_socket6, &msgh, 0)) < 0)
	{
    	//RY: start
    	printf("udp_xmit6 failed with err=%d:%s\n",err,strerror(errno));
		//RY: end
		l2tp_log(LOG_ERR, "udp_xmit6 failed with err=%d:%s\n",
    			err,strerror(errno));
    }//RY: start
    else
    {
    	printf("sent:bytes = %d\n", err);
    //sleep(1)
//.........这里部分代码省略.........

ssize_t 
recvfrom_flags(int fd, void *ptr, size_t nbytes, int *flagsp,
               struct sockaddr *sa, socklen_t *salenptr, struct my_in_pktinfo *pktp, u_char *ttl)
{
    struct msghdr   msg;
    struct iovec    iov[1];
    ssize_t         n;

#ifdef CMSG_FIRSTHDR
    struct cmsghdr  *cmptr;
    union {
      struct cmsghdr    cm;
      char              control[1024];
    } control_un;

	*ttl = 255;			// If kernel fails to provide TTL data then assume the TTL was 255 as it should be

    msg.msg_control = control_un.control;
    msg.msg_controllen = sizeof(control_un.control);
    msg.msg_flags = 0;
#else
    memset(&msg, 0, sizeof(msg));   /* make certain msg_accrightslen = 0 */
#endif /* CMSG_FIRSTHDR */

    msg.msg_name = (char *) sa;
    msg.msg_namelen = *salenptr;
    iov[0].iov_base = (char *)ptr;
    iov[0].iov_len = nbytes;
    msg.msg_iov = iov;
    msg.msg_iovlen = 1;

    if ( (n = recvmsg(fd, &msg, *flagsp)) < 0)
        return(n);

    *salenptr = msg.msg_namelen;    /* pass back results */
    if (pktp) {
        /* 0.0.0.0, i/f = -1 */
        /* We set the interface to -1 so that the caller can 
           tell whether we returned a meaningful value or 
           just some default.  Previously this code just 
           set the value to 0, but I'm concerned that 0 
           might be a valid interface value.
        */
        memset(pktp, 0, sizeof(struct my_in_pktinfo));
        pktp->ipi_ifindex = -1;
    }
/* end recvfrom_flags1 */

/* include recvfrom_flags2 */
#ifndef CMSG_FIRSTHDR
	#warning CMSG_FIRSTHDR not defined. Will not be able to determine destination address, received interface, etc.
    *flagsp = 0;                    /* pass back results */
    return(n);
#else

    *flagsp = msg.msg_flags;        /* pass back results */
    if (msg.msg_controllen < (socklen_t)sizeof(struct cmsghdr) ||
        (msg.msg_flags & MSG_CTRUNC) || pktp == NULL)
        return(n);

    for (cmptr = CMSG_FIRSTHDR(&msg); cmptr != NULL;
         cmptr = CMSG_NXTHDR(&msg, cmptr)) {

#ifdef  IP_PKTINFO
#if in_pktinfo_definition_is_missing
struct in_pktinfo
{
        int             ipi_ifindex;
        struct in_addr  ipi_spec_dst;
        struct in_addr  ipi_addr;
};
#endif
        if (cmptr->cmsg_level == IPPROTO_IP && 
            cmptr->cmsg_type == IP_PKTINFO) {
            struct in_pktinfo *tmp;
            struct sockaddr_in *sin = (struct sockaddr_in*)&pktp->ipi_addr;
            
            tmp = (struct in_pktinfo *) CMSG_DATA(cmptr);
            sin->sin_family = AF_INET;
            sin->sin_addr = tmp->ipi_addr;
            sin->sin_port = 0;
            pktp->ipi_ifindex = tmp->ipi_ifindex;
            continue;
        }
#endif

#ifdef  IP_RECVDSTADDR
        if (cmptr->cmsg_level == IPPROTO_IP &&
            cmptr->cmsg_type == IP_RECVDSTADDR) {
            struct sockaddr_in *sin = (struct sockaddr_in*)&pktp->ipi_addr;
            
            sin->sin_family = AF_INET;
            sin->sin_addr = *(struct in_addr*)CMSG_DATA(cmptr);
            sin->sin_port = 0;
            continue;
        }
#endif

#ifdef  IP_RECVIF
        if (cmptr->cmsg_level == IPPROTO_IP &&
//.........这里部分代码省略.........

static int32_t
qb_ipcs_uc_recv_and_auth(int32_t sock, void *msg, size_t len,
			 struct ipc_auth_ugp *ugp)
{
	int32_t res = 0;
	struct msghdr msg_recv;
	struct iovec iov_recv;

#ifdef SO_PASSCRED
	char cmsg_cred[CMSG_SPACE(sizeof(struct ucred))];
	int off = 0;
	int on = 1;
#endif
	msg_recv.msg_iov = &iov_recv;
	msg_recv.msg_iovlen = 1;
	msg_recv.msg_name = 0;
	msg_recv.msg_namelen = 0;
#ifdef SO_PASSCRED
	msg_recv.msg_control = (void *)cmsg_cred;
	msg_recv.msg_controllen = sizeof(cmsg_cred);
#endif
#ifdef QB_SOLARIS
	msg_recv.msg_accrights = 0;
	msg_recv.msg_accrightslen = 0;
#else
	msg_recv.msg_flags = 0;
#endif /* QB_SOLARIS */

	iov_recv.iov_base = msg;
	iov_recv.iov_len = len;
#ifdef SO_PASSCRED
	setsockopt(sock, SOL_SOCKET, SO_PASSCRED, &on, sizeof(on));
#endif

	res = qb_ipc_us_recv_msghdr(sock, &msg_recv, msg, len);
	if (res < 0) {
		goto cleanup_and_return;
	}
	if (res != len) {
		res = -EIO;
		goto cleanup_and_return;
	}

	/*
	 * currently support getpeerucred, getpeereid, and SO_PASSCRED credential
	 * retrieval mechanisms for various Platforms
	 */
#ifdef HAVE_GETPEERUCRED
	/*
	 * Solaris and some BSD systems
	 */
	{
		ucred_t *uc = NULL;

		if (getpeerucred(sock, &uc) == 0) {
			res = 0;
			ugp->uid = ucred_geteuid(uc);
			ugp->gid = ucred_getegid(uc);
			ugp->pid = ucred_getpid(uc);
			ucred_free(uc);
		} else {
			res = -errno;
		}
	}
#elif HAVE_GETPEEREID
	/*
	 * Usually MacOSX systems
	 */
	{
		/*
		 * TODO get the peer's pid.
		 * c->pid = ?;
		 */
		if (getpeereid(sock, &ugp->uid, &ugp->gid) == 0) {
			res = 0;
		} else {
			res = -errno;
		}
	}

#elif SO_PASSCRED
	/*
	 * Usually Linux systems
	 */
	{
		struct ucred cred;
		struct cmsghdr *cmsg;

		res = -EINVAL;
		for (cmsg = CMSG_FIRSTHDR(&msg_recv); cmsg != NULL;
		     cmsg = CMSG_NXTHDR(&msg_recv, cmsg)) {
			if (cmsg->cmsg_type != SCM_CREDENTIALS)
				continue;

			memcpy(&cred, CMSG_DATA(cmsg), sizeof(struct ucred));
			res = 0;
			ugp->pid = cred.pid;
			ugp->uid = cred.uid;
			ugp->gid = cred.gid;
			break;
//.........这里部分代码省略.........

static int
ping_recv (PING * p)
{
  int dupflag, n;
  int hops = -1;
  struct msghdr msg;
  struct iovec iov;
  struct icmp6_hdr *icmp6;
  struct cmsghdr *cmsg;
  char cmsg_data[1024];

  iov.iov_base = p->ping_buffer;
  iov.iov_len = _PING_BUFLEN (p, USE_IPV6);
  msg.msg_name = &p->ping_from.ping_sockaddr6;
  msg.msg_namelen = sizeof (p->ping_from.ping_sockaddr6);
  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = cmsg_data;
  msg.msg_controllen = sizeof (cmsg_data);
  msg.msg_flags = 0;

  n = recvmsg (p->ping_fd, &msg, 0);
  if (n < 0)
    return -1;

  for (cmsg = CMSG_FIRSTHDR (&msg); cmsg; cmsg = CMSG_NXTHDR (&msg, cmsg))
    {
      if (cmsg->cmsg_level == IPPROTO_IPV6
	  && cmsg->cmsg_type == IPV6_HOPLIMIT)
	{
	  hops = *(int *) CMSG_DATA (cmsg);
	  break;
	}
    }

  icmp6 = (struct icmp6_hdr *) p->ping_buffer;
  if (icmp6->icmp6_type == ICMP6_ECHO_REPLY)
    {
      /* We got an echo reply.  */
      if (ntohs (icmp6->icmp6_id) != p->ping_ident)
	return -1;		/* It's not a response to us.  */

      if (_PING_TST (p, ntohs (icmp6->icmp6_seq)))
	{
	  /* We already got the reply for this echo request.  */
	  p->ping_num_rept++;
	  dupflag = 1;
	}
      else
	{
	  _PING_SET (p, ntohs (icmp6->icmp6_seq));
	  p->ping_num_recv++;
	  dupflag = 0;
	}

      print_echo (dupflag, hops, p->ping_closure, &p->ping_dest.ping_sockaddr6,
		  &p->ping_from.ping_sockaddr6, icmp6, n);

    }
  else
    {
      /* We got an error reply.  */
      if (!my_echo_reply (p, icmp6))
	return -1;		/* It's not for us.  */

      print_icmp_error (&p->ping_from.ping_sockaddr6, icmp6, n);
    }

  return 0;
}

int SP_ProcPduUtils :: recv_fd( int sockfd )
{
	struct msghdr msg;
	struct iovec iov[1];

#ifdef	HAVE_MSGHDR_MSG_CONTROL
	union
	{
		cmsghdr cm;
		char control[CMSG_SPACE(sizeof(int))];
	} control_un;
	struct cmsghdr *cmptr;

	msg.msg_control = control_un.control;
	msg.msg_controllen = sizeof(control_un.control);
#else
	int newfd;

	msg.msg_accrights = (caddr_t) &newfd;
	msg.msg_accrightslen = sizeof(int);
#endif

	msg.msg_name = NULL;
	msg.msg_namelen = 0;

	char buf[1];
	iov[0].iov_base = buf;
	iov[0].iov_len = 1;
	msg.msg_iov = iov;
	msg.msg_iovlen = 1;

	for( ; ; ) {
		errno = 0;
		if (recvmsg(sockfd, &msg, 0) <= 0) {
			if( EINTR == errno ) {
				continue;
			} else {
				return -1;
			}
		} else {
			break;
		}
	}

#ifdef	HAVE_MSGHDR_MSG_CONTROL
	if ((cmptr = CMSG_FIRSTHDR(&msg)) != NULL && cmptr->cmsg_len == CMSG_LEN(sizeof(int)))
	{
		if (cmptr->cmsg_level != SOL_SOCKET)
		{
			syslog( LOG_WARNING,"control level != SOL_SOCKET");
			return -1;
		}
		if (cmptr->cmsg_type != SCM_RIGHTS)
		{
			syslog( LOG_WARNING,"control type != SCM_RIGHTS");
			return -1;
		}
		return *((int *) CMSG_DATA(cmptr));
	}
	else
		return -1; /* descriptor was not passed */
#else
	/* *INDENT-OFF* */
	if (msg.msg_accrightslen == sizeof(int))
		return newfd;
	else
		return -1; /* descriptor was not passed */
	/* *INDENT-ON* */
#endif

/*
	char tmpbuf[CONTROLLEN];
	struct cmsghdr *cmptr = (struct cmsghdr *) tmpbuf;
	struct iovec iov[1];
	struct msghdr msg;
	char buf[1];

	iov[0].iov_base = buf;
	iov[0].iov_len = sizeof (buf);
	msg.msg_iov = iov;
	msg.msg_iovlen = 1;
	msg.msg_name = NULL;
	msg.msg_namelen = 0;

	msg.msg_control = cmptr;
	msg.msg_controllen = CONTROLLEN;

	for( ; ; ) {
		errno = 0;
		if (recvmsg(sockfd, &msg, 0) <= 0) {
			if( EINTR == errno ) {
				continue;
			} else {
				return -1;
			}
		} else {
			break;
		}
	}

//.........这里部分代码省略.........

/* There is a lot of hair here because the alignment rules (and
 * thus placement) of cmsg headers and length are different for
 * 32-bit apps.  -DaveM
 */
int cmsghdr_from_user_compat_to_kern(struct msghdr *kmsg, struct sock *sk,
			       unsigned char *stackbuf, int stackbuf_size)
{
	struct compat_cmsghdr __user *ucmsg;
	struct cmsghdr *kcmsg, *kcmsg_base;
	compat_size_t ucmlen;
	__kernel_size_t kcmlen, tmp;
	int err = -EFAULT;

	kcmlen = 0;
	kcmsg_base = kcmsg = (struct cmsghdr *)stackbuf;
	ucmsg = CMSG_COMPAT_FIRSTHDR(kmsg);
	while (ucmsg != NULL) {
		if (get_user(ucmlen, &ucmsg->cmsg_len))
			return -EFAULT;

		/* Catch bogons. */
		if (!CMSG_COMPAT_OK(ucmlen, ucmsg, kmsg))
			return -EINVAL;

		tmp = ((ucmlen - CMSG_COMPAT_ALIGN(sizeof(*ucmsg))) +
		       CMSG_ALIGN(sizeof(struct cmsghdr)));
		tmp = CMSG_ALIGN(tmp);
		kcmlen += tmp;
		ucmsg = cmsg_compat_nxthdr(kmsg, ucmsg, ucmlen);
	}
	if (kcmlen == 0)
		return -EINVAL;

	/* The kcmlen holds the 64-bit version of the control length.
	 * It may not be modified as we do not stick it into the kmsg
	 * until we have successfully copied over all of the data
	 * from the user.
	 */
	if (kcmlen > stackbuf_size)
		kcmsg_base = kcmsg = sock_kmalloc(sk, kcmlen, GFP_KERNEL);
	if (kcmsg == NULL)
		return -ENOBUFS;

	/* Now copy them over neatly. */
	memset(kcmsg, 0, kcmlen);
	ucmsg = CMSG_COMPAT_FIRSTHDR(kmsg);
	while (ucmsg != NULL) {
		if (__get_user(ucmlen, &ucmsg->cmsg_len))
			goto Efault;
		if (!CMSG_COMPAT_OK(ucmlen, ucmsg, kmsg))
			goto Einval;
		tmp = ((ucmlen - CMSG_COMPAT_ALIGN(sizeof(*ucmsg))) +
		       CMSG_ALIGN(sizeof(struct cmsghdr)));
		if ((char *)kcmsg_base + kcmlen - (char *)kcmsg < CMSG_ALIGN(tmp))
			goto Einval;
		kcmsg->cmsg_len = tmp;
		tmp = CMSG_ALIGN(tmp);
		if (__get_user(kcmsg->cmsg_level, &ucmsg->cmsg_level) ||
		    __get_user(kcmsg->cmsg_type, &ucmsg->cmsg_type) ||
		    copy_from_user(CMSG_DATA(kcmsg),
				   CMSG_COMPAT_DATA(ucmsg),
				   (ucmlen - CMSG_COMPAT_ALIGN(sizeof(*ucmsg)))))
			goto Efault;

		/* Advance. */
		kcmsg = (struct cmsghdr *)((char *)kcmsg + tmp);
		ucmsg = cmsg_compat_nxthdr(kmsg, ucmsg, ucmlen);
	}

	/* Ok, looks like we made it.  Hook it up and return success. */
	kmsg->msg_control = kcmsg_base;
	kmsg->msg_controllen = kcmlen;
	return 0;

Einval:
	err = -EINVAL;
Efault:
	if (kcmsg_base != (struct cmsghdr *)stackbuf)
		sock_kfree_s(sk, kcmsg_base, kcmlen);
	return err;
}

static int32_t
qb_ipc_auth_creds(struct ipc_auth_data *data)
{
	int32_t res = 0;

	/*
	 * currently support getpeerucred, getpeereid, and SO_PASSCRED credential
	 * retrieval mechanisms for various Platforms
	 */
#ifdef HAVE_GETPEERUCRED
	/*
	 * Solaris and some BSD systems
	 */
	{
		ucred_t *uc = NULL;

		if (getpeerucred(data->sock, &uc) == 0) {
			res = 0;
			data->ugp.uid = ucred_geteuid(uc);
			data->ugp.gid = ucred_getegid(uc);
			data->ugp.pid = ucred_getpid(uc);
			ucred_free(uc);
		} else {
			res = -errno;
		}
	}
#elif defined(HAVE_GETPEEREID)
	/*
	* Usually MacOSX systems
	*/
	{
		/*
		* TODO get the peer's pid.
		* c->pid = ?;
		*/
		if (getpeereid(data->sock, &data->ugp.uid, &data->ugp.gid) == 0) {
			res = 0;
		} else {
			res = -errno;
		}
	}

#elif defined(SO_PASSCRED)
	/*
	* Usually Linux systems
	*/
	{
		struct ucred cred;
		struct cmsghdr *cmsg;

		res = -EINVAL;
		for (cmsg = CMSG_FIRSTHDR(&data->msg_recv); cmsg != NULL;
			cmsg = CMSG_NXTHDR(&data->msg_recv, cmsg)) {
			if (cmsg->cmsg_type != SCM_CREDENTIALS)
				continue;

			memcpy(&cred, CMSG_DATA(cmsg), sizeof(struct ucred));
			res = 0;
			data->ugp.pid = cred.pid;
			data->ugp.uid = cred.uid;
			data->ugp.gid = cred.gid;
			break;
		}
	}
#else /* no credentials */
	data->ugp.pid = 0;
	data->ugp.uid = 0;
	data->ugp.gid = 0;
	res = -ENOTSUP;
#endif /* no credentials */

	return res;
}

//.........这里部分代码省略.........
				if (fds[i].fd == sock)
					printf("device: disconnected\n");
				else
					printf("client: disconnect\n");
				return 0;
			}
		}

		if (mode == SERVER) {
			len = recv(fd, buf, snap_len, MSG_DONTWAIT);
			if (len == 0) {
				printf("client: disconnect\n");
				return 0;
			}
			if (len < 0 && errno != EAGAIN && errno != EINTR) {
				perror("Connection read failure");
				return -1;
			}
		}

		iv.iov_base = frm.data;
		iv.iov_len  = snap_len;

		msg.msg_iov = &iv;
		msg.msg_iovlen = 1;
		msg.msg_control = ctrl;
		msg.msg_controllen = 100;

		len = recvmsg(sock, &msg, MSG_DONTWAIT);
		if (len < 0) {
			if (errno == EAGAIN || errno == EINTR)
				continue;
			perror("Receive failed");
			return -1;
		}

		/* Process control message */
		frm.data_len = len;
		frm.dev_id = dev;
		frm.in = 0;
		frm.pppdump_fd = parser.pppdump_fd;
		frm.audio_fd   = parser.audio_fd;

		cmsg = CMSG_FIRSTHDR(&msg);
		while (cmsg) {
			int dir;
			switch (cmsg->cmsg_type) {
			case HCI_CMSG_DIR:
				memcpy(&dir, CMSG_DATA(cmsg), sizeof(int));
				frm.in = (uint8_t) dir;
				break;
			case HCI_CMSG_TSTAMP:
				memcpy(&frm.ts, CMSG_DATA(cmsg),
						sizeof(struct timeval));
				break;
			}
			cmsg = CMSG_NXTHDR(&msg, cmsg);
		}

		frm.ptr = frm.data;
		frm.len = frm.data_len;

		switch (mode) {
		case WRITE:
		case SERVER:
			/* Save or send dump */
			if (flags & DUMP_BTSNOOP) {
				uint64_t ts;
				uint8_t pkt_type = ((uint8_t *) frm.data)[0];
				dp->size = htonl(frm.data_len);
				dp->len  = dp->size;
				dp->flags = ntohl(frm.in & 0x01);
				dp->drops = 0;
				ts = (frm.ts.tv_sec - 946684800ll) * 1000000ll + frm.ts.tv_usec;
				dp->ts = hton64(ts + 0x00E03AB44A676000ll);
				if (pkt_type == HCI_COMMAND_PKT ||
						pkt_type == HCI_EVENT_PKT)
					dp->flags |= ntohl(0x02);
			} else {
				dh->len = htobs(frm.data_len);
				dh->in  = frm.in;
				dh->ts_sec  = htobl(frm.ts.tv_sec);
				dh->ts_usec = htobl(frm.ts.tv_usec);
			}

			if (write_n(fd, buf, frm.data_len + hdr_size) < 0) {
				perror("Write error");
				return -1;
			}
			break;

		default:
			/* Parse and print */
			parse(&frm);
			break;
		}
	}

	return 0;
}

int bind( int sockfd, const struct sockaddr *my_addr, socklen_t addrlen)
{
#ifdef SO_REUSEPORT
   // parse environment property (just once)
   if (reuse_ports.count < 0){
     char *env_reuse = getenv("PRIVBIND_REUSE_PORTS");
     if (env_reuse == NULL){
       reuse_ports.count = 0;
     }else{
       if (parselist(env_reuse, &reuse_ports, 1, 65535) != 0){
         reuse_ports.count = -1;
         return -1;
       }
     }
   }
   
   // get bind port
   int port = -1;
   if (my_addr->sa_family==AF_INET)
      port = (int) ntohs(((struct sockaddr_in *) my_addr)->sin_port);
   else if (my_addr->sa_family==AF_INET6)
      port = (int) ntohs(((struct sockaddr_in6 *) my_addr)->sin6_port);
   
   // check if we should setup SO_REUSEPORT for this port
   if (port != -1 && is_in_list(&reuse_ports, port)){
      int optval = 1;
      int retval = setsockopt(sockfd, SOL_SOCKET, SO_REUSEPORT, &optval, sizeof(optval));
      if (retval != 0)
        return retval;
   }
#endif
  
   /* First of all, attempt the bind. We only need the socket if it fails with access denied */

   int oret=_bind( sockfd, my_addr, addrlen );
   if( oret==0 || errno!=EACCES )
      return oret;

   /* In most cases, we can let the bind go through as is */
   if( master_quit || (my_addr->sa_family!=AF_INET && my_addr->sa_family!=AF_INET6) 
       || (my_addr->sa_family==AF_INET && addrlen<sizeof(struct sockaddr_in)) 
       || (my_addr->sa_family==AF_INET6 && addrlen<sizeof(struct sockaddr_in6))  
     ) { 
      errno=EACCES;
      return oret;
   }

   /* Prepare the ancillary data for passing the actual FD */
   struct msghdr msghdr={.msg_name=NULL};
   struct cmsghdr *cmsg;
   char buf[CMSG_SPACE(sizeof(int))];
   int *fdptr;

    msghdr.msg_control=buf;
    msghdr.msg_controllen=sizeof(buf);

    cmsg=CMSG_FIRSTHDR(&msghdr);
    cmsg->cmsg_level=SOL_SOCKET;
    cmsg->cmsg_type=SCM_RIGHTS;
    cmsg->cmsg_len=CMSG_LEN(sizeof(int));

    /* Initialize the payload */
    fdptr=(int *)CMSG_DATA(cmsg);
    fdptr[0]=sockfd;
    msghdr.msg_controllen=cmsg->cmsg_len;

    /* Don't forget the data component */
    struct ipc_msg_req request;
    struct iovec iov;

    msghdr.msg_iov=&iov;
    msghdr.msg_iovlen=1;

    iov.iov_base=&request;
    iov.iov_len=sizeof(request);

    request.type=MSG_REQ_BIND;

    /* Check family of the request, only INET and INET6 should make it here */
    if (my_addr->sa_family==AF_INET)
      request.data.bind4.addr=*(struct sockaddr_in *) my_addr;
    else if (my_addr->sa_family==AF_INET6)
      request.data.bind6.addr=*(struct sockaddr_in6 *) my_addr;

    int retval=oret;

    if( acquire_lock(1) ) {
       if( sendmsg( COMM_SOCKET, &msghdr, MSG_NOSIGNAL )>0 ) {
          /* Request was sent - wait for reply */
          struct ipc_msg_reply reply;

          if( recv( COMM_SOCKET, &reply, sizeof(reply), 0 )>0 ) {
             retval=reply.data.stat.retval;
             if( retval<0 )
                errno=reply.data.stat.error;
          } else {
             /* It would appear that the other process has closed, just return the original retval */
             master_cleanup();
          }
       } else {
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                {
                    printf("Sme Disconnected\n");
                    (void)fflush(stdout);
                    ipc_disconnect(smeConnection);
                    smeConnection = NULL;
                    continue;
                }
#endif
				else
				{
					printf("client: disconnect\n");
				}
			}
		}

        for (i = 0; i < nfds; i++)
        {
#ifdef IPC_IP
            if (smeConnection && fds[i].fd == ipc_getFd(smeConnection) && fds[i].revents & (POLLIN | POLLPRI))
            {
                CsrUint8* smeRxBuffer;
                CsrUint32 smeRxLength = 0;

                printf("Sme fd Triggered\n");
                (void)fflush(stdout);

                if (ipc_message_recv(smeConnection, 0, &smeRxBuffer, &smeRxLength))
                {
                    if (smeRxLength != 0)
                    {
                        if (!remote_bt_signal_receive(NULL, smeRxBuffer, (CsrUint16)smeRxLength))
                        {
                            printf("Sme unhandled ipc message\n");
                            (void)fflush(stdout);
                        }
                        ipc_message_free(smeConnection, smeRxBuffer);
                    }
                    continue;
                }

                /* Opps Disconnected */
                printf("Sme Disconnected\n");
                (void)fflush(stdout);
                ipc_disconnect(smeConnection);
                smeConnection = NULL;
                nfds--;
                onIpcDisconnect();
                continue;
            }
#endif

            if (sock != -1 && fds[i].fd == sock && fds[i].revents & (POLLIN | POLLPRI))
            {
                iv.iov_base = frm.data;
                iv.iov_len  = snap_len;

                msg.msg_iov = &iv;
                msg.msg_iovlen = 1;
                msg.msg_control = ctrl;
                msg.msg_controllen = 100;

                len = recvmsg(sock, &msg, MSG_DONTWAIT);
                if (len < 0) {
                    if (errno == EAGAIN || errno == EINTR)
                        continue;
                    perror("Receive failed");
                    return -1;
                }

                /* Process control message */
                frm.data_len = len;
                frm.dev_id = device;
                frm.in = 0;
                frm.pppdump_fd = parser.pppdump_fd;
                frm.audio_fd   = parser.audio_fd;

                cmsg = CMSG_FIRSTHDR(&msg);
                while (cmsg) {
                    switch (cmsg->cmsg_type) {
                    case HCI_CMSG_DIR:
                        frm.in = *((int *) CMSG_DATA(cmsg));
                        break;
                    case HCI_CMSG_TSTAMP:
                        frm.ts = *((struct timeval *) CMSG_DATA(cmsg));
                        break;
                    }
                    cmsg = CMSG_NXTHDR(&msg, cmsg);
                }

                frm.ptr = frm.data;
                frm.len = frm.data_len;

                /* Parse and print */
                parse(&frm);
            }
        }
	}

	return 0;
}

/* writes generated buffer to desired destination. For TCP syslog,
 * we use RFC6587 octet-stuffing (unless octet-counting is selected).
 * This is not great, but doing full blown RFC5425 (TLS) looks like
 * it is too much for the logger utility. If octet-counting is
 * selected, we use that.
 */
static void write_output(const struct logger_ctl *ctl, const char *const msg)
{
	struct iovec iov[4];
	int iovlen = 0;
	char *octet = NULL;

	/* 1) octen count */
	if (ctl->octet_count) {
		size_t len = xasprintf(&octet, "%zu ", strlen(ctl->hdr) + strlen(msg));
		iovec_add_string(iov, iovlen, octet, len);
	}

	/* 2) header */
	iovec_add_string(iov, iovlen, ctl->hdr, 0);

	/* 3) message */
	iovec_add_string(iov, iovlen, msg, 0);

	if (!ctl->noact) {
		struct msghdr message = { 0 };
		struct cmsghdr *cmhp;
		struct ucred *cred;
		union {
			struct cmsghdr cmh;
			char   control[CMSG_SPACE(sizeof(struct ucred))];
		} cbuf;

		/* 4) add extra \n to make sure message is terminated */
		if ((ctl->socket_type == TYPE_TCP) && !ctl->octet_count)
			iovec_add_string(iov, iovlen, "\n", 1);

		message.msg_iov = iov;
		message.msg_iovlen = iovlen;

		/* syslog/journald may follow local socket credentials rather
		 * than in the message PID. If we use --id as root than we can
		 * force kernel to accept another valid PID than the real logger(1)
		 * PID.
		 */
		if (ctl->pid && !ctl->server && ctl->pid != getpid()
		    && geteuid() == 0 && kill(ctl->pid, 0) == 0) {

			message.msg_control = cbuf.control;
			message.msg_controllen = CMSG_SPACE(sizeof(struct ucred));

			cmhp = CMSG_FIRSTHDR(&message);
			cmhp->cmsg_len = CMSG_LEN(sizeof(struct ucred));
			cmhp->cmsg_level = SOL_SOCKET;
			cmhp->cmsg_type = SCM_CREDENTIALS;
			cred = (struct ucred *) CMSG_DATA(cmhp);

			cred->pid = ctl->pid;
		}

		if (sendmsg(ctl->fd, &message, 0) < 0)
			warn(_("send message failed"));
	}

	if (ctl->stderr_printout) {
		/* make sure it's terminated for stderr */
		if (iovec_memcmp(iov, iovlen, "\n", 1) != 0)
			iovec_add_string(iov, iovlen, "\n", 1);

		ignore_result( writev(STDERR_FILENO, iov, iovlen) );
	}

	free(octet);
}

//.........这里部分代码省略.........
    if (bind(can, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("bind");
        return 1;
    }

    init_car_state();

    door_id = DEFAULT_DOOR_ID;
    signal_id = DEFAULT_SIGNAL_ID;
    speed_id = DEFAULT_SPEED_ID;

    if (randomize || seed) {
        if(randomize) seed = time(NULL);
        srand(seed);
        door_id = (rand() % 2046) + 1;
        signal_id = (rand() % 2046) + 1;
        speed_id = (rand() % 2046) + 1;
        door_pos = rand() % 9;
        signal_pos = rand() % 9;
        speed_pos = rand() % 8;
        printf("Seed: %d\n", seed);
    }

    SDL_Window *window = NULL;
    SDL_Surface *screenSurface = NULL;
    if(SDL_Init ( SDL_INIT_VIDEO ) < 0 ) {
        printf("SDL Could not initializes\n");
        exit(40);
    }
    window = SDL_CreateWindow("IC Simulator", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, SCREEN_WIDTH, SCREEN_HEIGHT, SDL_WINDOW_SHOWN );
    if(window == NULL) {
        printf("Window could not be shown\n");
    }
    renderer = SDL_CreateRenderer(window, -1, 0);
    SDL_Surface *image = IMG_Load(get_data("ic.png"));
    SDL_Surface *needle = IMG_Load(get_data("needle.png"));
    SDL_Surface *sprites = IMG_Load(get_data("spritesheet.png"));
    base_texture = SDL_CreateTextureFromSurface(renderer, image);
    needle_tex = SDL_CreateTextureFromSurface(renderer, needle);
    sprite_tex = SDL_CreateTextureFromSurface(renderer, sprites);

    speed_rect.x = 212;
    speed_rect.y = 175;
    speed_rect.h = needle->h;
    speed_rect.w = needle->w;

    // Draw the IC
    redraw_ic();

    /* For now we will just operate on one CAN interface */
    while(running) {
        while( SDL_PollEvent(&event) != 0 ) {
            switch(event.type) {
            case SDL_QUIT:
                running = 0;
                break;
            }
        }

        nbytes = recvmsg(can, &msg, 0);
        if (nbytes < 0) {
            perror("read");
            return 1;
        }
        if ((size_t)nbytes == CAN_MTU)
            maxdlen = CAN_MAX_DLEN;
        else if ((size_t)nbytes == CANFD_MTU)
            maxdlen = CANFD_MAX_DLEN;
        else {
            fprintf(stderr, "read: incomplete CAN frame\n");
            return 1;
        }
        for (cmsg = CMSG_FIRSTHDR(&msg);
                cmsg && (cmsg->cmsg_level == SOL_SOCKET);
                cmsg = CMSG_NXTHDR(&msg,cmsg)) {
            if (cmsg->cmsg_type == SO_TIMESTAMP)
                tv = *(struct timeval *)CMSG_DATA(cmsg);
            else if (cmsg->cmsg_type == SO_RXQ_OVFL)
                //dropcnt[i] = *(__u32 *)CMSG_DATA(cmsg);
                fprintf(stderr, "Dropped packet\n");
        }
//      if(debug) fprint_canframe(stdout, &frame, "\n", 0, maxdlen);
        if(frame.can_id == door_id) update_door_status(&frame, maxdlen);
        if(frame.can_id == signal_id) update_signal_status(&frame, maxdlen);
        if(frame.can_id == speed_id) update_speed_status(&frame, maxdlen);
    }

    SDL_DestroyTexture(base_texture);
    SDL_DestroyTexture(needle_tex);
    SDL_DestroyTexture(sprite_tex);
    SDL_FreeSurface(image);
    SDL_FreeSurface(needle);
    SDL_FreeSurface(sprites);
    SDL_DestroyRenderer(renderer);
    SDL_DestroyWindow(window);
    IMG_Quit();
    SDL_Quit();

    return 0;
}

void
send_packet(struct interface *ifp,
	    struct stream *s, u_int32_t dst, struct prefix *p, u_int32_t ttl)
{
	static struct sockaddr_in sockdst = { AF_INET };
	struct ip *ip;
	struct icmphdr *icmp;
	struct msghdr *msg;
	struct cmsghdr *cmsg;
	struct iovec iovector;
	char msgbuf[256];
	char buf[256];
	struct in_pktinfo *pktinfo;
	u_long src;
	int on;

	if (!(ifp->flags & IFF_UP))
		return;

	if (p)
		src = ntohl(p->u.prefix4.s_addr);
	else
		src = 0;	/* Is filled in */

	ip = (struct ip *)buf;
	ip->ip_hl = sizeof(struct ip) >> 2;
	ip->ip_v = IPVERSION;
	ip->ip_tos = 0xC0;
	ip->ip_off = 0L;
	ip->ip_p = 1;		/* IP_ICMP */
	ip->ip_ttl = ttl;
	ip->ip_src.s_addr = src;
	ip->ip_dst.s_addr = dst;
	icmp = (struct icmphdr *)(buf + sizeof(struct ip));

	/* Merge IP header with icmp packet */
	assert(stream_get_endp(s) < (sizeof(buf) - sizeof(struct ip)));
	stream_get(icmp, s, stream_get_endp(s));

	/* icmp->checksum is already calculated */
	ip->ip_len = sizeof(struct ip) + stream_get_endp(s);

	on = 1;
	if (setsockopt(irdp_sock, IPPROTO_IP, IP_HDRINCL,
		       (char *)&on, sizeof(on)) < 0)
		zlog_warn("sendto %s", safe_strerror(errno));

	if (dst == INADDR_BROADCAST) {
		on = 1;
		if (setsockopt(irdp_sock, SOL_SOCKET, SO_BROADCAST,
			       (char *)&on, sizeof(on)) < 0)
			zlog_warn("sendto %s", safe_strerror(errno));
	}

	if (dst != INADDR_BROADCAST) {
		on = 0;
		if (setsockopt(irdp_sock, IPPROTO_IP, IP_MULTICAST_LOOP,
			       (char *)&on, sizeof(on)) < 0)
			zlog_warn("sendto %s", safe_strerror(errno));
	}

	memset(&sockdst, 0, sizeof(sockdst));
	sockdst.sin_family = AF_INET;
	sockdst.sin_addr.s_addr = dst;

	cmsg = (struct cmsghdr *)(msgbuf + sizeof(struct msghdr));
	cmsg->cmsg_len = sizeof(struct cmsghdr) + sizeof(struct in_pktinfo);
	cmsg->cmsg_level = SOL_IP;
	cmsg->cmsg_type = IP_PKTINFO;
	pktinfo = (struct in_pktinfo *)CMSG_DATA(cmsg);
	pktinfo->ipi_ifindex = ifp->ifindex;
	pktinfo->ipi_spec_dst.s_addr = src;
	pktinfo->ipi_addr.s_addr = src;

	iovector.iov_base = (void *)buf;
	iovector.iov_len = ip->ip_len;
	msg = (struct msghdr *)msgbuf;
	msg->msg_name = &sockdst;
	msg->msg_namelen = sizeof(sockdst);
	msg->msg_iov = &iovector;
	msg->msg_iovlen = 1;
	msg->msg_control = cmsg;
	msg->msg_controllen = cmsg->cmsg_len;

	sockopt_iphdrincl_swab_htosys(ip);

	if (sendmsg(irdp_sock, msg, 0) < 0) {
		zlog_warn("sendto %s", safe_strerror(errno));
	}
	/*   printf("TX on %s idx %d\n", ifp->name, ifp->ifindex); */
}

/** receives an ipv4 packet using a raw socket.
 * An ipv4 packet is received in buf, using IP_PKTINFO or IP_RECVDSTADDR.
 * from and to are filled (only the ip part the ports are 0 since this
 * function doesn't try to look beyond the IP level).
 * @param sock - raw socket
 * @param buf - detination buffer.
 * @param len - buffer len (should be enough for receiving a packet +
 *               IP header).
 * @param from - result parameter, the IP address part of it will be filled
 *                with the source address and the port with 0.
 * @param to - result parameter, the IP address part of it will be filled
 *                with the destination (local) address and the port with 0.
 * @return packet len or <0 on error: -1 (check errno),
 *        -2 no IP_PKTINFO/IP_RECVDSTADDR found or AF mismatch
 */
int recvpkt4(int sock, char* buf, int len, union sockaddr_union* from,
					union sockaddr_union* to)
{
	struct iovec iov[1];
	struct msghdr rcv_msg;
	struct cmsghdr* cmsg;
#ifdef IP_PKTINFO
	struct in_pktinfo* rcv_pktinfo;
#endif /* IP_PKTINFO */
	int n, ret;
	char msg_ctrl_buf[1024];

	iov[0].iov_base=buf;
	iov[0].iov_len=len;
	memset(&rcv_msg, 0, sizeof(struct msghdr));
	rcv_msg.msg_name=from;
	rcv_msg.msg_namelen=sockaddru_len(*from);
	rcv_msg.msg_control=msg_ctrl_buf;
	rcv_msg.msg_controllen=sizeof(msg_ctrl_buf);
	rcv_msg.msg_iov=&iov[0];
	rcv_msg.msg_iovlen=1;
	ret=-2; /* no PKT_INFO or AF mismatch */
retry:
	n=recvmsg(sock, &rcv_msg, MSG_WAITALL);
	if (unlikely(n==-1)){
		if (errno==EINTR)
			goto retry;
		ret=n;
		goto end;
	}
	/* find the pkt info */
	for (cmsg=CMSG_FIRSTHDR(&rcv_msg); cmsg; cmsg=CMSG_NXTHDR(&rcv_msg, cmsg)){
#ifdef IP_PKTINFO
		if (likely((cmsg->cmsg_level==IPPROTO_IP) &&
					(cmsg->cmsg_type==IP_PKTINFO))) {
			rcv_pktinfo=(struct in_pktinfo*)CMSG_DATA(cmsg);
			to->sin.sin_family=AF_INET;
			memcpy(&to->sin.sin_addr, &rcv_pktinfo->ipi_spec_dst.s_addr, 
									sizeof(to->sin.sin_addr));
			to->sin.sin_port=0; /* not known */
			/* interface no. in ipi_ifindex */
			ret=n; /* success */
			break;
		}
#elif defined (IP_RECVDSTADDR)
		if (likely((cmsg->cmsg_level==IPPROTO_IP) &&
					(cmsg->cmsg_type==IP_RECVDSTADDR))) {
			to->sin.sin_family=AF_INET;
			memcpy(&to->sin.sin_addr, CMSG_DATA(cmsg),
									sizeof(to->sin.sin_addr));
			to->sin.sin_port=0; /* not known */
			ret=n; /* success */
			break;
		}
#else
#error "no method of getting the destination ip address supported"
#endif /* IP_PKTINFO / IP_RECVDSTADDR */
	}
end:
	return ret;
}