int unix_sendmsgf (int sock, void *buf, int buf_len, int *fds, int num_fds, int flags) { struct iovec iov1 = {.iov_base = buf,.iov_len = buf_len }; int cmsg_buf_size = CMSG_SPACE (sizeof (int) * num_fds); char cmsg_buf[cmsg_buf_size]; struct msghdr msgh = {.msg_name = NULL, .msg_namelen = 0, .msg_iov = &iov1, .msg_iovlen = 1, .msg_control = cmsg_buf, .msg_controllen = cmsg_buf_size, .msg_flags = 0 }; struct cmsghdr *cmsg = CMSG_FIRSTHDR (&msgh); cmsg->cmsg_level = SOL_SOCKET; cmsg->cmsg_type = SCM_RIGHTS; cmsg->cmsg_len = CMSG_LEN (sizeof (int) * num_fds); memcpy (CMSG_DATA (cmsg), fds, sizeof (int) * num_fds); msgh.msg_controllen = cmsg->cmsg_len; return sendmsg (sock, &msgh, flags); // TODO: Free msgh, cmsg_buf. } int unix_recvmsgf (int sock, void *buf, int buf_len, int *fds, int *p_num_fds, int flags) { struct iovec iov1 = {.iov_base = buf,.iov_len = buf_len }; int cmsg_buf_size = CMSG_SPACE (sizeof (int) * (*p_num_fds)); char cmsg_buf[cmsg_buf_size]; struct msghdr msgh = {.msg_name = NULL, .msg_namelen = 0, .msg_iov = &iov1, .msg_iovlen = 1, .msg_control = cmsg_buf, .msg_controllen = cmsg_buf_size, .msg_flags = 0 }; int ret; if ((ret = recvmsg (sock, &msgh, flags)) > 0) { struct cmsghdr *cmsg; for (cmsg = CMSG_FIRSTHDR (&msgh); cmsg; cmsg = CMSG_NXTHDR (&msgh, cmsg)) { if ((SOL_SOCKET == cmsg->cmsg_level) && (SCM_RIGHTS == cmsg->cmsg_type)) { size_t fds_size = cmsg->cmsg_len - CMSG_LEN (0); // this might be wrong if (fds_size > sizeof (int) * (*p_num_fds)) { fprintf (stderr, "tried to send a large number of items in cmsg. abort.\n"); exit (1); } memcpy (fds, CMSG_DATA (cmsg), fds_size); *p_num_fds = fds_size / sizeof (int); break; } } } else { fprintf (stderr, "errno: %d\n", errno); perror ("recvmsg"); } return ret; }
void rxpacket(int sock) { Packet pkt; memset(&pkt, 0, sizeof(pkt)); // using recvmsg int size; // size of the received data struct msghdr msg; memset(&msg, 0, sizeof(msg)); struct sockaddr_in from; int fromlen=sizeof(from); msg.msg_name = &from; msg.msg_namelen = fromlen; char anciliary[2048]; msg.msg_control = anciliary; msg.msg_controllen = sizeof(anciliary); struct iovec iov[1]; memset(iov, 0, sizeof(iov)); iov[0].iov_base = &pkt.data; iov[0].iov_len = sizeof(pkt.data); msg.msg_iov = iov; msg.msg_iovlen = 1; struct cmsghdr *cmsg; unsigned int ifindex; // interface index struct in_addr hdraddr; // destination IP address in IP header size = recvmsg(sock, &msg, 0); if (size == -1) { ASSERT(0); rcpLog(muxsock, RCP_PROC_RIP, RLOG_ERR, RLOG_FC_RIP, "cannot read data on socket, attempting recovery..."); exit(1); } // verify packet size int sz = size - 4; if (sz<= 0 || (sz % sizeof(RipRoute)) != 0) { rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, "Invalid RIP packet size"); return; } int routes = sz / sizeof(RipRoute); int found = 0; for(cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == IPPROTO_IP && cmsg->cmsg_type == IP_PKTINFO) { // struct in_pktinfo { // unsigned int ipi_ifindex; /* Interface index */ // struct in_addr ipi_spec_dst; /* Local address */ // struct in_addr ipi_addr; /* Header Destination // address */ // }; hdraddr = ((struct in_pktinfo*)CMSG_DATA(cmsg))->ipi_addr; ifindex = ((struct in_pktinfo*)CMSG_DATA(cmsg))->ipi_ifindex; found = 1; } } if (!found) return; pkt.ip_source = ntohl(from.sin_addr.s_addr); pkt.ip_dest = ntohl(hdraddr.s_addr); pkt.if_index = ifindex; // is the source ip address one of our addresses? RcpInterface *rxif = rcpFindInterface(shm, pkt.ip_source); if (rxif) { // on Linux, packets we send to the multicast group will be received back on the socket return; } char *cmd[] = {"", "request", "response"}; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, "Receiving RIP packet of size %d, from %d.%d.%d.%d, destination %d.%d.%d.%d, RIP %s, protocol version %d", size, RCP_PRINT_IP(pkt.ip_source), RCP_PRINT_IP(pkt.ip_dest), cmd[(pkt.data.command <= 2) ? pkt.data.command: 0], pkt.data.version); // update neighbor list RipNeighbor *neigh = neighbors; while (neigh != NULL) { if (neigh->ip == pkt.ip_source) { neigh->rx_time = 0; break; } neigh = neigh->next; } if (neigh == NULL) { RipNeighbor *newneigh = malloc(sizeof(RipNeighbor)); if (newneigh != NULL) { memset(newneigh, 0, sizeof(RipNeighbor)); newneigh->ip = pkt.ip_source; newneigh->next = neighbors; neighbors = newneigh; neigh = newneigh; } else { ASSERT(0); rcpLog(muxsock, RCP_PROC_RIP, RLOG_ERR, RLOG_FC_RIP, "cannot allocate memory, attempting recovery..."); exit(1); } } // do we have a valid interface? rxif =rcpFindInterfaceByKIndex(shm, pkt.if_index); if (rxif == NULL) { neigh->errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid interface, dropping..."); return; } // do we have a configured neighbor? RcpRipPartner *rxnetwork = NULL; RcpRipPartner *net; int i; for (i = 0, net = shm->config.rip_neighbor; i < RCP_RIP_NEIGHBOR_LIMIT; i++, net++) { if (!net->valid) continue; // matching both source and destination addresses if (net->ip == pkt.ip_source && rxif->ip == pkt.ip_dest) { rxnetwork = net; break; } } // if no configured neighbor was found, try to find a configured network if (rxnetwork == NULL) rxnetwork = find_network_for_interface(rxif); // no network or neighbor configured, just drop the packet if (rxnetwork == NULL) { neigh->errors++; // the network can get disabled while receiving packets rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid network or neighbor, dropping..."); return; } // the source of the datagram must be on a directly-connected network if ((pkt.ip_source & rxif->mask) != (rxif->ip & rxif->mask)) { neigh->errors++; // interface ip addresses are changing dynamically via CLI rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid source IP address, dropping..."); return; } // drop invalid command packets if (pkt.data.command > 2) { neigh->errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid RIP command, dropping..."); return; } if (pkt.data.command == 1) { rxnetwork->req_rx++; // force a response in one second rxif->rip_timeout = 1; return; } else rxnetwork->resp_rx++; ASSERT(sizeof(RipAuthMd5) == sizeof(RipAuthSimple)); ASSERT(sizeof(RipAuthSimple) == sizeof(RipRoute)); RipRoute *ptr = &pkt.data.routes[0]; int rt = 0; // if md5 auth configured, and the packet is missing the auth header, drop the packet if (rxif->rip_passwd[0] != '\0') { if (ptr->family != 0xffff || ntohs(ptr->tag) != 3) { neigh->md5_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " missing MD5 authentication header"); return; } } // checking auth header and calculate md5 if (ptr->family == 0xffff) { // we don't care about simple auth if (ntohs(ptr->tag) == 3 && rxif->rip_passwd[0] != '\0') { RipAuthMd5 *md5 = (RipAuthMd5 *) ptr; uint16_t offset = ntohs(md5->offset); uint32_t seq = ntohl(md5->seq); rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " MD5 auth offset %u, key id %d, auth_len %d, seq %u", offset, md5->key_id, md5->auth_len, ntohl(md5->seq)); // check offset if ((offset + sizeof(RipRoute)) != size) { neigh->md5_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid offset"); return; } // check seq if (seq != 0 && seq < neigh->auth_seq) { neigh->md5_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid sequence number"); return; } neigh->auth_seq = seq; // calculate md5 uint8_t secret[16]; memset(secret, 0, 16); memcpy(secret, rxif->rip_passwd, strlen(rxif->rip_passwd)); MD5_CTX context; uint8_t digest[16]; MD5Init (&context); MD5Update (&context, (uint8_t *) &pkt, size - 16); MD5Update (&context, secret, 16); MD5Final (digest, &context); #if 0 { int i; uint8_t *p = digest; printf("rx digest:\n"); for (i = 0; i < 16; i++,p++) printf("%02x ", *p); printf("\n"); } #endif // compare md5 if (memcmp((uint8_t *) ptr + offset, digest, 16) != 0) { neigh->md5_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid MD5 digest"); return; } } ptr++; rt++; routes--; // the last route is the digest } // parsing routes while (rt < routes) { uint32_t metric = ntohl(ptr->metric); uint32_t mask = ntohl(ptr->mask); uint32_t ip = ntohl(ptr->ip); uint32_t gw = ntohl(ptr->gw); // if (trace_prefix == 0 || // (trace_prefix != 0 && trace_prefix == ip)) { // if (gw == 0) // rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, // " %d.%d.%d.%d/%d metric %u", // RCP_PRINT_IP(ip), // mask2bits(mask), // metric); // else // rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, // " %d.%d.%d.%d/%d metric %u next hop %d.%d.%d.%d", // RCP_PRINT_IP(ip), // mask2bits(mask), // metric, // RCP_PRINT_IP(gw)); // } // only AF_INET family is supported if (ntohs(ptr->family) != AF_INET) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid route family"); goto next_element; } // check destination for loopback addresses if (isLoopback(ip)) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid loopback route prefix"); goto next_element; } // check destination for broadcast addresses if (isBroadcast(ip)) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid broadcast route prefix"); goto next_element; } // check destination for multicast addresses if (isMulticast(ip)) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid multicast route prefix"); goto next_element; } // validate route metric else if (metric > 16 || metric == 0) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid metric"); goto next_element; } // validate route entry if (ip == 0 && mask == 0) { rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " received default route metric %d", metric); } else if (pkt.data.version == 2 && mask == 0) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid RIP route"); goto next_element; } else if (pkt.data.version == 1 && mask != 0) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid RIP route"); goto next_element; } // check if the mask is contiguous if (mask != 0 && !maskContiguous(mask)) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid mask"); goto next_element; } // validate next hop if (gw) { if (isLoopback(gw) || isMulticast(gw) || isBroadcast(gw)) { neigh->route_errors++; rcpLog(muxsock, RCP_PROC_RIP, RLOG_DEBUG, RLOG_FC_RIP, " invalid next hop"); goto next_element; } } // manufacture mask for rip v1 if (pkt.data.version == 1) mask = classMask(ip); // RFC metric = metric + interface cost // we assume a cost of 1 for each interface metric++; // add the route in the database if (metric < 16) { //RFC //- Setting the destination address to the destination address in the // RTE // // - Setting the metric to the newly calculated metric (as described // above) // // - Set the next hop address to be the address of the router from which // the datagram came // // - Initialize the timeout for the route. If the garbage-collection // timer is running for this route, stop it (see section 3.6 for a // discussion of the timers) // // - Set the route change flag // // - Signal the output process to trigger an update (see section 3.8.1) // a next hop of 0 means send the packets to me if (gw == 0) gw = pkt.ip_source; ripdb_add(RCP_ROUTE_RIP, ip, mask, gw, metric, pkt.ip_source, rxif); } else { // a next hop of 0 means send the packets to me if (gw == 0) gw = pkt.ip_source; ripdb_delete(RCP_ROUTE_RIP, ip, mask, gw, pkt.ip_source); } next_element: ptr++; rt++; } }
/* * Receive packet, with src/dst information. It is assumed that necessary * setsockopt() have already performed on socket. */ int recvfromto(int s, void *buf, size_t buflen, int flags, struct sockaddr *from, int *fromlen, struct sockaddr *to, int *tolen) { int otolen; int len; socklen_t sslen; struct sockaddr_storage ss; struct msghdr m; struct cmsghdr *cm; struct iovec iov[2]; unsigned char cmsgbuf[256]; #if defined(INET6) && defined(ADVAPI) struct in6_pktinfo *pi; #endif /*ADVAPI*/ #ifdef INET6 struct sockaddr_in6 *sin6; #endif sslen = sizeof(ss); if (getsockname(s, (struct sockaddr *)&ss, &sslen) < 0) { plog(PLOG_INTERR, PLOGLOC, NULL, "getsockname (%s)\n", strerror(errno)); return -1; } #if 1 /* quick hack */ memcpy(to, &ss, sslen < *tolen ? sslen : *tolen); #endif m.msg_name = (caddr_t)from; m.msg_namelen = *fromlen; iov[0].iov_base = (caddr_t)buf; iov[0].iov_len = buflen; m.msg_iov = iov; m.msg_iovlen = 1; memset(cmsgbuf, 0, sizeof(cmsgbuf)); cm = (struct cmsghdr *)cmsgbuf; m.msg_control = (caddr_t)cm; m.msg_controllen = sizeof(cmsgbuf); if ((len = recvmsg(s, &m, flags)) < 0) { plog(PLOG_INTERR, PLOGLOC, NULL, "recvmsg (%s)\n", strerror(errno)); return -1; } *fromlen = m.msg_namelen; otolen = *tolen; *tolen = 0; for (cm = (struct cmsghdr *)CMSG_FIRSTHDR(&m); m.msg_controllen != 0 && cm; cm = (struct cmsghdr *)CMSG_NXTHDR(&m, cm)) { #if 1 plog(PLOG_DEBUG, PLOGLOC, NULL, "cmsg %d %d\n", cm->cmsg_level, cm->cmsg_type); #endif #if defined(INET6) && defined(ADVAPI) if (SOCKADDR_FAMILY(&ss) == AF_INET6 && cm->cmsg_level == IPPROTO_IPV6 && cm->cmsg_type == IPV6_PKTINFO && otolen >= (int)sizeof(*sin6)) { pi = (struct in6_pktinfo *)(CMSG_DATA(cm)); *tolen = sizeof(*sin6); sin6 = (struct sockaddr_in6 *)to; memset(sin6, 0, sizeof(*sin6)); sin6->sin6_family = AF_INET6; SET_SOCKADDR_LEN(sin6, sizeof(*sin6)); memcpy(&sin6->sin6_addr, &pi->ipi6_addr, sizeof(sin6->sin6_addr)); /* XXX other cases, such as site-local? */ if (IN6_IS_ADDR_LINKLOCAL(&sin6->sin6_addr)) sin6->sin6_scope_id = pi->ipi6_ifindex; else sin6->sin6_scope_id = 0; sin6->sin6_port = ((struct sockaddr_in6 *)&ss)->sin6_port; otolen = -1; /* "to" already set */ continue; } #endif #if defined(INET6) && defined(IPV6_RECVDSTADDR) if (ss.ss_family == AF_INET6 && cm->cmsg_level == IPPROTO_IPV6 && cm->cmsg_type == IPV6_RECVDSTADDR && otolen >= sizeof(*sin6)) { *tolen = sizeof(*sin6); sin6 = (struct sockaddr_in6 *)to; memset(sin6, 0, sizeof(*sin6)); sin6->sin6_family = AF_INET6; sin6->sin6_len = sizeof(*sin6); memcpy(&sin6->sin6_addr, CMSG_DATA(cm), sizeof(sin6->sin6_addr)); sin6->sin6_port = ((struct sockaddr_in6 *)&ss)->sin6_port; otolen = -1; /* "to" already set */ continue; } #endif #ifdef IP_RECVDSTADDR if (ss.ss_family == AF_INET && cm->cmsg_level == IPPROTO_IP && cm->cmsg_type == IP_RECVDSTADDR && otolen >= (int)sizeof(struct sockaddr_in)) { struct sockaddr_in *sin; *tolen = sizeof(*sin); sin = (struct sockaddr_in *)to; memset(sin, 0, sizeof(*sin)); sin->sin_family = AF_INET; sin->sin_len = sizeof(*sin); memcpy(&sin->sin_addr, CMSG_DATA(cm), sizeof(sin->sin_addr)); sin->sin_port = ((struct sockaddr_in *)&ss)->sin_port; otolen = -1; /* "to" already set */ continue; } #else #if defined(IP_PKTINFO) #else #error #endif #endif } return len; }
ssize_t imsg_read(struct imsgbuf *ibuf) { struct msghdr msg; struct cmsghdr *cmsg; union { struct cmsghdr hdr; char buf[CMSG_SPACE(sizeof(int) * 1)]; } cmsgbuf; struct iovec iov; ssize_t n = -1; int fd; struct imsg_fd *ifd; memset(&msg, 0, sizeof(msg)); memset(&cmsgbuf, 0, sizeof(cmsgbuf)); iov.iov_base = ibuf->r.buf + ibuf->r.wpos; iov.iov_len = sizeof(ibuf->r.buf) - ibuf->r.wpos; msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = &cmsgbuf.buf; msg.msg_controllen = sizeof(cmsgbuf.buf); if ((ifd = calloc(1, sizeof(struct imsg_fd))) == NULL) return (-1); again: if (available_fds(imsg_fd_overhead + (CMSG_SPACE(sizeof(int))-CMSG_SPACE(0))/sizeof(int))) { errno = EAGAIN; free(ifd); return (-1); } if ((n = recvmsg(ibuf->fd, &msg, 0)) == -1) { if (errno == EMSGSIZE) goto fail; if (errno != EINTR && errno != EAGAIN) goto fail; goto again; } ibuf->r.wpos += n; for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS) { int i; int j; /* * We only accept one file descriptor. Due to C * padding rules, our control buffer might contain * more than one fd, and we must close them. */ j = ((char *)cmsg + cmsg->cmsg_len - (char *)CMSG_DATA(cmsg)) / sizeof(int); for (i = 0; i < j; i++) { fd = ((int *)CMSG_DATA(cmsg))[i]; if (ifd != NULL) { ifd->fd = fd; TAILQ_INSERT_TAIL(&ibuf->fds, ifd, entry); ifd = NULL; } else close(fd); } } /* we do not handle other ctl data level */ } fail: if (ifd) free(ifd); return (n); }
int red_recv_udp_pkt(int fd, char *buf, size_t buflen, struct sockaddr_in *inaddr, struct sockaddr_in *toaddr) { socklen_t addrlen = sizeof(*inaddr); ssize_t pktlen; struct msghdr msg; struct iovec io; char control[1024]; memset(&msg, 0, sizeof(msg)); msg.msg_name = inaddr; msg.msg_namelen = sizeof(*inaddr); msg.msg_iov = &io; msg.msg_iovlen = 1; msg.msg_control = control; msg.msg_controllen = sizeof(control); io.iov_base = buf; io.iov_len = buflen; pktlen = recvmsg(fd, &msg, 0); if (pktlen == -1) { log_errno(LOG_WARNING, "recvfrom"); return -1; } if (toaddr) { memset(toaddr, 0, sizeof(*toaddr)); for (struct cmsghdr* cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if ( cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_ORIGDSTADDR && cmsg->cmsg_len >= CMSG_LEN(sizeof(*toaddr)) ) { struct sockaddr_in* cmsgaddr = (struct sockaddr_in*)CMSG_DATA(cmsg); //char buf[RED_INET_ADDRSTRLEN]; //log_error(LOG_DEBUG, "IP_ORIGDSTADDR: %s", red_inet_ntop(cmsgaddr, buf, sizeof(buf))); memcpy(toaddr, cmsgaddr, sizeof(*toaddr)); } else { log_error(LOG_WARNING, "unexepcted cmsg (level,type) = (%d,%d)", cmsg->cmsg_level, cmsg->cmsg_type); } } if (toaddr->sin_family != AF_INET) { log_error(LOG_WARNING, "(SOL_IP, IP_ORIGDSTADDR) not found"); return -1; } } if (addrlen != sizeof(*inaddr)) { log_error(LOG_WARNING, "unexpected address length %u instead of %zu", addrlen, sizeof(*inaddr)); return -1; } if (pktlen >= buflen) { char buf[RED_INET_ADDRSTRLEN]; log_error(LOG_WARNING, "wow! Truncated udp packet of size %zd from %s! impossible! dropping it...", pktlen, red_inet_ntop(inaddr, buf, sizeof(buf))); return -1; } return pktlen; }
static int afalg_start_cipher_sk(afalg_ctx *actx, const unsigned char *in, size_t inl, const unsigned char *iv, unsigned int enc) { struct msghdr msg = { 0 }; struct cmsghdr *cmsg; struct iovec iov; ssize_t sbytes; # ifdef ALG_ZERO_COPY int ret; # endif char cbuf[CMSG_SPACE(ALG_IV_LEN(ALG_AES_IV_LEN)) + CMSG_SPACE(ALG_OP_LEN)]; memset(cbuf, 0, sizeof(cbuf)); msg.msg_control = cbuf; msg.msg_controllen = sizeof(cbuf); /* * cipher direction (i.e. encrypt or decrypt) and iv are sent to the * kernel as part of sendmsg()'s ancillary data */ cmsg = CMSG_FIRSTHDR(&msg); afalg_set_op_sk(cmsg, enc); cmsg = CMSG_NXTHDR(&msg, cmsg); afalg_set_iv_sk(cmsg, iv, ALG_AES_IV_LEN); /* iov that describes input data */ iov.iov_base = (unsigned char *)in; iov.iov_len = inl; msg.msg_flags = MSG_MORE; # ifdef ALG_ZERO_COPY /* * ZERO_COPY mode * Works best when buffer is 4k aligned * OPENS: out of place processing (i.e. out != in) */ /* Input data is not sent as part of call to sendmsg() */ msg.msg_iovlen = 0; msg.msg_iov = NULL; /* Sendmsg() sends iv and cipher direction to the kernel */ sbytes = sendmsg(actx->sfd, &msg, 0); if (sbytes < 0) { ALG_PERR("%s: sendmsg failed for zero copy cipher operation : ", __func__); return 0; } /* * vmsplice and splice are used to pin the user space input buffer for * kernel space processing avoiding copys from user to kernel space */ ret = vmsplice(actx->zc_pipe[1], &iov, 1, SPLICE_F_GIFT); if (ret < 0) { ALG_PERR("%s: vmsplice failed : ", __func__); return 0; } ret = splice(actx->zc_pipe[0], NULL, actx->sfd, NULL, inl, 0); if (ret < 0) { ALG_PERR("%s: splice failed : ", __func__); return 0; } # else msg.msg_iovlen = 1; msg.msg_iov = &iov; /* Sendmsg() sends iv, cipher direction and input data to the kernel */ sbytes = sendmsg(actx->sfd, &msg, 0); if (sbytes < 0) { ALG_PERR("%s: sendmsg failed for cipher operation : ", __func__); return 0; } if (sbytes != (ssize_t) inl) { ALG_WARN("Cipher operation send bytes %zd != inlen %zd\n", sbytes, inl); return 0; } # endif return 1; }
void tftp_request(struct listener *listen, time_t now) { ssize_t len; char *packet = daemon->packet; char *filename, *mode, *p, *end, *opt; union mysockaddr addr, peer; struct msghdr msg; struct iovec iov; struct ifreq ifr; int is_err = 1, if_index = 0, mtu = 0; #ifdef HAVE_DHCP struct iname *tmp; #endif struct tftp_transfer *transfer; int port = daemon->start_tftp_port; /* may be zero to use ephemeral port */ #if defined(IP_MTU_DISCOVER) && defined(IP_PMTUDISC_DONT) int mtuflag = IP_PMTUDISC_DONT; #endif char namebuff[IF_NAMESIZE]; char *name = NULL; char *prefix = daemon->tftp_prefix; struct tftp_prefix *pref; union { struct cmsghdr align; /* this ensures alignment */ #ifdef HAVE_IPV6 char control6[CMSG_SPACE(sizeof(struct in6_pktinfo))]; #endif #if defined(HAVE_LINUX_NETWORK) char control[CMSG_SPACE(sizeof(struct in_pktinfo))]; #elif defined(HAVE_SOLARIS_NETWORK) char control[CMSG_SPACE(sizeof(unsigned int))]; #elif defined(IP_RECVDSTADDR) && defined(IP_RECVIF) char control[CMSG_SPACE(sizeof(struct sockaddr_dl))]; #endif } control_u; msg.msg_controllen = sizeof(control_u); msg.msg_control = control_u.control; msg.msg_flags = 0; msg.msg_name = &peer; msg.msg_namelen = sizeof(peer); msg.msg_iov = &iov; msg.msg_iovlen = 1; iov.iov_base = packet; iov.iov_len = daemon->packet_buff_sz; /* we overwrote the buffer... */ daemon->srv_save = NULL; if ((len = recvmsg(listen->tftpfd, &msg, 0)) < 2) return; if (option_bool(OPT_NOWILD)) { if (listen->iface) { addr = listen->iface->addr; mtu = listen->iface->mtu; name = listen->iface->name; } else { /* we're listening on an address that doesn't appear on an interface, ask the kernel what the socket is bound to */ socklen_t tcp_len = sizeof(union mysockaddr); if (getsockname(listen->tftpfd, (struct sockaddr *)&addr, &tcp_len) == -1) return; } } else { struct cmsghdr *cmptr; if (msg.msg_controllen < sizeof(struct cmsghdr)) return; addr.sa.sa_family = listen->family; #if defined(HAVE_LINUX_NETWORK) if (listen->family == AF_INET) for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_PKTINFO) { union { unsigned char *c; struct in_pktinfo *p; } p; p.c = CMSG_DATA(cmptr); addr.in.sin_addr = p.p->ipi_spec_dst; if_index = p.p->ipi_ifindex; } #elif defined(HAVE_SOLARIS_NETWORK) if (listen->family == AF_INET) for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) { union { unsigned char *c; struct in_addr *a; unsigned int *i; } p; p.c = CMSG_DATA(cmptr); if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVDSTADDR) addr.in.sin_addr = *(p.a); else if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVIF) if_index = *(p.i); } #elif defined(IP_RECVDSTADDR) && defined(IP_RECVIF) if (listen->family == AF_INET) for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) { union { unsigned char *c; struct in_addr *a; struct sockaddr_dl *s; } p; p.c = CMSG_DATA(cmptr); if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVDSTADDR) addr.in.sin_addr = *(p.a); else if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVIF) if_index = p.s->sdl_index; } #endif #ifdef HAVE_IPV6 if (listen->family == AF_INET6) { for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) if (cmptr->cmsg_level == IPPROTO_IPV6 && cmptr->cmsg_type == daemon->v6pktinfo) { union { unsigned char *c; struct in6_pktinfo *p; } p; p.c = CMSG_DATA(cmptr); addr.in6.sin6_addr = p.p->ipi6_addr; if_index = p.p->ipi6_ifindex; } } #endif if (!indextoname(listen->tftpfd, if_index, namebuff)) return; name = namebuff; #ifdef HAVE_IPV6 if (listen->family == AF_INET6) { if (!iface_check(AF_INET6, (struct all_addr *)&addr.in6.sin6_addr, name)) return; } else #endif if (!iface_check(AF_INET, (struct all_addr *)&addr.in.sin_addr, name)) return; #ifdef HAVE_DHCP /* allowed interfaces are the same as for DHCP */ for (tmp = daemon->dhcp_except; tmp; tmp = tmp->next) if (tmp->name && (strcmp(tmp->name, name) == 0)) return; #endif strncpy(ifr.ifr_name, name, IF_NAMESIZE); if (ioctl(listen->tftpfd, SIOCGIFMTU, &ifr) != -1) mtu = ifr.ifr_mtu; } if (name) { /* check for per-interface prefix */ for (pref = daemon->if_prefix; pref; pref = pref->next) if (strcmp(pref->interface, name) == 0) prefix = pref->prefix; } if (listen->family == AF_INET) { addr.in.sin_port = htons(port); #ifdef HAVE_SOCKADDR_SA_LEN addr.in.sin_len = sizeof(addr.in); #endif } #ifdef HAVE_IPV6 else { addr.in6.sin6_port = htons(port); addr.in6.sin6_flowinfo = 0; addr.in6.sin6_scope_id = 0; #ifdef HAVE_SOCKADDR_SA_LEN addr.in6.sin6_len = sizeof(addr.in6); #endif } #endif if (!(transfer = whine_malloc(sizeof(struct tftp_transfer)))) return; if ((transfer->sockfd = socket(listen->family, SOCK_DGRAM, 0)) == -1) { free(transfer); return; } transfer->peer = peer; transfer->timeout = now + 2; transfer->backoff = 1; transfer->block = 1; transfer->blocksize = 512; transfer->offset = 0; transfer->file = NULL; transfer->opt_blocksize = transfer->opt_transize = 0; transfer->netascii = transfer->carrylf = 0; prettyprint_addr(&peer, daemon->addrbuff); /* if we have a nailed-down range, iterate until we find a free one. */ while (1) { if (bind(transfer->sockfd, &addr.sa, sa_len(&addr)) == -1 || #if defined(IP_MTU_DISCOVER) && defined(IP_PMTUDISC_DONT) setsockopt(transfer->sockfd, IPPROTO_IP, IP_MTU_DISCOVER, &mtuflag, sizeof(mtuflag)) == -1 || #endif !fix_fd(transfer->sockfd)) { if (errno == EADDRINUSE && daemon->start_tftp_port != 0) { if (++port <= daemon->end_tftp_port) { if (listen->family == AF_INET) addr.in.sin_port = htons(port); #ifdef HAVE_IPV6 else addr.in6.sin6_port = htons(port); #endif continue; } my_syslog(MS_TFTP | LOG_ERR, _("unable to get free port for TFTP")); } free_transfer(transfer); return; } break; } p = packet + 2; end = packet + len; if (ntohs(*((unsigned short *)packet)) != OP_RRQ || !(filename = next(&p, end)) || !(mode = next(&p, end)) || (strcasecmp(mode, "octet") != 0 && strcasecmp(mode, "netascii") != 0)) { len = tftp_err(ERR_ILL, packet, _("unsupported request from %s"), daemon->addrbuff); is_err = 1; } else { if (strcasecmp(mode, "netascii") == 0) transfer->netascii = 1; while ((opt = next(&p, end))) { if (strcasecmp(opt, "blksize") == 0) { if ((opt = next(&p, end)) && !option_bool(OPT_TFTP_NOBLOCK)) { transfer->blocksize = atoi(opt); if (transfer->blocksize < 1) transfer->blocksize = 1; if (transfer->blocksize > (unsigned)daemon->packet_buff_sz - 4) transfer->blocksize = (unsigned)daemon->packet_buff_sz - 4; /* 32 bytes for IP, UDP and TFTP headers */ if (mtu != 0 && transfer->blocksize > (unsigned)mtu - 32) transfer->blocksize = (unsigned)mtu - 32; transfer->opt_blocksize = 1; transfer->block = 0; } } else if (strcasecmp(opt, "tsize") == 0 && next(&p, end) && !transfer->netascii) { transfer->opt_transize = 1; transfer->block = 0; } } /* cope with backslashes from windows boxen. */ for (p = filename; *p; p++) if (*p == '\\') *p = '/'; else if (option_bool(OPT_TFTP_LC)) *p = tolower(*p); strcpy(daemon->namebuff, "/"); if (prefix) { if (prefix[0] == '/') daemon->namebuff[0] = 0; strncat(daemon->namebuff, prefix, (MAXDNAME-1) - strlen(daemon->namebuff)); if (prefix[strlen(prefix)-1] != '/') strncat(daemon->namebuff, "/", (MAXDNAME-1) - strlen(daemon->namebuff)); if (option_bool(OPT_TFTP_APREF)) { size_t oldlen = strlen(daemon->namebuff); struct stat statbuf; strncat(daemon->namebuff, daemon->addrbuff, (MAXDNAME-1) - strlen(daemon->namebuff)); strncat(daemon->namebuff, "/", (MAXDNAME-1) - strlen(daemon->namebuff)); /* remove unique-directory if it doesn't exist */ if (stat(daemon->namebuff, &statbuf) == -1 || !S_ISDIR(statbuf.st_mode)) daemon->namebuff[oldlen] = 0; } /* Absolute pathnames OK if they match prefix */ if (filename[0] == '/') { if (strstr(filename, daemon->namebuff) == filename) daemon->namebuff[0] = 0; else filename++; } } else if (filename[0] == '/') daemon->namebuff[0] = 0; strncat(daemon->namebuff, filename, (MAXDNAME-1) - strlen(daemon->namebuff)); /* check permissions and open file */ if ((transfer->file = check_tftp_fileperm(&len, prefix))) { if ((len = get_block(packet, transfer)) == -1) len = tftp_err_oops(packet, daemon->namebuff); else is_err = 0; } } while (sendto(transfer->sockfd, packet, len, 0, (struct sockaddr *)&peer, sa_len(&peer)) == -1 && errno == EINTR); if (is_err) free_transfer(transfer); else { transfer->next = daemon->tftp_trans; daemon->tftp_trans = transfer; } }
static int bt_monitor_read(pcap_t *handle, int max_packets _U_, pcap_handler callback, u_char *user) { struct cmsghdr *cmsg; struct msghdr msg; struct iovec iv[2]; ssize_t ret; struct pcap_pkthdr pkth; pcap_bluetooth_linux_monitor_header *bthdr; struct mgmt_hdr hdr; bthdr = (pcap_bluetooth_linux_monitor_header*) &handle->buffer[handle->offset]; iv[0].iov_base = &hdr; iv[0].iov_len = MGMT_HDR_SIZE; iv[1].iov_base = &handle->buffer[handle->offset + sizeof(pcap_bluetooth_linux_monitor_header)]; iv[1].iov_len = handle->snapshot; memset(&pkth.ts, 0, sizeof(pkth.ts)); memset(&msg, 0, sizeof(msg)); msg.msg_iov = iv; msg.msg_iovlen = 2; msg.msg_control = handle->buffer; msg.msg_controllen = handle->offset; do { ret = recvmsg(handle->fd, &msg, 0); if (handle->break_loop) { handle->break_loop = 0; return -2; } } while ((ret == -1) && (errno == EINTR)); if (ret < 0) { snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't receive packet: %s", strerror(errno)); return -1; } pkth.caplen = ret - MGMT_HDR_SIZE + sizeof(pcap_bluetooth_linux_monitor_header); pkth.len = pkth.caplen; for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level != SOL_SOCKET) continue; if (cmsg->cmsg_type == SCM_TIMESTAMP) { memcpy(&pkth.ts, CMSG_DATA(cmsg), sizeof(pkth.ts)); } } bthdr->adapter_id = htons(hdr.index); bthdr->opcode = htons(hdr.opcode); if (handle->fcode.bf_insns == NULL || bpf_filter(handle->fcode.bf_insns, &handle->buffer[handle->offset], pkth.len, pkth.caplen)) { callback(user, &pkth, &handle->buffer[handle->offset]); return 1; } return 0; /* didn't pass filter */ }
AvahiDnsPacket *avahi_recv_dns_packet_ipv4( int fd, AvahiIPv4Address *ret_src_address, uint16_t *ret_src_port, AvahiIPv4Address *ret_dst_address, AvahiIfIndex *ret_iface, uint8_t *ret_ttl) { AvahiDnsPacket *p= NULL; struct msghdr msg; struct iovec io; size_t aux[1024 / sizeof(size_t)]; /* for alignment on ia64 ! */ ssize_t l; struct cmsghdr *cmsg; int found_addr = 0; int ms; struct sockaddr_in sa; assert(fd >= 0); if (ioctl(fd, FIONREAD, &ms) < 0) { avahi_log_warn("ioctl(): %s", strerror(errno)); goto fail; } if (ms < 0) { avahi_log_warn("FIONREAD returned negative value."); goto fail; } p = avahi_dns_packet_new(ms + AVAHI_DNS_PACKET_EXTRA_SIZE); io.iov_base = AVAHI_DNS_PACKET_DATA(p); io.iov_len = p->max_size; memset(&msg, 0, sizeof(msg)); msg.msg_name = &sa; msg.msg_namelen = sizeof(sa); msg.msg_iov = &io; msg.msg_iovlen = 1; msg.msg_control = aux; msg.msg_controllen = sizeof(aux); msg.msg_flags = 0; if ((l = recvmsg(fd, &msg, 0)) < 0) { /* Linux returns EAGAIN when an invalid IP packet has been received. We suppress warnings in this case because this might create quite a bit of log traffic on machines with unstable links. (See #60) */ if (errno != EAGAIN) avahi_log_warn("recvmsg(): %s", strerror(errno)); goto fail; } if (sa.sin_addr.s_addr == INADDR_ANY) { /* Linux 2.4 behaves very strangely sometimes! */ goto fail; } assert(!(msg.msg_flags & MSG_CTRUNC)); assert(!(msg.msg_flags & MSG_TRUNC)); p->size = (size_t) l; if (ret_src_port) *ret_src_port = avahi_port_from_sockaddr((struct sockaddr*) &sa); if (ret_src_address) { AvahiAddress a; avahi_address_from_sockaddr((struct sockaddr*) &sa, &a); *ret_src_address = a.data.ipv4; } if (ret_ttl) *ret_ttl = 255; if (ret_iface) *ret_iface = AVAHI_IF_UNSPEC; for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == IPPROTO_IP) { switch (cmsg->cmsg_type) { #ifdef IP_RECVTTL case IP_RECVTTL: #endif case IP_TTL: if (ret_ttl) *ret_ttl = (uint8_t) (*(int *) CMSG_DATA(cmsg)); break; #ifdef IP_PKTINFO case IP_PKTINFO: { struct in_pktinfo *i = (struct in_pktinfo*) CMSG_DATA(cmsg); if (ret_iface) *ret_iface = (int) i->ipi_ifindex; if (ret_dst_address) ret_dst_address->address = i->ipi_addr.s_addr; found_addr = 1; break; } #endif #ifdef IP_RECVIF case IP_RECVIF: { struct sockaddr_dl *sdl = (struct sockaddr_dl *) CMSG_DATA (cmsg); if (ret_iface) #ifdef __sun *ret_iface = *(uint_t*) sdl; #else *ret_iface = (int) sdl->sdl_index; #endif break; } #endif #ifdef IP_RECVDSTADDR case IP_RECVDSTADDR: if (ret_dst_address) memcpy(&ret_dst_address->address, CMSG_DATA (cmsg), 4); found_addr = 1; break; #endif default: avahi_log_warn("Unhandled cmsg_type : %d", cmsg->cmsg_type); break; } } } assert(found_addr); return p; fail: if (p) avahi_dns_packet_free(p); return NULL; }
int __scm_send(struct socket *sock, struct msghdr *msg, struct scm_cookie *p) { struct cmsghdr *cmsg; int err; for (cmsg = CMSG_FIRSTHDR(msg); cmsg; cmsg = CMSG_NXTHDR(msg, cmsg)) { err = -EINVAL; /* Verify that cmsg_len is at least sizeof(struct cmsghdr) */ /* The first check was omitted in <= 2.2.5. The reasoning was that parser checks cmsg_len in any case, so that additional check would be work duplication. But if cmsg_level is not SOL_SOCKET, we do not check for too short ancillary data object at all! Oops. OK, let's add it... */ if (!CMSG_OK(msg, cmsg)) goto error; if (cmsg->cmsg_level != SOL_SOCKET) continue; switch (cmsg->cmsg_type) { case SCM_RIGHTS: if (!sock->ops || sock->ops->family != PF_UNIX) goto error; err=scm_fp_copy(cmsg, &p->fp); if (err<0) goto error; break; case SCM_CREDENTIALS: if (cmsg->cmsg_len != CMSG_LEN(sizeof(struct ucred))) goto error; memcpy(&p->creds, CMSG_DATA(cmsg), sizeof(struct ucred)); err = scm_check_creds(&p->creds); if (err) goto error; if (!p->pid || pid_vnr(p->pid) != p->creds.pid) { struct pid *pid; err = -ESRCH; pid = find_get_pid(p->creds.pid); if (!pid) goto error; put_pid(p->pid); p->pid = pid; } if (!p->cred || (p->cred->euid != p->creds.uid) || (p->cred->egid != p->creds.gid)) { struct cred *cred; err = -ENOMEM; cred = prepare_creds(); if (!cred) goto error; cred->uid = cred->euid = p->creds.uid; cred->gid = cred->egid = p->creds.gid; if (p->cred) put_cred(p->cred); p->cred = cred; } break; default: goto error; } } if (p->fp && !p->fp->count) { kfree(p->fp); p->fp = NULL; } return 0; error: scm_destroy(p); return err; }
static int process_frames(int dev, int sock, int fd, unsigned long flags) { struct cmsghdr *cmsg; struct msghdr msg; struct iovec iv; struct hcidump_hdr *dh; struct btsnoop_pkt *dp; struct frame frm; struct pollfd fds[2]; int nfds = 0; char *buf, *ctrl; int len, hdr_size = HCIDUMP_HDR_SIZE; if (sock < 0) return -1; if (snap_len < SNAP_LEN) snap_len = SNAP_LEN; if (flags & DUMP_BTSNOOP) hdr_size = BTSNOOP_PKT_SIZE; buf = malloc(snap_len + hdr_size); if (!buf) { perror("Can't allocate data buffer"); return -1; } dh = (void *) buf; dp = (void *) buf; frm.data = buf + hdr_size; ctrl = malloc(100); if (!ctrl) { free(buf); perror("Can't allocate control buffer"); return -1; } if (dev == HCI_DEV_NONE) printf("system: "); else printf("device: hci%d ", dev); printf("snap_len: %d filter: 0x%lx\n", snap_len, parser.filter); memset(&msg, 0, sizeof(msg)); if (mode == SERVER) { struct btsnoop_hdr *hdr = (void *) buf; btsnoop_version = 1; btsnoop_type = 1002; memcpy(hdr->id, btsnoop_id, sizeof(btsnoop_id)); hdr->version = htonl(btsnoop_version); hdr->type = htonl(btsnoop_type); printf("btsnoop version: %d datalink type: %d\n", btsnoop_version, btsnoop_type); len = write(fd, buf, BTSNOOP_HDR_SIZE); if (len < 0) { perror("Can't create dump header"); return -1; } if (len != BTSNOOP_HDR_SIZE) { fprintf(stderr, "Header size mismatch\n"); return -1; } fds[nfds].fd = fd; fds[nfds].events = POLLIN; fds[nfds].revents = 0; nfds++; } fds[nfds].fd = sock; fds[nfds].events = POLLIN; fds[nfds].revents = 0; nfds++; while (1) { int i, n = poll(fds, nfds, -1); if (n <= 0) continue; for (i = 0; i < nfds; i++) { if (fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) { 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 recverr(int fd, int ttl) { int res; struct probehdr rcvbuf; char cbuf[512]; struct iovec iov; struct msghdr msg; struct cmsghdr *cmsg; struct sock_extended_err *e; struct sockaddr_storage addr; struct timeval tv; struct timeval *rettv; int slot = 0; int rethops; int sndhops; int progress = -1; int broken_router; restart: memset(&rcvbuf, -1, sizeof(rcvbuf)); iov.iov_base = &rcvbuf; iov.iov_len = sizeof(rcvbuf); msg.msg_name = (caddr_t)&addr; msg.msg_namelen = sizeof(addr); msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_flags = 0; msg.msg_control = cbuf; msg.msg_controllen = sizeof(cbuf); gettimeofday(&tv, NULL); res = recvmsg(fd, &msg, MSG_ERRQUEUE); if (res < 0) { if (errno == EAGAIN) return progress; goto restart; } progress = mtu; rethops = -1; sndhops = -1; e = NULL; rettv = NULL; slot = -base_port; switch (family) { case AF_INET6: slot += ntohs(((struct sockaddr_in6 *)&addr)->sin6_port); break; case AF_INET: slot += ntohs(((struct sockaddr_in *)&addr)->sin_port); break; } if (slot >= 0 && slot < 63 && his[slot].hops) { sndhops = his[slot].hops; rettv = &his[slot].sendtime; his[slot].hops = 0; } broken_router = 0; if (res == sizeof(rcvbuf)) { if (rcvbuf.ttl == 0 || rcvbuf.tv.tv_sec == 0) broken_router = 1; else { sndhops = rcvbuf.ttl; rettv = &rcvbuf.tv; } } for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { switch (cmsg->cmsg_level) { case SOL_IPV6: switch(cmsg->cmsg_type) { case IPV6_RECVERR: e = (struct sock_extended_err *)CMSG_DATA(cmsg); break; case IPV6_HOPLIMIT: #ifdef IPV6_2292HOPLIMIT case IPV6_2292HOPLIMIT: #endif memcpy(&rethops, CMSG_DATA(cmsg), sizeof(rethops)); break; default: printf("cmsg6:%d\n ", cmsg->cmsg_type); } break; case SOL_IP: switch(cmsg->cmsg_type) { case IP_RECVERR: e = (struct sock_extended_err *)CMSG_DATA(cmsg); break; case IP_TTL: rethops = *(__u8*)CMSG_DATA(cmsg); break; default: printf("cmsg4:%d\n ", cmsg->cmsg_type); } } } if (e == NULL) { printf("no info\n"); return 0; } if (e->ee_origin == SO_EE_ORIGIN_LOCAL) printf("%2d?: %-32s ", ttl, "[LOCALHOST]"); else if (e->ee_origin == SO_EE_ORIGIN_ICMP6 || e->ee_origin == SO_EE_ORIGIN_ICMP) { char abuf[NI_MAXHOST], hbuf[NI_MAXHOST]; struct sockaddr *sa = (struct sockaddr *)(e + 1); socklen_t salen; if (sndhops>0) printf("%2d: ", sndhops); else printf("%2d?: ", ttl); switch (sa->sa_family) { case AF_INET6: salen = sizeof(struct sockaddr_in6); break; case AF_INET: salen = sizeof(struct sockaddr_in); break; default: salen = 0; } if (no_resolve || show_both) { if (getnameinfo(sa, salen, abuf, sizeof(abuf), NULL, 0, NI_NUMERICHOST)) strcpy(abuf, "???"); } else abuf[0] = 0; if (!no_resolve || show_both) { fflush(stdout); if (getnameinfo(sa, salen, hbuf, sizeof(hbuf), NULL, 0, 0 #ifdef USE_IDN | NI_IDN #endif )) strcpy(hbuf, "???"); } else hbuf[0] = 0; if (no_resolve) print_host(abuf, hbuf, show_both); else print_host(hbuf, abuf, show_both); } if (rettv) { int diff = (tv.tv_sec-rettv->tv_sec)*1000000+(tv.tv_usec-rettv->tv_usec); printf("%3d.%03dms ", diff/1000, diff%1000); if (broken_router) printf("(This broken router returned corrupted payload) "); } if (rethops<=64) rethops = 65-rethops; else if (rethops<=128) rethops = 129-rethops; else rethops = 256-rethops; switch (e->ee_errno) { case ETIMEDOUT: printf("\n"); break; case EMSGSIZE: printf("pmtu %d\n", e->ee_info); mtu = e->ee_info; progress = mtu; break; case ECONNREFUSED: printf("reached\n"); hops_to = sndhops<0 ? ttl : sndhops; hops_from = rethops; return 0; case EPROTO: printf("!P\n"); return 0; case EHOSTUNREACH: if ((e->ee_origin == SO_EE_ORIGIN_ICMP && e->ee_type == 11 && e->ee_code == 0) || (e->ee_origin == SO_EE_ORIGIN_ICMP6 && e->ee_type == 3 && e->ee_code == 0)) { if (rethops>=0) { if (sndhops>=0 && rethops != sndhops) printf("asymm %2d ", rethops); else if (sndhops<0 && rethops != ttl) printf("asymm %2d ", rethops); } printf("\n"); break; } printf("!H\n"); return 0; case ENETUNREACH: printf("!N\n"); return 0; case EACCES: printf("!A\n"); return 0; default: printf("\n"); errno = e->ee_errno; perror("NET ERROR"); return 0; } goto restart; }
void mcast_recv_igmp(int sockfd, int version) { unsigned char buf[MAX_RECV_BUF_LEN] = {0}; int num = 0; struct iphdr* ip = NULL; int iph_len = 0, igh_len = 0; struct igmphdr *p_igh; pi_addr pig, pia; struct msghdr msg; struct sockaddr sa; struct iovec iov; struct cmsghdr *cmsg; char *ctrl = (char *) malloc(MAXCTRLSIZE); struct in_pktinfo *info; unsigned int if_index = 0; imp_interface* p_if = NULL; /*To get the unique index of the interfacethe packet was received on*/ bzero(&msg, sizeof(msg)); iov.iov_base = buf; iov.iov_len = 2048; msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = ctrl; msg.msg_controllen = MAXCTRLSIZE; msg.msg_name = &sa; msg.msg_namelen = sizeof(struct sockaddr); num = recvmsg(get_igmp_mld_socket(AF_INET), &msg, MSG_WAITALL); if (num <= sizeof(*ip)) { IMP_LOG_ERROR("BAD packet received n=%d \n", num); free(ctrl); return; } for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_type == IP_PKTINFO) { info = (struct in_pktinfo *) CMSG_DATA(cmsg); if_index = info->ipi_ifindex; } } free(ctrl); /*return if the interface don't enable igmp*/ for(p_if = imp_interface_first();p_if;p_if = LIST_NEXT(p_if, link)) { if(p_if->if_index == if_index && p_if->if_addr.ss.ss_family == AF_INET) break; } if(p_if == NULL){ IMP_LOG_WARNING("Don't exist this VIF\n", if_index); return; } IMP_LOG_DEBUG("src addr = %s\nreceived interface %d\n", imp_inet_ntoa(((struct sockaddr_in*)&sa)->sin_addr.s_addr), if_index); ip = (struct iphdr*)buf; if (ip->protocol != IPPROTO_IGMP && ip->protocol != 0) { IMP_LOG_ERROR("error protocol number %d\n", ip->protocol); return; } imp_build_piaddr(AF_INET, &ip->daddr, &pig); imp_build_piaddr(AF_INET, &ip->saddr, &pia); if (mcast_if_get_by_addr(&pia) != NULL) { IMP_LOG_DEBUG("Ignore igmp from myself\n"); return; } /*when protocol is zero, we need add MFC base one this kind of packet*/ if (ip->protocol == 0 && p_if->type == INTERFACE_UPSTREAM) { if_set ttls; bzero(&ttls, sizeof(ttls)); /*get ttls*/ if(imp_get_mfcc_ttls(&ttls, MAXVIFS, &pia, &pig) != 0){ IMP_LOG_DEBUG("add MFC:src -- %s group -- %s\n\n", imp_pi_ntoa(&pia), imp_pi_ntoa(&pig)); imp_membership_db_mfc_add(&pig, &pia, &ttls); } return; } else if (ip->protocol == 0) { return; } if (p_if->type != INTERFACE_DOWNSTREAM) { IMP_LOG_WARNING("Don't exist this Downstream VIF\n"); return; } #if 0 /*print hex*/ for(;i<num;i++) { if(i%10 == 0) printf("\n"); printf("0x%02x ", buf[i]); } printf("\n"); #endif iph_len = ip->ihl * sizeof(int); if(iph_len < 24) { /*to do:need to judge option if is RAO or not */ IMP_LOG_WARNING("Don't contain Router alter option\n"); return; } igh_len = num - iph_len; p_igh = (struct igmphdr*)((char*)ip + iph_len); if(in_cusm((unsigned short*)p_igh, igh_len) != 0) { IMP_LOG_ERROR("check sum is error\n"); return; } switch (p_igh->type) { case IGMP_HOST_MEMBERSHIP_REPORT: imp_input_report_v1v2(p_if, p_igh); break; case IGMP_HOST_LEAVE_MESSAGE: case IGMPV2_HOST_MEMBERSHIP_REPORT: if (version < IM_IGMPv2_MLDv1) { IMP_LOG_WARNING("Don't support IGMPv2, " \ "current version is igmpv%d\n", version); return; } imp_input_report_v1v2(p_if, p_igh); break; case IGMPV3_HOST_MEMBERSHIP_REPORT: if (version < IM_IGMPv3_MLDv2) { IMP_LOG_WARNING("Don't support IGMPv3, " \ "current version is igmpv%d\n", version); return; } imp_input_report_v3(p_if, (struct igmpv3_report*)p_igh, igh_len); break; default: IMP_LOG_WARNING("unknown igmp type 0x%x\n", p_igh->type); break; } }
void mcast_recv_mld(int sockfd, int version) { unsigned char buf[MAX_RECV_BUF_LEN] = {0}; int num = 0; pi_addr pig, pia; struct msghdr msg; struct sockaddr_in6 sa; struct iovec iov; struct cmsghdr *cmsg; char *ctrl = (char *) malloc(MAXCTRLSIZE); struct in6_pktinfo *info; unsigned int if_index = 0; struct mld_hdr *p_mldh; imp_interface *p_if = NULL; /*To get the unique index of the interfacethe packet was received on*/ bzero(&msg, sizeof(msg)); iov.iov_base = buf; iov.iov_len = 2048; msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = ctrl; msg.msg_controllen = MAXCTRLSIZE; msg.msg_name = &sa; msg.msg_namelen = sizeof(struct sockaddr_in6); num = recvmsg(get_igmp_mld_socket(AF_INET6), &msg, MSG_WAITALL); if (num <= 20) { IMP_LOG_ERROR("BAD packet received n=%d \n", num); free(ctrl); return; } for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_type == IPV6_PKTINFO) { info = (struct in6_pktinfo *) CMSG_DATA(cmsg); if_index = info->ipi6_ifindex; imp_build_piaddr(AF_INET6, &info->ipi6_addr, &pig); imp_build_piaddr(AF_INET6, &sa.sin6_addr, &pia); IMP_LOG_DEBUG("from %s %s %d\n", imp_pi_ntoa(&pig), imp_pi_ntoa(&pia), if_index); } } free(ctrl); if (if_index == 0) { /* * Structure used to communicate from kernel to multicast router. * We'll overlay the structure onto an MLD header (not an IPv6 heder like igmpmsg{} * used for IPv4 implementation). This is because this structure will be passed via an * IPv6 raw socket, on wich an application will only receiver the payload i.e the data after * the IPv6 header and all the extension headers. (See section 3 of RFC 3542) */ struct mrt6msg *p_mrmsg; int mif = 0; p_mrmsg = msg.msg_iov->iov_base; IMP_LOG_DEBUG("DUMP mrmsg:\n\tmif %d\n\tmsgtype %d\n\tsrc %s\n\tgroup %s\n", p_mrmsg->im6_mif, p_mrmsg->im6_msgtype, imp_inet6_ntoa(p_mrmsg->im6_src.s6_addr), imp_inet6_ntoa(p_mrmsg->im6_dst.s6_addr)); imp_build_piaddr(AF_INET6, &p_mrmsg->im6_dst, &pig); imp_build_piaddr(AF_INET6, &p_mrmsg->im6_src, &pia); mif = k_get_vmif(get_up_if_index(), AF_INET6); IMP_LOG_DEBUG("k_get_vmif = %d im6_mif %d\n", mif, p_mrmsg->im6_mif); if (mif == p_mrmsg->im6_mif) { if_set ttls; bzero(&ttls, sizeof(ttls)); /*get ttls*/ if (imp_get_mfcc_ttls(&ttls, MAXVIFS, &pia, &pig) != 0) { IMP_LOG_DEBUG("add MFC:src -- %s group -- %s\n\n", imp_pi_ntoa(&pia), imp_pi_ntoa(&pig)); imp_membership_db_mfc_add(&pig, &pia, &ttls); } } return; } /* An MLDv2 Report MUST be sent with a valid IPv6 link-local source * address, or the unspecified address (::), if the sending interface * has not acquired a valid link-local address yet. * [rfc 3810 5.2.13] */ if (!(IN6_IS_ADDR_LINKLOCAL(&sa.sin6_addr) || IN6_IS_ADDR_UNSPECIFIED(&sa.sin6_addr))) { IMP_LOG_ERROR("MLDv2 Report MUST be sent with a valid IPv6 link-local " "source address, or the unspecified address (::).\n"); return; } /*return if the interface don't enable igmp*/ for(p_if = imp_interface_first();p_if;p_if = LIST_NEXT(p_if, link)) { if(p_if->if_index == if_index && p_if->if_addr.ss.ss_family == AF_INET6) break; } if (p_if == NULL) { IMP_LOG_WARNING("Don't exist this VIF\n"); return; } p_mldh = msg.msg_iov->iov_base; switch (p_mldh->mld_type) { case MLD_LISTENER_REPORT: case MLD_LISTENER_REDUCTION: imp_input_report_mld(p_if, p_mldh); break; case MLD_V2_LISTENER_REPORT: if (version < IM_IGMPv3_MLDv2) { IMP_LOG_WARNING("current version is mldv%d\n", version - 1); return; } imp_input_report_mldv2(p_if, p_mldh, num); break; default: IMP_LOG_WARNING("unknown type %d\n", p_mldh->mld_type); break; } }
/** * Send netlink message. * @arg sk Netlink socket. * @arg msg Netlink message to be sent. * @arg iov iovec to be sent. * @arg iovlen number of struct iovec to be sent. * @see nl_sendmsg() * @return Number of characters sent on success or a negative error code. */ int nl_send_iovec(struct nl_sock *sk, struct nl_msg *msg, struct iovec *iov, unsigned iovlen) { struct sockaddr_nl *dst; struct ucred *creds; struct msghdr hdr = { .msg_name = (void *) &sk->s_peer, .msg_namelen = sizeof(struct sockaddr_nl), .msg_iov = iov, .msg_iovlen = iovlen, }; /* Overwrite destination if specified in the message itself, defaults * to the peer address of the socket. */ dst = nlmsg_get_dst(msg); if (dst->nl_family == AF_NETLINK) hdr.msg_name = dst; /* Add credentials if present. */ creds = nlmsg_get_creds(msg); if (creds != NULL) { char buf[CMSG_SPACE(sizeof(struct ucred))]; struct cmsghdr *cmsg; hdr.msg_control = buf; hdr.msg_controllen = sizeof(buf); cmsg = CMSG_FIRSTHDR(&hdr); cmsg->cmsg_level = SOL_SOCKET; cmsg->cmsg_type = SCM_CREDENTIALS; cmsg->cmsg_len = CMSG_LEN(sizeof(struct ucred)); memcpy(CMSG_DATA(cmsg), creds, sizeof(struct ucred)); } return nl_sendmsg(sk, msg, &hdr); } /** * Send netlink message. * @arg sk Netlink socket. * @arg msg Netlink message to be sent. * @see nl_sendmsg() * @return Number of characters sent on success or a negative error code. */ int nl_send(struct nl_sock *sk, struct nl_msg *msg) { struct iovec iov = { .iov_base = (void *) nlmsg_hdr(msg), .iov_len = nlmsg_hdr(msg)->nlmsg_len, }; return nl_send_iovec(sk, msg, &iov, 1); } void nl_auto_complete(struct nl_sock *sk, struct nl_msg *msg) { struct nlmsghdr *nlh; nlh = nlmsg_hdr(msg); if (nlh->nlmsg_pid == 0) nlh->nlmsg_pid = sk->s_local.nl_pid; if (nlh->nlmsg_seq == 0) nlh->nlmsg_seq = sk->s_seq_next++; if (msg->nm_protocol == -1) msg->nm_protocol = sk->s_proto; nlh->nlmsg_flags |= NLM_F_REQUEST; if (!(sk->s_flags & NL_NO_AUTO_ACK)) nlh->nlmsg_flags |= NLM_F_ACK; } /** * Send netlink message and check & extend header values as needed. * @arg sk Netlink socket. * @arg msg Netlink message to be sent. * * Checks the netlink message \c nlh for completness and extends it * as required before sending it out. Checked fields include pid, * sequence nr, and flags. * * @see nl_send() * @return Number of characters sent or a negative error code. */ int nl_send_auto_complete(struct nl_sock *sk, struct nl_msg *msg) { struct nl_cb *cb = sk->s_cb; nl_auto_complete(sk, msg); if (cb->cb_send_ow) return cb->cb_send_ow(sk, msg); else return nl_send(sk, msg); } /** * Send simple netlink message using nl_send_auto_complete() * @arg sk Netlink socket. * @arg type Netlink message type. * @arg flags Netlink message flags. * @arg buf Data buffer. * @arg size Size of data buffer. * * Builds a netlink message with the specified type and flags and * appends the specified data as payload to the message. * * @see nl_send_auto_complete() * @return Number of characters sent on success or a negative error code. */ int nl_send_simple(struct nl_sock *sk, int type, int flags, void *buf, size_t size) { int err; struct nl_msg *msg; msg = nlmsg_alloc_simple(type, flags); if (!msg) return -NLE_NOMEM; if (buf && size) { err = nlmsg_append(msg, buf, size, NLMSG_ALIGNTO); if (err < 0) goto errout; } err = nl_send_auto_complete(sk, msg); errout: nlmsg_free(msg); return err; } /** @} */ /** * @name Receive * @{ */ /** * Receive data from netlink socket * @arg sk Netlink socket. * @arg nla Destination pointer for peer's netlink address. * @arg buf Destination pointer for message content. * @arg creds Destination pointer for credentials. * * Receives a netlink message, allocates a buffer in \c *buf and * stores the message content. The peer's netlink address is stored * in \c *nla. The caller is responsible for freeing the buffer allocated * in \c *buf if a positive value is returned. Interruped system calls * are handled by repeating the read. The input buffer size is determined * by peeking before the actual read is done. * * A non-blocking sockets causes the function to return immediately with * a return value of 0 if no data is available. * * @return Number of octets read, 0 on EOF or a negative error code. */ int nl_recv(struct nl_sock *sk, struct sockaddr_nl *nla, unsigned char **buf, struct ucred **creds) { int n; int flags = 0; static int page_size = 0; struct iovec iov; struct msghdr msg = { .msg_name = (void *) nla, .msg_namelen = sizeof(struct sockaddr_nl), .msg_iov = &iov, .msg_iovlen = 1, .msg_control = NULL, .msg_controllen = 0, .msg_flags = 0, }; struct cmsghdr *cmsg; if (sk->s_flags & NL_MSG_PEEK) flags |= MSG_PEEK; if (page_size == 0) page_size = getpagesize(); iov.iov_len = page_size; iov.iov_base = *buf = malloc(iov.iov_len); if (sk->s_flags & NL_SOCK_PASSCRED) { msg.msg_controllen = CMSG_SPACE(sizeof(struct ucred)); msg.msg_control = calloc(1, msg.msg_controllen); } retry: n = recvmsg(sk->s_fd, &msg, flags); if (!n) goto abort; else if (n < 0) { if (errno == EINTR) { NL_DBG(3, "recvmsg() returned EINTR, retrying\n"); goto retry; } else if (errno == EAGAIN) { NL_DBG(3, "recvmsg() returned EAGAIN, aborting\n"); goto abort; } else { free(msg.msg_control); free(*buf); return -nl_syserr2nlerr(errno); } } if (iov.iov_len < n || msg.msg_flags & MSG_TRUNC) { /* Provided buffer is not long enough, enlarge it * and try again. */ iov.iov_len *= 2; iov.iov_base = *buf = realloc(*buf, iov.iov_len); goto retry; } else if (msg.msg_flags & MSG_CTRUNC) { msg.msg_controllen *= 2; msg.msg_control = realloc(msg.msg_control, msg.msg_controllen); goto retry; } else if (flags != 0) { /* Buffer is big enough, do the actual reading */ flags = 0; goto retry; } if (msg.msg_namelen != sizeof(struct sockaddr_nl)) { free(msg.msg_control); free(*buf); return -NLE_NOADDR; } for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_CREDENTIALS) { *creds = calloc(1, sizeof(struct ucred)); memcpy(*creds, CMSG_DATA(cmsg), sizeof(struct ucred)); break; } } free(msg.msg_control); return n; abort: free(msg.msg_control); free(*buf); return 0; } #define NL_CB_CALL(cb, type, msg) \ do { \ err = nl_cb_call(cb, type, msg); \ switch (err) { \ case NL_OK: \ err = 0; \ break; \ case NL_SKIP: \ goto skip; \ case NL_STOP: \ goto stop; \ default: \ goto out; \ } \ } while (0) static int recvmsgs(struct nl_sock *sk, struct nl_cb *cb) { int n, err = 0, multipart = 0; unsigned char *buf = NULL; struct nlmsghdr *hdr; struct sockaddr_nl nla = {0}; struct nl_msg *msg = NULL; struct ucred *creds = NULL; continue_reading: NL_DBG(3, "Attempting to read from %p\n", sk); if (cb->cb_recv_ow) n = cb->cb_recv_ow(sk, &nla, &buf, &creds); else n = nl_recv(sk, &nla, &buf, &creds); if (n <= 0) return n; NL_DBG(3, "recvmsgs(%p): Read %d bytes\n", sk, n); hdr = (struct nlmsghdr *) buf; while (nlmsg_ok(hdr, n)) { NL_DBG(3, "recgmsgs(%p): Processing valid message...\n", sk); nlmsg_free(msg); msg = nlmsg_convert(hdr); if (!msg) { err = -NLE_NOMEM; goto out; } nlmsg_set_proto(msg, sk->s_proto); nlmsg_set_src(msg, &nla); if (creds) nlmsg_set_creds(msg, creds); /* Raw callback is the first, it gives the most control * to the user and he can do his very own parsing. */ if (cb->cb_set[NL_CB_MSG_IN]) NL_CB_CALL(cb, NL_CB_MSG_IN, msg); /* Sequence number checking. The check may be done by * the user, otherwise a very simple check is applied * enforcing strict ordering */ if (cb->cb_set[NL_CB_SEQ_CHECK]) NL_CB_CALL(cb, NL_CB_SEQ_CHECK, msg); else if (hdr->nlmsg_seq != sk->s_seq_expect) { if (cb->cb_set[NL_CB_INVALID]) NL_CB_CALL(cb, NL_CB_INVALID, msg); else { err = -NLE_SEQ_MISMATCH; goto out; } } if (hdr->nlmsg_type == NLMSG_DONE || hdr->nlmsg_type == NLMSG_ERROR || hdr->nlmsg_type == NLMSG_NOOP || hdr->nlmsg_type == NLMSG_OVERRUN) { /* We can't check for !NLM_F_MULTI since some netlink * users in the kernel are broken. */ sk->s_seq_expect++; NL_DBG(3, "recvmsgs(%p): Increased expected " \ "sequence number to %d\n", sk, sk->s_seq_expect); } if (hdr->nlmsg_flags & NLM_F_MULTI) multipart = 1; /* Other side wishes to see an ack for this message */ if (hdr->nlmsg_flags & NLM_F_ACK) { if (cb->cb_set[NL_CB_SEND_ACK]) NL_CB_CALL(cb, NL_CB_SEND_ACK, msg); else { /* FIXME: implement */ } } /* messages terminates a multpart message, this is * usually the end of a message and therefore we slip * out of the loop by default. the user may overrule * this action by skipping this packet. */ if (hdr->nlmsg_type == NLMSG_DONE) { multipart = 0; if (cb->cb_set[NL_CB_FINISH]) NL_CB_CALL(cb, NL_CB_FINISH, msg); } /* Message to be ignored, the default action is to * skip this message if no callback is specified. The * user may overrule this action by returning * NL_PROCEED. */ else if (hdr->nlmsg_type == NLMSG_NOOP) { if (cb->cb_set[NL_CB_SKIPPED]) NL_CB_CALL(cb, NL_CB_SKIPPED, msg); else goto skip; } /* Data got lost, report back to user. The default action is to * quit parsing. The user may overrule this action by retuning * NL_SKIP or NL_PROCEED (dangerous) */ else if (hdr->nlmsg_type == NLMSG_OVERRUN) { if (cb->cb_set[NL_CB_OVERRUN]) NL_CB_CALL(cb, NL_CB_OVERRUN, msg); else { err = -NLE_MSG_OVERFLOW; goto out; } } /* Message carries a nlmsgerr */ else if (hdr->nlmsg_type == NLMSG_ERROR) { struct nlmsgerr *e = nlmsg_data(hdr); if (hdr->nlmsg_len < nlmsg_msg_size(sizeof(*e))) { /* Truncated error message, the default action * is to stop parsing. The user may overrule * this action by returning NL_SKIP or * NL_PROCEED (dangerous) */ if (cb->cb_set[NL_CB_INVALID]) NL_CB_CALL(cb, NL_CB_INVALID, msg); else { err = -NLE_MSG_TRUNC; goto out; } } else if (e->error) { /* Error message reported back from kernel. */ if (cb->cb_err) { err = cb->cb_err(&nla, e, cb->cb_err_arg); if (err < 0) goto out; else if (err == NL_SKIP) goto skip; else if (err == NL_STOP) { err = -nl_syserr2nlerr(e->error); goto out; } } else { err = -nl_syserr2nlerr(e->error); goto out; } } else if (cb->cb_set[NL_CB_ACK]) NL_CB_CALL(cb, NL_CB_ACK, msg); } else { /* Valid message (not checking for MULTIPART bit to * get along with broken kernels. NL_SKIP has no * effect on this. */ if (cb->cb_set[NL_CB_VALID]) NL_CB_CALL(cb, NL_CB_VALID, msg); } skip: err = 0; hdr = nlmsg_next(hdr, &n); } nlmsg_free(msg); free(buf); free(creds); buf = NULL; msg = NULL; creds = NULL; if (multipart) { /* Multipart message not yet complete, continue reading */ goto continue_reading; } stop: err = 0; out: nlmsg_free(msg); free(buf); free(creds); return err; }
/* Get a packet from the socket. It also returns the destination addres and * source port of the packet */ int sock_ctrl_recv(int sock, struct lbuf *buf, uconn_t *uc) { union control_data { struct cmsghdr cmsg; u_char data4[CMSG_SPACE(sizeof(struct in_pktinfo))]; u_char data6[CMSG_SPACE(sizeof(struct in6_pktinfo))]; }; union sockunion su; struct msghdr msg; struct iovec iov[1]; union control_data cmsg; struct cmsghdr *cmsgptr = NULL; int nbytes = 0; iov[0].iov_base = lbuf_data(buf); iov[0].iov_len = lbuf_tailroom(buf); memset(&msg, 0, sizeof msg); msg.msg_iov = iov; msg.msg_iovlen = 1; msg.msg_control = &cmsg; msg.msg_controllen = sizeof cmsg; msg.msg_name = &su; msg.msg_namelen = sizeof(union sockunion); nbytes = recvmsg(sock, &msg, 0); if (nbytes == -1) { LMLOG(LWRN, "sock_recv_ctrl: recvmsg error: %s", strerror(errno)); return (BAD); } lbuf_set_size(buf, lbuf_size(buf) + nbytes); /* read local address, remote port and remote address */ if (su.s4.sin_family == AF_INET) { for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)) { if (cmsgptr->cmsg_level == IPPROTO_IP && cmsgptr->cmsg_type == IP_PKTINFO) { lisp_addr_ip_init(&uc->la, &(((struct in_pktinfo *) (CMSG_DATA(cmsgptr)))->ipi_addr), AF_INET); break; } } lisp_addr_ip_init(&uc->ra, &su.s4.sin_addr, AF_INET); uc->rp = ntohs(su.s4.sin_port); } else { for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)) { if (cmsgptr->cmsg_level == IPPROTO_IPV6 && cmsgptr->cmsg_type == IPV6_PKTINFO) { lisp_addr_ip_init(&uc->la, &(((struct in6_pktinfo *) (CMSG_DATA(cmsgptr)))->ipi6_addr), AF_INET6); break; } } lisp_addr_ip_init(&uc->ra, &su.s6.sin6_addr, AF_INET6); uc->rp = ntohs(su.s6.sin6_port); } return (GOOD); }
int sock_data_recv(int sock, lbuf_t *b, int *afi, uint8_t *ttl, uint8_t *tos) { /* Space for TTL and TOS data */ union control_data { struct cmsghdr cmsg; u_char data[CMSG_SPACE(sizeof(int)) + CMSG_SPACE(sizeof(int))]; }; union sockunion su; struct msghdr msg; struct iovec iov[1]; union control_data cmsg; struct cmsghdr *cmsgptr = NULL; int nbytes = 0; iov[0].iov_base = lbuf_data(b); iov[0].iov_len = lbuf_tailroom(b); memset(&msg, 0, sizeof msg); msg.msg_iov = iov; msg.msg_iovlen = 1; msg.msg_control = &cmsg; msg.msg_controllen = sizeof cmsg; msg.msg_name = &su; msg.msg_namelen = sizeof(union sockunion); nbytes = recvmsg(sock, &msg, 0); if (nbytes == -1) { LMLOG(LWRN, "read_packet: recvmsg error: %s", strerror(errno)); return (BAD); } lbuf_set_size(b, lbuf_size(b) + nbytes); if (su.s4.sin_family == AF_INET) { for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr != NULL; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)) { if (cmsgptr->cmsg_level == IPPROTO_IP && cmsgptr->cmsg_type == IP_TTL) { *ttl = *((uint8_t *) CMSG_DATA(cmsgptr)); } if (cmsgptr->cmsg_level == IPPROTO_IP && cmsgptr->cmsg_type == IP_TOS) { *tos = *((uint8_t *) CMSG_DATA(cmsgptr)); } } *afi = AF_INET; } else { for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr != NULL; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)) { if (cmsgptr->cmsg_level == IPPROTO_IPV6 && cmsgptr->cmsg_type == IPV6_HOPLIMIT) { *ttl = *((uint8_t *) CMSG_DATA(cmsgptr)); } if (cmsgptr->cmsg_level == IPPROTO_IPV6 && cmsgptr->cmsg_type == IPV6_TCLASS) { *tos = *((uint8_t *) CMSG_DATA(cmsgptr)); } } *afi = AF_INET6; } return (GOOD); }
AvahiDnsPacket *avahi_recv_dns_packet_ipv6( int fd, AvahiIPv6Address *ret_src_address, uint16_t *ret_src_port, AvahiIPv6Address *ret_dst_address, AvahiIfIndex *ret_iface, uint8_t *ret_ttl) { AvahiDnsPacket *p = NULL; struct msghdr msg; struct iovec io; size_t aux[1024 / sizeof(size_t)]; ssize_t l; int ms; struct cmsghdr *cmsg; int found_ttl = 0, found_iface = 0; struct sockaddr_in6 sa; assert(fd >= 0); if (ioctl(fd, FIONREAD, &ms) < 0) { avahi_log_warn("ioctl(): %s", strerror(errno)); goto fail; } if (ms < 0) { avahi_log_warn("FIONREAD returned negative value."); goto fail; } p = avahi_dns_packet_new(ms + AVAHI_DNS_PACKET_EXTRA_SIZE); io.iov_base = AVAHI_DNS_PACKET_DATA(p); io.iov_len = p->max_size; memset(&msg, 0, sizeof(msg)); msg.msg_name = (struct sockaddr*) &sa; msg.msg_namelen = sizeof(sa); msg.msg_iov = &io; msg.msg_iovlen = 1; msg.msg_control = aux; msg.msg_controllen = sizeof(aux); msg.msg_flags = 0; if ((l = recvmsg(fd, &msg, 0)) < 0) { /* Linux returns EAGAIN when an invalid IP packet has been received. We suppress warnings in this case because this might create quite a bit of log traffic on machines with unstable links. (See #60) */ if (errno != EAGAIN) avahi_log_warn("recvmsg(): %s", strerror(errno)); goto fail; } assert(!(msg.msg_flags & MSG_CTRUNC)); assert(!(msg.msg_flags & MSG_TRUNC)); p->size = (size_t) l; if (ret_src_port) *ret_src_port = avahi_port_from_sockaddr((struct sockaddr*) &sa); if (ret_src_address) { AvahiAddress a; avahi_address_from_sockaddr((struct sockaddr*) &sa, &a); *ret_src_address = a.data.ipv6; } for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level == IPPROTO_IPV6) { switch (cmsg->cmsg_type) { case IPV6_HOPLIMIT: if (ret_ttl) *ret_ttl = (uint8_t) (*(int *) CMSG_DATA(cmsg)); found_ttl = 1; break; case IPV6_PKTINFO: { struct in6_pktinfo *i = (struct in6_pktinfo*) CMSG_DATA(cmsg); if (ret_iface) *ret_iface = i->ipi6_ifindex; if (ret_dst_address) memcpy(ret_dst_address->address, i->ipi6_addr.s6_addr, 16); found_iface = 1; break; } default: avahi_log_warn("Unhandled cmsg_type : %d", cmsg->cmsg_type); break; } } } assert(found_iface); assert(found_ttl); return p; fail: if (p) avahi_dns_packet_free(p); return NULL; }
void main_loop(int icmp_sock, __u8 *packet, int packlen) { char addrbuf[128]; char ans_data[4096]; struct iovec iov; struct msghdr msg; struct cmsghdr *c; int cc; int next; int polling; iov.iov_base = (char *)packet; for (;;) { /* Check exit conditions. */ if (exiting) break; if (npackets && nreceived + nerrors >= npackets) break; if (deadline && nerrors) break; /* Check for and do special actions. */ if (status_snapshot) status(); /* Send probes scheduled to this time. */ do { next = pinger(); next = schedule_exit(next); } while (next <= 0); /* "next" is time to send next probe, if positive. * If next<=0 send now or as soon as possible. */ /* Technical part. Looks wicked. Could be dropped, * if everyone used the newest kernel. :-) * Its purpose is: * 1. Provide intervals less than resolution of scheduler. * Solution: spinning. * 2. Avoid use of poll(), when recvmsg() can provide * timed waiting (SO_RCVTIMEO). */ polling = 0; if ((options & (F_ADAPTIVE|F_FLOOD_POLL)) || next<SCHINT(interval)) { int recv_expected = in_flight(); /* If we are here, recvmsg() is unable to wait for * required timeout. */ if (1000*next <= 1000000/(int)HZ) { /* Very short timeout... So, if we wait for * something, we sleep for MININTERVAL. * Otherwise, spin! */ if (recv_expected) { next = MININTERVAL; } else { next = 0; /* When spinning, no reasons to poll. * Use nonblocking recvmsg() instead. */ polling = MSG_DONTWAIT; /* But yield yet. */ sched_yield(); } } if (!polling && ((options & (F_ADAPTIVE|F_FLOOD_POLL)) || interval)) { struct pollfd pset; pset.fd = icmp_sock; pset.events = POLLIN|POLLERR; pset.revents = 0; if (poll(&pset, 1, next) < 1 || !(pset.revents&(POLLIN|POLLERR))) continue; polling = MSG_DONTWAIT; } } for (;;) { struct timeval *recv_timep = NULL; struct timeval recv_time; int not_ours = 0; /* Raw socket can receive messages * destined to other running pings. */ iov.iov_len = packlen; memset(&msg, 0, sizeof(msg)); msg.msg_name = addrbuf; msg.msg_namelen = sizeof(addrbuf); msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = ans_data; msg.msg_controllen = sizeof(ans_data); cc = recvmsg(icmp_sock, &msg, polling); polling = MSG_DONTWAIT; if (cc < 0) { if (errno == EAGAIN || errno == EINTR) break; if (!receive_error_msg()) { if (errno) { perror("ping: recvmsg"); break; } not_ours = 1; } } else { #ifdef SO_TIMESTAMP for (c = CMSG_FIRSTHDR(&msg); c; c = CMSG_NXTHDR(&msg, c)) { if (c->cmsg_level != SOL_SOCKET || c->cmsg_type != SO_TIMESTAMP) continue; if (c->cmsg_len < CMSG_LEN(sizeof(struct timeval))) continue; recv_timep = (struct timeval*)CMSG_DATA(c); } #endif if ((options&F_LATENCY) || recv_timep == NULL) { if ((options&F_LATENCY) || ioctl(icmp_sock, SIOCGSTAMP, &recv_time)) gettimeofday(&recv_time, NULL); recv_timep = &recv_time; } not_ours = parse_reply(&msg, cc, addrbuf, recv_timep); } /* See? ... someone runs another ping on this host. */ if (not_ours) install_filter(); /* If nothing is in flight, "break" returns us to pinger. */ if (in_flight() == 0) break; /* Otherwise, try to recvmsg() again. recvmsg() * is nonblocking after the first iteration, so that * if nothing is queued, it will receive EAGAIN * and return to pinger. */ } } finish(); }
/** * The main select loop. * * @param flush flush output to disk (out_file) after every message; if 0, flush * only when scan completes */ static int run_scan(int dev_sd, int scan_length, int flush, FILE *out_file) { int rc, len, flush_after_this_message; fd_set readfds_master, readfds; struct timespec select_timeout; sigset_t emptyset; unsigned char buf[HCI_MAX_FRAME_SIZE]; unsigned char control_buf[1024]; /* arbitrary */ struct iovec iov; struct msghdr msg; struct cmsghdr *cmsg; struct timeval tstamp; hci_event_hdr *hdr; /* set up arguments for select */ FD_ZERO(&readfds_master); FD_SET(dev_sd, &readfds_master); select_timeout.tv_sec = SELECT_TIMEOUT; select_timeout.tv_nsec = 0; sigemptyset(&emptyset); /* set up arguments for recvmsg */ iov.iov_base = &buf; iov.iov_len = sizeof(buf); bzero(&msg, sizeof(msg)); msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = &control_buf; msg.msg_controllen = sizeof(control_buf); request_stop_scan = 0; while (!request_stop_scan) { memcpy(&readfds, &readfds_master, sizeof(readfds_master)); rc = pselect(dev_sd + 1, &readfds, NULL, NULL, &select_timeout, &emptyset); if (rc < 0 && errno != EINTR) { syslog(LOG_ERR, "select failed: %m"); return EXIT_FAILURE; } else if (rc == 0) { syslog(LOG_ERR, "select timed out"); return EXIT_FAILURE; } else if (rc > 0) { if (rc != 1) syslog(LOG_ERR, "only one fd in set but rc > 1"); /* OK; some data is ready */ len = recvmsg(dev_sd, &msg, 0); if (len < 0 && errno != EINTR) { syslog(LOG_ERR, "recvmsg: %m"); return EXIT_FAILURE; } else if (len > HCI_EVENT_HDR_SIZE) { /* process the message header to get a high-precision timestamp */ bzero(&tstamp, sizeof(tstamp)); cmsg = CMSG_FIRSTHDR(&msg); while (cmsg) { if (cmsg->cmsg_type == HCI_CMSG_TSTAMP) { tstamp = *((struct timeval *) CMSG_DATA(cmsg)); } cmsg = CMSG_NXTHDR(&msg, cmsg); } /* process the message itself */ if (buf[0] == HCI_EVENT_PKT) { hdr = (hci_event_hdr *)(buf + 1); syslog(LOG_DEBUG, "HCI_EVENT_PKT: evt=%hhd, plen=%hhd", hdr->evt, hdr->plen); /* check that we got all the data; if not, just call recvmsg again */ if (len == 1 + HCI_EVENT_HDR_SIZE + hdr->plen) { /* flush either after each message or upon completion of a scan */ flush_after_this_message = flush; /* dispatch on event */ switch(hdr->evt) { case EVT_INQUIRY_RESULT: rc = handle_inquiry_result(out_file, tstamp, hdr, buf + 3); break; case EVT_INQUIRY_RESULT_WITH_RSSI: rc = handle_inquiry_result_with_rssi(out_file, tstamp, hdr, buf + 3); break; case EVT_INQUIRY_COMPLETE: flush_after_this_message = 1; rc = handle_inquiry_complete(out_file, tstamp, hdr, buf + 3); break; default: rc = EXIT_SUCCESS; syslog(LOG_WARNING, "unknown evt=%hhd", hdr->evt); break; } /* check for message processing failure */ if (rc != EXIT_SUCCESS) break; if (flush_after_this_message) { fflush(out_file); } } else { /* this is not an error; recvmsg may have read part of a message, * and if we call it again, it is clever enough get the rest */ syslog(LOG_DEBUG, "partial read from recvmsg: len=%d, plen=%hhd", len, hdr->plen); } } else { syslog(LOG_WARNING, "got non-HCI_EVENT_PKT: buf[0]=%hhd", buf[0]); } } } } return EXIT_SUCCESS; }
static PyObject *sendmsg_recvmsg(PyObject *self, PyObject *args, PyObject *keywds) { int fd = -1; int flags = 0; int maxsize = 8192; int cmsg_size = 4*1024; size_t cmsg_space; Py_ssize_t recvmsg_result; struct msghdr message_header; struct cmsghdr *control_message; struct iovec iov[1]; char *cmsgbuf; PyObject *ancillary; PyObject *final_result = NULL; static char *kwlist[] = {"fd", "flags", "maxsize", "cmsg_size", NULL}; if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|iii:recvmsg", kwlist, &fd, &flags, &maxsize, &cmsg_size)) { return NULL; } cmsg_space = CMSG_SPACE(cmsg_size); /* overflow check */ if (cmsg_space > SOCKLEN_MAX) { PyErr_Format(PyExc_OverflowError, "CMSG_SPACE(cmsg_size) greater than SOCKLEN_MAX: %d", cmsg_size); return NULL; } message_header.msg_name = NULL; message_header.msg_namelen = 0; iov[0].iov_len = maxsize; iov[0].iov_base = malloc(maxsize); if (!iov[0].iov_base) { PyErr_NoMemory(); return NULL; } message_header.msg_iov = iov; message_header.msg_iovlen = 1; cmsgbuf = malloc(cmsg_space); if (!cmsgbuf) { free(iov[0].iov_base); PyErr_NoMemory(); return NULL; } memset(cmsgbuf, 0, cmsg_space); message_header.msg_control = cmsgbuf; /* see above for overflow check */ message_header.msg_controllen = (socklen_t) cmsg_space; recvmsg_result = recvmsg(fd, &message_header, flags); if (recvmsg_result < 0) { PyErr_SetFromErrno(sendmsg_socket_error); goto finished; } ancillary = PyList_New(0); if (!ancillary) { goto finished; } for (control_message = CMSG_FIRSTHDR(&message_header); control_message; control_message = CMSG_NXTHDR(&message_header, control_message)) { PyObject *entry; /* Some platforms apparently always fill out the ancillary data structure with a single bogus value if none is provided; ignore it, if that is the case. */ if ((!(control_message->cmsg_level)) && (!(control_message->cmsg_type))) { continue; } entry = Py_BuildValue( "(iis#)", control_message->cmsg_level, control_message->cmsg_type, CMSG_DATA(control_message), (Py_ssize_t) (control_message->cmsg_len - sizeof(struct cmsghdr))); if (!entry) { Py_DECREF(ancillary); goto finished; } if (PyList_Append(ancillary, entry) < 0) { Py_DECREF(ancillary); Py_DECREF(entry); goto finished; } else { Py_DECREF(entry); } } final_result = Py_BuildValue( "s#iO", iov[0].iov_base, recvmsg_result, message_header.msg_flags, ancillary); Py_DECREF(ancillary); finished: free(iov[0].iov_base); free(cmsgbuf); return final_result; }
void peisk_bluetoothScan() { int i,j; unsigned char scanRequest[5] = {0x33,0x8b,0x9e,PEISK_BT_SCAN_LENGTH,0x00}; static unsigned char *buf = NULL , *ctrl = NULL; int len, hdr_size = HCIDUMP_HDR_SIZE; struct cmsghdr *cmsg; struct msghdr msg; struct iovec iv; struct hcidump_hdr *dh; struct btsnoop_pkt *dp; struct frame frm; char str[256], val[256]; int n; /* Go through all known bluetooth interfaces and initiate scans on those that are not currenly scanning and have passed their time. */ for(n=0;n<peisk_nBluetoothAdaptors;n++) { PeisBluetoothAdaptor *adaptor = &peisk_bluetoothAdaptors[n]; if(adaptor->interfaceSocket < 0) continue; //printf("Initiating scan on device %d\n",n); /* Handle special case if time goes backwards. */ if(adaptor->nextScan > peisk_timeNow+PEISK_BT_SCAN_PERIOD) adaptor->nextScan = peisk_timeNow+PEISK_BT_SCAN_PERIOD; /* Initiate new scans */ if(adaptor->nextScan < peisk_timeNow) { adaptor->nextScan = peisk_timeNow + PEISK_BT_SCAN_PERIOD; adaptor->isScanning=1; /*printf("Initiating scan on %s\n",adaptor->name);*/ fflush(stdout); hci_send_cmd(adaptor->interfaceSocket,0x01,0x01,sizeof(scanRequest),scanRequest); } /* Process any frames on the raw sockets */ int sock = adaptor->interfaceSocket; int snap_len = HCI_MAX_FRAME_SIZE; if(!buf) buf = malloc(snap_len + hdr_size); if(!buf) { perror("Can't allocate data buffer for bluetooth devices"); return; } dh = (void *) buf; dp = (void *) buf; frm.data = buf + hdr_size; if(!ctrl) ctrl = malloc(100); if (!ctrl) { free(buf); perror("Can't allocate control buffer"); return; } memset(&msg, 0, sizeof(msg)); 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) return; return; } printf("Read %d bytes from bluetooth socket %d\n",len,sock); /* Process control mes printf("Read data is\n"sage */ frm.data_len = len; frm.in = 0; 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; unsigned char btAddr[6]; unsigned char *data = frm.data; printf("frm.len = %d\n",frm.len); peisk_hexDump(data,len); if(frm.len == 4 && data[0] == 0x04 && data[1] == 0x01 && data[2] == 0x01 && data[3] == 0x00) { adaptor->isScanning = 0; /*printf("Interface %s is FINISHED\n",adaptor->name);*/ } /* Short "normal" inquiry results */ if(frm.len == 18 && data[0] == 0x04 && data[1] == 0x22 && data[2] == 0x0f) { for(i=0;i<6;i++) btAddr[i] = data[9-i]; for(i=0;i<peisk_nBluetoothDevices;i++) { for(j=0;j<6;j++) if(btAddr[j] != peisk_bluetoothDevices[i].btAddr[j]) break; if(j == 6) break; /*else printf("does not match %d (j=%d)\n",i,j);*/ } if(i == peisk_nBluetoothDevices) { /* No previous device found */ peisk_nBluetoothDevices++; for(j=0;j<6;j++) peisk_bluetoothDevices[i].btAddr[j] = btAddr[j]; for(j=0;j<PEISK_MAX_BLUETOOTH_ADAPTORS;j++) { peisk_bluetoothDevices[i].rawStrength[j]=0; peisk_bluetoothDevices[i].lastSeen[j]=0.0; } peisk_bluetoothDevices[i].name[0]=0; peisk_bluetoothDevices[i].nextHelloAttempt=0.0; peisk_bluetoothDevices[i].isConnectable=0; printf("Found a new device: "); for(j=0;j<6;j++) printf("%02x:",peisk_bluetoothDevices[i].btAddr[j]); printf("\n"); } if(peisk_bluetoothDevices[i].name[0] == 0 && 0) { /** \todo Read names of bluetooth devices asynchronosly */ /* Scan for name of this device */ bdaddr_t baddr; for(j=0;j<6;j++) baddr.b[j] = btAddr[5-j]; printf("Attempting to read remote name of: "); ba2str(&baddr,val); printf("%s\n",val); int s = hci_open_dev(adaptor->id); if(hci_read_remote_name(s,&baddr,255,val,500) == 0) { printf("Read name: %s\n",val); strncpy(peisk_bluetoothDevices[i].name,val,255); sprintf(str,"kernel.%s.%02X%02X%02X%02X%02X%02X.name",adaptor->name, btAddr[0],btAddr[1],btAddr[2],btAddr[3],btAddr[4],btAddr[5]); peisk_setStringTuple(str,val); } else perror("Failed"); close(s); } peisk_bluetoothDevices[i].lastSeen[n] = peisk_timeNow; peisk_bluetoothDevices[i].rawStrength[n] = data[17]; /*printf("Device %d, iface %s <- %d\n",i,adaptor->name,data[17]);*/ sprintf(str,"kernel.%s.%02X%02X%02X%02X%02X%02X.str",adaptor->name, btAddr[0],btAddr[1],btAddr[2],btAddr[3],btAddr[4],btAddr[5]); sprintf(val,"%d",data[17]); peisk_setStringTuple(str,val); } else if(frm.len == 258 && data[0] == 0x04 && data[1] == 0x2f && data[2] == 0xff && data[3] == 0x01) { for(i=0;i<6;i++) btAddr[i] = data[9-i]; for(i=0;i<peisk_nBluetoothDevices;i++) { for(j=0;j<6;j++) if(btAddr[j] != peisk_bluetoothDevices[i].btAddr[j]) break; if(j == 6) break; /*else printf("does not match %d (j=%d)\n",i,j);*/ } if(i == peisk_nBluetoothDevices) { /* No previous device found */ peisk_nBluetoothDevices++; for(j=0;j<6;j++) peisk_bluetoothDevices[i].btAddr[j] = btAddr[j]; for(j=0;j<PEISK_MAX_BLUETOOTH_ADAPTORS;j++) { peisk_bluetoothDevices[i].rawStrength[j]=0; peisk_bluetoothDevices[i].lastSeen[j]=0.0; } peisk_bluetoothDevices[i].name[0]=0; peisk_bluetoothDevices[i].nextHelloAttempt=0.0; peisk_bluetoothDevices[i].isConnectable=0; printf("Found a new device: "); for(j=0;j<6;j++) printf("%02x:",peisk_bluetoothDevices[i].btAddr[j]); printf("\n"); } peisk_bluetoothDevices[i].lastSeen[n] = peisk_timeNow; peisk_bluetoothDevices[i].rawStrength[n] = data[17]; sprintf(str,"kernel.%s.%02X%02X%02X%02X%02X%02X.str",adaptor->name, btAddr[0],btAddr[1],btAddr[2],btAddr[3],btAddr[4],btAddr[5]); sprintf(val,"%d",data[17]); peisk_setStringTuple(str,val); } } }
void dhcp_packet(time_t now, int pxe_fd) { int fd = pxe_fd ? daemon->pxefd : daemon->dhcpfd; struct dhcp_packet *mess; struct dhcp_context *context; struct iname *tmp; struct ifreq ifr; struct msghdr msg; struct sockaddr_in dest; struct cmsghdr *cmptr; struct iovec iov; ssize_t sz; int iface_index = 0, unicast_dest = 0, is_inform = 0; struct in_addr iface_addr; struct iface_param parm; #ifdef HAVE_LINUX_NETWORK struct arpreq arp_req; #endif union { struct cmsghdr align; /* this ensures alignment */ #if defined(HAVE_LINUX_NETWORK) char control[CMSG_SPACE(sizeof(struct in_pktinfo))]; #elif defined(HAVE_SOLARIS_NETWORK) char control[CMSG_SPACE(sizeof(unsigned int))]; #elif defined(HAVE_BSD_NETWORK) char control[CMSG_SPACE(sizeof(struct sockaddr_dl))]; #endif } control_u; struct dhcp_bridge *bridge, *alias; msg.msg_controllen = sizeof(control_u); msg.msg_control = control_u.control; msg.msg_name = &dest; msg.msg_namelen = sizeof(dest); msg.msg_iov = &daemon->dhcp_packet; msg.msg_iovlen = 1; if ((sz = recv_dhcp_packet(fd, &msg)) == -1 || (sz < (ssize_t)(sizeof(*mess) - sizeof(mess->options)))) return; #if defined (HAVE_LINUX_NETWORK) if (msg.msg_controllen >= sizeof(struct cmsghdr)) for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_PKTINFO) { union { unsigned char *c; struct in_pktinfo *p; } p; p.c = CMSG_DATA(cmptr); iface_index = p.p->ipi_ifindex; if (p.p->ipi_addr.s_addr != INADDR_BROADCAST) unicast_dest = 1; } #elif defined(HAVE_BSD_NETWORK) if (msg.msg_controllen >= sizeof(struct cmsghdr)) for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVIF) { union { unsigned char *c; struct sockaddr_dl *s; } p; p.c = CMSG_DATA(cmptr); iface_index = p.s->sdl_index; } #elif defined(HAVE_SOLARIS_NETWORK) if (msg.msg_controllen >= sizeof(struct cmsghdr)) for (cmptr = CMSG_FIRSTHDR(&msg); cmptr; cmptr = CMSG_NXTHDR(&msg, cmptr)) if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVIF) { union { unsigned char *c; unsigned int *i; } p; p.c = CMSG_DATA(cmptr); iface_index = *(p.i); } #endif if (!indextoname(daemon->dhcpfd, iface_index, ifr.ifr_name)) return; #ifdef HAVE_LINUX_NETWORK /* ARP fiddling uses original interface even if we pretend to use a different one. */ strncpy(arp_req.arp_dev, ifr.ifr_name, 16); #endif /* One form of bridging on BSD has the property that packets can be recieved on bridge interfaces which do not have an IP address. We allow these to be treated as aliases of another interface which does have an IP address with --dhcp-bridge=interface,alias,alias */ for (bridge = daemon->bridges; bridge; bridge = bridge->next) { for (alias = bridge->alias; alias; alias = alias->next) if (strncmp(ifr.ifr_name, alias->iface, IF_NAMESIZE) == 0) { if (!(iface_index = if_nametoindex(bridge->iface))) { my_syslog(LOG_WARNING, _("unknown interface %s in bridge-interface"), ifr.ifr_name); return; } else { strncpy(ifr.ifr_name, bridge->iface, IF_NAMESIZE); break; } } if (alias) break; } #ifdef MSG_BCAST /* OpenBSD tells us when a packet was broadcast */ if (!(msg.msg_flags & MSG_BCAST)) unicast_dest = 1; #endif ifr.ifr_addr.sa_family = AF_INET; if (ioctl(daemon->dhcpfd, SIOCGIFADDR, &ifr) != -1 ) iface_addr = ((struct sockaddr_in *) &ifr.ifr_addr)->sin_addr; else { my_syslog(MS_DHCP | LOG_WARNING, _("DHCP packet received on %s which has no address"), ifr.ifr_name); return; } for (tmp = daemon->dhcp_except; tmp; tmp = tmp->next) if (tmp->name && (strcmp(tmp->name, ifr.ifr_name) == 0)) return; /* weird libvirt-inspired access control */ for (context = daemon->dhcp; context; context = context->next) if (!context->interface || strcmp(context->interface, ifr.ifr_name) == 0) break; if (!context) return; /* unlinked contexts are marked by context->current == context */ for (context = daemon->dhcp; context; context = context->next) context->current = context; parm.current = NULL; parm.ind = iface_index; if (!iface_check(AF_INET, (struct all_addr *)&iface_addr, ifr.ifr_name)) { /* If we failed to match the primary address of the interface, see if we've got a --listen-address for a secondary */ struct match_param match; match.matched = 0; match.ind = iface_index; if (!daemon->if_addrs || !iface_enumerate(AF_INET, &match, check_listen_addrs) || !match.matched) return; iface_addr = match.addr; /* make sure secondary address gets priority in case there is more than one address on the interface in the same subnet */ complete_context(match.addr, iface_index, match.netmask, match.broadcast, &parm); } if (!iface_enumerate(AF_INET, &parm, complete_context)) return; lease_prune(NULL, now); /* lose any expired leases */ iov.iov_len = dhcp_reply(parm.current, ifr.ifr_name, iface_index, (size_t)sz, now, unicast_dest, &is_inform, pxe_fd, iface_addr); lease_update_file(now); lease_update_dns(0); if (iov.iov_len == 0) return; msg.msg_name = &dest; msg.msg_namelen = sizeof(dest); msg.msg_control = NULL; msg.msg_controllen = 0; msg.msg_iov = &iov; iov.iov_base = daemon->dhcp_packet.iov_base; /* packet buffer may have moved */ mess = (struct dhcp_packet *)daemon->dhcp_packet.iov_base; #ifdef HAVE_SOCKADDR_SA_LEN dest.sin_len = sizeof(struct sockaddr_in); #endif if (pxe_fd) { if (mess->ciaddr.s_addr != 0) dest.sin_addr = mess->ciaddr; } else if (mess->giaddr.s_addr) { /* Send to BOOTP relay */ dest.sin_port = htons(daemon->dhcp_server_port); dest.sin_addr = mess->giaddr; } else if (mess->ciaddr.s_addr) { /* If the client's idea of its own address tallys with the source address in the request packet, we believe the source port too, and send back to that. If we're replying to a DHCPINFORM, trust the source address always. */ if ((!is_inform && dest.sin_addr.s_addr != mess->ciaddr.s_addr) || dest.sin_port == 0 || dest.sin_addr.s_addr == 0) { dest.sin_port = htons(daemon->dhcp_client_port); dest.sin_addr = mess->ciaddr; } } #if defined(HAVE_LINUX_NETWORK) else if ((ntohs(mess->flags) & 0x8000) || mess->hlen == 0 || mess->hlen > sizeof(ifr.ifr_addr.sa_data) || mess->htype == 0) { /* broadcast to 255.255.255.255 (or mac address invalid) */ struct in_pktinfo *pkt; msg.msg_control = control_u.control; msg.msg_controllen = sizeof(control_u); cmptr = CMSG_FIRSTHDR(&msg); pkt = (struct in_pktinfo *)CMSG_DATA(cmptr); pkt->ipi_ifindex = iface_index; pkt->ipi_spec_dst.s_addr = 0; msg.msg_controllen = cmptr->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo)); cmptr->cmsg_level = IPPROTO_IP; cmptr->cmsg_type = IP_PKTINFO; dest.sin_addr.s_addr = INADDR_BROADCAST; dest.sin_port = htons(daemon->dhcp_client_port); } else { /* unicast to unconfigured client. Inject mac address direct into ARP cache. struct sockaddr limits size to 14 bytes. */ dest.sin_addr = mess->yiaddr; dest.sin_port = htons(daemon->dhcp_client_port); memcpy(&arp_req.arp_pa, &dest, sizeof(struct sockaddr_in)); arp_req.arp_ha.sa_family = mess->htype; memcpy(arp_req.arp_ha.sa_data, mess->chaddr, mess->hlen); /* interface name already copied in */ arp_req.arp_flags = ATF_COM; ioctl(daemon->dhcpfd, SIOCSARP, &arp_req); } #elif defined(HAVE_SOLARIS_NETWORK) else if ((ntohs(mess->flags) & 0x8000) || mess->hlen != ETHER_ADDR_LEN || mess->htype != ARPHRD_ETHER)
static ERL_NIF_TERM nif_recvmsg(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]) { int s = -1; ErlNifBinary buf = {0}; ErlNifBinary src_addr = {0}; char *ctrldata = NULL; ERL_NIF_TERM ctrldatalist; int flags = 0; unsigned long bufsize = 0; unsigned long ctrlsize = 0; unsigned long sasize = 0; struct iovec iov[1]; struct msghdr message; struct cmsghdr *cmsg; ssize_t n = 0; if (!enif_get_int(env, argv[0], &s)) return enif_make_badarg(env); if (!enif_get_ulong(env, argv[1], &bufsize)) return enif_make_badarg(env); if (!enif_get_int(env, argv[2], &flags)) return enif_make_badarg(env); if (!enif_get_ulong(env, argv[3], &ctrlsize)) return enif_make_badarg(env); if (!enif_get_ulong(env, argv[4], &sasize)) return enif_make_badarg(env); if (!enif_alloc_binary(bufsize, &buf)) return error_tuple(env, ENOMEM); if (ctrlsize > 0 && !(ctrldata = malloc(ctrlsize))) { enif_release_binary(&buf); return error_tuple(env, ENOMEM); } if (!enif_alloc_binary(sasize, &src_addr)) { enif_release_binary(&buf); free(ctrldata); return error_tuple(env, ENOMEM); } iov[0].iov_base = (buf.size == 0 ? NULL : buf.data); iov[0].iov_len=buf.size; message.msg_name = (src_addr.size == 0 ? NULL : src_addr.data); message.msg_namelen=src_addr.size; message.msg_iov=iov; message.msg_iovlen=1; message.msg_control=ctrldata; message.msg_controllen=ctrlsize; n = recvmsg(s, &message, flags); if (n < 0) { int err = errno; enif_release_binary(&buf); enif_release_binary(&src_addr); free(ctrldata); return error_tuple(env, err); } /* resize the binary to the actual size of the received packet * * XXX On error, the macro will return ENOMEM here, leaking buf, * XXX src_addr and ctrldata. Since the VM has OOM'ed, it will probably * XXX crash anyway. */ PROCKET_REALLOC(buf, n); PROCKET_REALLOC(src_addr, message.msg_namelen); ctrldatalist = enif_make_list(env, 0); for (cmsg = CMSG_FIRSTHDR(&message); cmsg != NULL; cmsg = CMSG_NXTHDR(&message, cmsg)) { size_t len = cmsg->cmsg_len - CMSG_LEN(0); ErlNifBinary cdata = {0}; if (!enif_alloc_binary(len, &cdata)) { enif_release_binary(&buf); enif_release_binary(&src_addr); free(ctrldata); return error_tuple(env, ENOMEM); } memcpy(cdata.data, CMSG_DATA(cmsg), len); ctrldatalist = enif_make_list_cell(env, enif_make_tuple3(env, enif_make_int(env, cmsg->cmsg_level), enif_make_int(env, cmsg->cmsg_type), enif_make_binary(env, &cdata)), ctrldatalist); } free(ctrldata); return enif_make_tuple5(env, atom_ok, enif_make_binary(env, &buf), /* Data packet */ enif_make_int(env, message.msg_flags), /* the message flags, eg. MSG_EOR, MSG_OOB, etc. */ ctrldatalist, /* array of 3-tuples of {cmsg->cmsg_level, cmsg->cmsg_type, cmsg->cmsg_data} */ enif_make_binary(env, &src_addr) /* source address, as a sockaddr_storage */ ); }
static void uv__read(uv_stream_t* stream) { uv_buf_t buf; ssize_t nread; struct msghdr msg; struct cmsghdr* cmsg; char cmsg_space[64]; int count; /* Prevent loop starvation when the data comes in as fast as (or faster than) * we can read it. XXX Need to rearm fd if we switch to edge-triggered I/O. */ count = 32; /* XXX: Maybe instead of having UV_STREAM_READING we just test if * tcp->read_cb is NULL or not? */ while ((stream->read_cb || stream->read2_cb) && (stream->flags & UV_STREAM_READING) && (count-- > 0)) { assert(stream->alloc_cb); buf = stream->alloc_cb((uv_handle_t*)stream, 64 * 1024); assert(buf.len > 0); assert(buf.base); assert(stream->fd >= 0); if (stream->read_cb) { do { nread = read(stream->fd, buf.base, buf.len); } while (nread < 0 && errno == EINTR); } else { assert(stream->read2_cb); /* read2_cb uses recvmsg */ msg.msg_flags = 0; msg.msg_iov = (struct iovec*) &buf; msg.msg_iovlen = 1; msg.msg_name = NULL; msg.msg_namelen = 0; /* Set up to receive a descriptor even if one isn't in the message */ msg.msg_controllen = 64; msg.msg_control = (void *) cmsg_space; do { nread = recvmsg(stream->fd, &msg, 0); } while (nread < 0 && errno == EINTR); } if (nread < 0) { /* Error */ if (errno == EAGAIN || errno == EWOULDBLOCK) { /* Wait for the next one. */ if (stream->flags & UV_STREAM_READING) { uv__io_start(stream->loop, &stream->read_watcher); } uv__set_sys_error(stream->loop, EAGAIN); if (stream->read_cb) { stream->read_cb(stream, 0, buf); } else { stream->read2_cb((uv_pipe_t*)stream, 0, buf, UV_UNKNOWN_HANDLE); } return; } else { /* Error. User should call uv_close(). */ uv__set_sys_error(stream->loop, errno); if (stream->read_cb) { stream->read_cb(stream, -1, buf); } else { stream->read2_cb((uv_pipe_t*)stream, -1, buf, UV_UNKNOWN_HANDLE); } assert(!uv__io_active(&stream->read_watcher)); return; } } else if (nread == 0) { /* EOF */ uv__set_artificial_error(stream->loop, UV_EOF); uv__io_stop(stream->loop, &stream->read_watcher); if (!uv__io_active(&stream->write_watcher)) uv__handle_stop(stream); if (stream->read_cb) { stream->read_cb(stream, -1, buf); } else { stream->read2_cb((uv_pipe_t*)stream, -1, buf, UV_UNKNOWN_HANDLE); } return; } else { /* Successful read */ ssize_t buflen = buf.len; if (stream->read_cb) { stream->read_cb(stream, nread, buf); } else { assert(stream->read2_cb); /* * XXX: Some implementations can send multiple file descriptors in a * single message. We should be using CMSG_NXTHDR() to walk the * chain to get at them all. This would require changing the API to * hand these back up the caller, is a pain. */ for (cmsg = CMSG_FIRSTHDR(&msg); msg.msg_controllen > 0 && cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_type == SCM_RIGHTS) { if (stream->accepted_fd != -1) { fprintf(stderr, "(libuv) ignoring extra FD received\n"); } stream->accepted_fd = *(int *) CMSG_DATA(cmsg); } else { fprintf(stderr, "ignoring non-SCM_RIGHTS ancillary data: %d\n", cmsg->cmsg_type); } } if (stream->accepted_fd >= 0) { stream->read2_cb((uv_pipe_t*)stream, nread, buf, uv__handle_type(stream->accepted_fd)); } else { stream->read2_cb((uv_pipe_t*)stream, nread, buf, UV_UNKNOWN_HANDLE); } } /* Return if we didn't fill the buffer, there is no more data to read. */ if (nread < buflen) { return; } } } }
static ERL_NIF_TERM nif_sendmsg(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]) { int sockfd = -1; int flags = 0; ssize_t n = 0; ErlNifBinary buf = {0}; ErlNifBinary sa = {0}; ERL_NIF_TERM cdata_list, head, tail; char *cdata = NULL; struct iovec iov[1]; struct msghdr message; struct cmsghdr *cmsg; size_t cdata_size = 0; if (!enif_get_int(env, argv[0], &sockfd)) return enif_make_badarg(env); if (!enif_inspect_binary(env, argv[1], &buf)) return enif_make_badarg(env); if (!enif_get_int(env, argv[2], &flags)) return enif_make_badarg(env); if (!enif_is_list(env, argv[3])) return enif_make_badarg(env); if (!enif_inspect_binary(env, argv[4], &sa)) return enif_make_badarg(env); cdata_list = argv[3]; // figure out how much control data we'll need to send while(enif_get_list_cell(env, cdata_list, &head, &tail)) { const ERL_NIF_TERM* fields; int arity; int level, type; ErlNifBinary cdata_field = {0}; if (!enif_get_tuple(env, head, &arity, &fields) || arity != 3) { return enif_make_badarg(env); } if (!enif_get_int(env, fields[0], &level)) { return enif_make_badarg(env); } if (!enif_get_int(env, fields[1], &type)) { return enif_make_badarg(env); } if (!enif_inspect_binary(env, fields[2], &cdata_field)) { return enif_make_badarg(env); } cdata_size += CMSG_SPACE(cdata_field.size); cdata_list = tail; } if (cdata_size > 0) { // allocate enough control data space, if any // freebsd throws einval if the cdata length is 0 // but the pointer isn't NULL if (!(cdata = malloc(cdata_size))) { return error_tuple(env, ENOMEM); } } // set up the iov and msghdr stuff iov[0].iov_base=(buf.size == 0 ? NULL : buf.data); iov[0].iov_len=buf.size; message.msg_name=(sa.size == 0 ? NULL : sa.data); message.msg_namelen=sa.size; message.msg_iov=iov; message.msg_iovlen=1; message.msg_control=cdata; message.msg_controllen=cdata_size; // loop over the control data again, this time filling in the data in the // msghdr cdata_list = argv[3]; cmsg = CMSG_FIRSTHDR(&message); while(enif_get_list_cell(env, cdata_list, &head, &tail)) { const ERL_NIF_TERM* fields; int arity; unsigned char *cmsg_data; ErlNifBinary cdata_field = {0}; // don't need to check here, we'd have crashed in the last loop if // things were wrong enif_get_tuple(env, head, &arity, &fields); enif_get_int(env, fields[0], &cmsg->cmsg_level); enif_get_int(env, fields[1], &cmsg->cmsg_type); enif_inspect_binary(env, fields[2], &cdata_field); cmsg_data = CMSG_DATA(cmsg); // copy the control data into the cdata struct memcpy(cmsg_data, cdata_field.data, cdata_field.size); // set the length cmsg->cmsg_len=CMSG_LEN(cdata_field.size); cdata_list = tail; cmsg = CMSG_NXTHDR(&message,cmsg); } n = sendmsg(sockfd, &message, flags); if (n < 0) return error_tuple(env, errno); return atom_ok; }
static int dgram_sctp_read(BIO *b, char *out, int outl) { int ret = 0, n = 0, i, optval; socklen_t optlen; bio_dgram_sctp_data *data = (bio_dgram_sctp_data *)b->ptr; union sctp_notification *snp; struct msghdr msg; struct iovec iov; struct cmsghdr *cmsg; char cmsgbuf[512]; if (out != NULL) { clear_socket_error(); do { memset(&data->rcvinfo, 0x00, sizeof(struct bio_dgram_sctp_rcvinfo)); iov.iov_base = out; iov.iov_len = outl; msg.msg_name = NULL; msg.msg_namelen = 0; msg.msg_iov = &iov; msg.msg_iovlen = 1; msg.msg_control = cmsgbuf; msg.msg_controllen = 512; msg.msg_flags = 0; n = recvmsg(b->num, &msg, 0); if (msg.msg_controllen > 0) { for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_level != IPPROTO_SCTP) continue; #ifdef SCTP_RCVINFO if (cmsg->cmsg_type == SCTP_RCVINFO) { struct sctp_rcvinfo *rcvinfo; rcvinfo = (struct sctp_rcvinfo *)CMSG_DATA(cmsg); data->rcvinfo.rcv_sid = rcvinfo->rcv_sid; data->rcvinfo.rcv_ssn = rcvinfo->rcv_ssn; data->rcvinfo.rcv_flags = rcvinfo->rcv_flags; data->rcvinfo.rcv_ppid = rcvinfo->rcv_ppid; data->rcvinfo.rcv_tsn = rcvinfo->rcv_tsn; data->rcvinfo.rcv_cumtsn = rcvinfo->rcv_cumtsn; data->rcvinfo.rcv_context = rcvinfo->rcv_context; } #endif #ifdef SCTP_SNDRCV if (cmsg->cmsg_type == SCTP_SNDRCV) { struct sctp_sndrcvinfo *sndrcvinfo; sndrcvinfo = (struct sctp_sndrcvinfo *)CMSG_DATA(cmsg); data->rcvinfo.rcv_sid = sndrcvinfo->sinfo_stream; data->rcvinfo.rcv_ssn = sndrcvinfo->sinfo_ssn; data->rcvinfo.rcv_flags = sndrcvinfo->sinfo_flags; data->rcvinfo.rcv_ppid = sndrcvinfo->sinfo_ppid; data->rcvinfo.rcv_tsn = sndrcvinfo->sinfo_tsn; data->rcvinfo.rcv_cumtsn = sndrcvinfo->sinfo_cumtsn; data->rcvinfo.rcv_context = sndrcvinfo->sinfo_context; } #endif } } if (n <= 0) { if (n < 0) ret = n; break; } if (msg.msg_flags & MSG_NOTIFICATION) { snp = (union sctp_notification*) out; if (snp->sn_header.sn_type == SCTP_SENDER_DRY_EVENT) { #ifdef SCTP_EVENT struct sctp_event event; #else struct sctp_event_subscribe event; socklen_t eventsize; #endif /* If a message has been delayed until the socket * is dry, it can be sent now. */ if (data->saved_message.length > 0) { dgram_sctp_write(data->saved_message.bio, data->saved_message.data, data->saved_message.length); OPENSSL_free(data->saved_message.data); data->saved_message.length = 0; } /* disable sender dry event */ #ifdef SCTP_EVENT memset(&event, 0, sizeof(struct sctp_event)); event.se_assoc_id = 0; event.se_type = SCTP_SENDER_DRY_EVENT; event.se_on = 0; i = setsockopt(b->num, IPPROTO_SCTP, SCTP_EVENT, &event, sizeof(struct sctp_event)); OPENSSL_assert(i >= 0); #else eventsize = sizeof(struct sctp_event_subscribe); i = getsockopt(b->num, IPPROTO_SCTP, SCTP_EVENTS, &event, &eventsize); OPENSSL_assert(i >= 0); event.sctp_sender_dry_event = 0; i = setsockopt(b->num, IPPROTO_SCTP, SCTP_EVENTS, &event, sizeof(struct sctp_event_subscribe)); OPENSSL_assert(i >= 0); #endif } #ifdef SCTP_AUTHENTICATION_EVENT if (snp->sn_header.sn_type == SCTP_AUTHENTICATION_EVENT) dgram_sctp_handle_auth_free_key_event(b, snp); #endif if (data->handle_notifications != NULL) data->handle_notifications(b, data->notification_context, (void*) out); memset(out, 0, outl); } else ret += n; } while ((msg.msg_flags & MSG_NOTIFICATION) && (msg.msg_flags & MSG_EOR) && (ret < outl)); if (ret > 0 && !(msg.msg_flags & MSG_EOR)) { /* Partial message read, this should never happen! */ /* The buffer was too small, this means the peer sent * a message that was larger than allowed. */ if (ret == outl) return -1; /* Test if socket buffer can handle max record * size (2^14 + 2048 + 13) */ optlen = (socklen_t) sizeof(int); ret = getsockopt(b->num, SOL_SOCKET, SO_RCVBUF, &optval, &optlen); OPENSSL_assert(ret >= 0); OPENSSL_assert(optval >= 18445); /* Test if SCTP doesn't partially deliver below * max record size (2^14 + 2048 + 13) */ optlen = (socklen_t) sizeof(int); ret = getsockopt(b->num, IPPROTO_SCTP, SCTP_PARTIAL_DELIVERY_POINT, &optval, &optlen); OPENSSL_assert(ret >= 0); OPENSSL_assert(optval >= 18445); /* Partially delivered notification??? Probably a bug.... */ OPENSSL_assert(!(msg.msg_flags & MSG_NOTIFICATION)); /* Everything seems ok till now, so it's most likely * a message dropped by PR-SCTP. */ memset(out, 0, outl); BIO_set_retry_read(b); return -1; } BIO_clear_retry_flags(b); if (ret < 0) { if (BIO_dgram_should_retry(ret)) { BIO_set_retry_read(b); data->_errno = get_last_socket_error(); } } /* Test if peer uses SCTP-AUTH before continuing */ if (!data->peer_auth_tested) { int ii, auth_data = 0, auth_forward = 0; unsigned char *p; struct sctp_authchunks *authchunks; optlen = (socklen_t)(sizeof(sctp_assoc_t) + 256 * sizeof(uint8_t)); authchunks = OPENSSL_malloc(optlen); memset(authchunks, 0, sizeof(optlen)); ii = getsockopt(b->num, IPPROTO_SCTP, SCTP_PEER_AUTH_CHUNKS, authchunks, &optlen); OPENSSL_assert(ii >= 0); for (p = (unsigned char*) authchunks + sizeof(sctp_assoc_t); p < (unsigned char*) authchunks + optlen; p += sizeof(uint8_t)) { if (*p == OPENSSL_SCTP_DATA_CHUNK_TYPE) auth_data = 1; if (*p == OPENSSL_SCTP_FORWARD_CUM_TSN_CHUNK_TYPE) auth_forward = 1; } OPENSSL_free(authchunks); if (!auth_data || !auth_forward) { BIOerr(BIO_F_DGRAM_SCTP_READ,BIO_R_CONNECT_ERROR); return -1; } data->peer_auth_tested = 1; } } return(ret); }
/* NB: this will never set port# in 'to'! * _Only_ IP/IPv6 address part of 'to' is _maybe_ modified. * Typical usage is to preinit 'to' with "default" value * before calling recv_from_to(). */ ssize_t FAST_FUNC recv_from_to(int fd, void *buf, size_t len, int flags, struct sockaddr *from, struct sockaddr *to, socklen_t sa_size) { #ifndef IP_PKTINFO return recvfrom(fd, buf, len, flags, from, &sa_size); #else /* man recvmsg and man cmsg is needed to make sense of code below */ struct iovec iov[1]; union { char cmsg[CMSG_SPACE(sizeof(struct in_pktinfo))]; #if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO) char cmsg6[CMSG_SPACE(sizeof(struct in6_pktinfo))]; #endif } u; struct cmsghdr *cmsgptr; struct msghdr msg; ssize_t recv_length; iov[0].iov_base = buf; iov[0].iov_len = len; memset(&msg, 0, sizeof(msg)); msg.msg_name = (struct sockaddr *)from; msg.msg_namelen = sa_size; msg.msg_iov = iov; msg.msg_iovlen = 1; msg.msg_control = &u; msg.msg_controllen = sizeof(u); recv_length = recvmsg(fd, &msg, flags); if (recv_length < 0) return recv_length; /* Here we try to retrieve destination IP and memorize it */ for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr != NULL; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr) ) { if (cmsgptr->cmsg_level == IPPROTO_IP && cmsgptr->cmsg_type == IP_PKTINFO ) { #define pktinfo(cmsgptr) ( (struct in_pktinfo*)(CMSG_DATA(cmsgptr)) ) to->sa_family = AF_INET; ((struct sockaddr_in*)to)->sin_addr = pktinfo(cmsgptr)->ipi_addr; /* ((struct sockaddr_in*)to)->sin_port = 123; */ #undef pktinfo break; } #if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO) if (cmsgptr->cmsg_level == IPPROTO_IPV6 && cmsgptr->cmsg_type == IPV6_PKTINFO ) { #define pktinfo(cmsgptr) ( (struct in6_pktinfo*)(CMSG_DATA(cmsgptr)) ) to->sa_family = AF_INET6; ((struct sockaddr_in6*)to)->sin6_addr = pktinfo(cmsgptr)->ipi6_addr; /* ((struct sockaddr_in6*)to)->sin6_port = 123; */ #undef pktinfo break; } #endif } return recv_length; #endif }
static PyObject *sendmsg_sendmsg(PyObject *self, PyObject *args, PyObject *keywds) { int fd; int flags = 0; Py_ssize_t sendmsg_result, iovec_length; struct msghdr message_header; struct iovec iov[1]; PyObject *ancillary = NULL; PyObject *iterator = NULL; PyObject *item = NULL; PyObject *result_object = NULL; static char *kwlist[] = {"fd", "data", "flags", "ancillary", NULL}; if (!PyArg_ParseTupleAndKeywords( args, keywds, "it#|iO:sendmsg", kwlist, &fd, &iov[0].iov_base, &iovec_length, &flags, &ancillary)) { return NULL; } iov[0].iov_len = iovec_length; message_header.msg_name = NULL; message_header.msg_namelen = 0; message_header.msg_iov = iov; message_header.msg_iovlen = 1; message_header.msg_control = NULL; message_header.msg_controllen = 0; message_header.msg_flags = 0; if (ancillary) { if (!PyList_Check(ancillary)) { PyErr_Format(PyExc_TypeError, "send1msg argument 3 expected list, got %s", ancillary->ob_type->tp_name); goto finished; } iterator = PyObject_GetIter(ancillary); if (iterator == NULL) { goto finished; } size_t all_data_len = 0; /* First we need to know how big the buffer needs to be in order to have enough space for all of the messages. */ while ( (item = PyIter_Next(iterator)) ) { int type, level; Py_ssize_t data_len; size_t prev_all_data_len; char *data; if (!PyTuple_Check(item)) { PyErr_Format(PyExc_TypeError, "send1msg argument 3 expected list of tuple, " "got list containing %s", item->ob_type->tp_name); goto finished; } if (!PyArg_ParseTuple( item, "iit#:sendmsg ancillary data (level, type, data)", &level, &type, &data, &data_len)) { goto finished; } prev_all_data_len = all_data_len; all_data_len += CMSG_SPACE(data_len); Py_DECREF(item); item = NULL; if (all_data_len < prev_all_data_len) { PyErr_Format(PyExc_OverflowError, "Too much msg_control to fit in a size_t: %zu", prev_all_data_len); goto finished; } } Py_DECREF(iterator); iterator = NULL; /* Allocate the buffer for all of the ancillary elements, if we have * any. */ if (all_data_len) { if (all_data_len > SOCKLEN_MAX) { PyErr_Format(PyExc_OverflowError, "Too much msg_control to fit in a socklen_t: %zu", all_data_len); goto finished; } message_header.msg_control = PyMem_Malloc(all_data_len); if (!message_header.msg_control) { PyErr_NoMemory(); goto finished; } } else { message_header.msg_control = NULL; } message_header.msg_controllen = (socklen_t) all_data_len; iterator = PyObject_GetIter(ancillary); /* again */ if (!iterator) { goto finished; } /* Unpack the tuples into the control message. */ struct cmsghdr *control_message = CMSG_FIRSTHDR(&message_header); while ( (item = PyIter_Next(iterator)) ) { int type, level; Py_ssize_t data_len; size_t data_size; unsigned char *data, *cmsg_data; /* We explicitly allocated enough space for all ancillary data above; if there isn't enough room, all bets are off. */ assert(control_message); if (!PyArg_ParseTuple(item, "iit#:sendmsg ancillary data (level, type, data)", &level, &type, &data, &data_len)) { goto finished; } control_message->cmsg_level = level; control_message->cmsg_type = type; data_size = CMSG_LEN(data_len); if (data_size > SOCKLEN_MAX) { PyErr_Format(PyExc_OverflowError, "CMSG_LEN(%zd) > SOCKLEN_MAX", data_len); goto finished; } control_message->cmsg_len = (socklen_t) data_size; cmsg_data = CMSG_DATA(control_message); memcpy(cmsg_data, data, data_len); Py_DECREF(item); item = NULL; control_message = CMSG_NXTHDR(&message_header, control_message); } Py_DECREF(iterator); iterator = NULL; if (PyErr_Occurred()) { goto finished; } } sendmsg_result = sendmsg(fd, &message_header, flags); if (sendmsg_result < 0) { PyErr_SetFromErrno(sendmsg_socket_error); goto finished; } result_object = Py_BuildValue("n", sendmsg_result); finished: if (item) { Py_DECREF(item); item = NULL; } if (iterator) { Py_DECREF(iterator); iterator = NULL; } if (message_header.msg_control) { PyMem_Free(message_header.msg_control); message_header.msg_control = NULL; } return result_object; }
int myrecvfrom (int s, void *buf, int len, unsigned int flags, struct sockaddr *from, int *fromlen, struct sockaddr_in *myaddr) { struct msghdr msg; struct iovec iov[1]; int n; struct cmsghdr *cmptr; union { struct cmsghdr cm; #ifdef IP_PKTINFO char control[CMSG_SPACE (sizeof (struct in_addr)) + CMSG_SPACE (sizeof (struct in_pktinfo))]; #else char control[CMSG_SPACE (sizeof (struct in_addr))]; #endif } control_un; int on = 1; #ifdef IP_PKTINFO struct in_pktinfo pktinfo; #endif /* Try to enable getting the return address */ #ifdef IP_RECVDSTADDR setsockopt (s, IPPROTO_IP, IP_RECVDSTADDR, &on, sizeof (on)); #endif #ifdef IP_PKTINFO setsockopt (s, IPPROTO_IP, IP_PKTINFO, &on, sizeof (on)); #endif msg.msg_control = control_un.control; msg.msg_controllen = sizeof (control_un.control); msg.msg_flags = 0; msg.msg_name = from; msg.msg_namelen = *fromlen; iov[0].iov_base = buf; iov[0].iov_len = len; msg.msg_iov = iov; msg.msg_iovlen = 1; if ((n = recvmsg (s, &msg, flags)) < 0) return n; /* Error */ *fromlen = msg.msg_namelen; if (myaddr) { bzero (myaddr, sizeof (struct sockaddr_in)); myaddr->sin_family = AF_INET; if (msg.msg_controllen < sizeof (struct cmsghdr) || (msg.msg_flags & MSG_CTRUNC)) return n; /* No information available */ for (cmptr = CMSG_FIRSTHDR (&msg); cmptr != NULL; cmptr = CMSG_NXTHDR (&msg, cmptr)) { #ifdef IP_RECVSTDADDR if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_RECVDSTADDR) { memcpy (&myaddr->sin_addr, CMSG_DATA (cmptr), sizeof (struct in_addr)); } #endif #ifdef IP_PKTINFO if (cmptr->cmsg_level == IPPROTO_IP && cmptr->cmsg_type == IP_PKTINFO) { memcpy (&pktinfo, CMSG_DATA (cmptr), sizeof (struct in_pktinfo)); memcpy (&myaddr->sin_addr, &pktinfo.ipi_addr, sizeof (struct in_addr)); } #endif } } return n; }