git.maemo.org Git - busybox4maemo/blob - libbb/udp_io.c

   1 /* vi: set sw=4 ts=4: */
   2 /*
   3  * Utility routines.
   4  *
   5  * Copyright (C) 2007 Denys Vlasenko
   6  *
   7  * Licensed under GPL version 2, see file LICENSE in this tarball for details.
   8  */
   9
  10 #include "libbb.h"
  11
  12 /*
  13  * This asks kernel to let us know dst addr/port of incoming packets
  14  * We don't check for errors here. Not supported == won't be used
  15  */
  16 void
  17 socket_want_pktinfo(int fd)
  18 {
  19 #ifdef IP_PKTINFO
  20         setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &const_int_1, sizeof(int));
  21 #endif
  22 #if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
  23         setsockopt(fd, IPPROTO_IPV6, IPV6_PKTINFO, &const_int_1, sizeof(int));
  24 #endif
  25 }
  26
  27
  28 ssize_t
  29 send_to_from(int fd, void *buf, size_t len, int flags,
  30                 const struct sockaddr *to,
  31                 const struct sockaddr *from,
  32                 socklen_t tolen)
  33 {
  34 #ifndef IP_PKTINFO
  35         return sendto(fd, buf, len, flags, to, tolen);
  36 #else
  37         struct iovec iov[1];
  38         struct msghdr msg;
  39         char cbuf[sizeof(struct in_pktinfo)
  40 #if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
  41                 | sizeof(struct in6_pktinfo) /* (a|b) is poor man's max(a,b) */
  42 #endif
  43         ];
  44         struct cmsghdr* cmsgptr;
  45
  46         if (from->sa_family != AF_INET
  47 #if ENABLE_FEATURE_IPV6
  48          && from->sa_family != AF_INET6
  49 #endif
  50         ) {
  51                 /* ANY local address */
  52                 return sendto(fd, buf, len, flags, to, tolen);
  53         }
  54
  55         /* man recvmsg and man cmsg is needed to make sense of code below */
  56
  57         iov[0].iov_base = buf;
  58         iov[0].iov_len = len;
  59
  60         memset(cbuf, 0, sizeof(cbuf));
  61
  62         memset(&msg, 0, sizeof(msg));
  63         msg.msg_name = (void *)(struct sockaddr *)to; /* or compiler will annoy us */
  64         msg.msg_namelen = tolen;
  65         msg.msg_iov = iov;
  66         msg.msg_iovlen = 1;
  67         msg.msg_control = cbuf;
  68         msg.msg_controllen = sizeof(cbuf);
  69         msg.msg_flags = flags;
  70
  71         cmsgptr = CMSG_FIRSTHDR(&msg);
  72         if (to->sa_family == AF_INET && from->sa_family == AF_INET) {
  73                 struct in_pktinfo *pktptr;
  74                 cmsgptr->cmsg_level = IPPROTO_IP;
  75                 cmsgptr->cmsg_type = IP_PKTINFO;
  76                 cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
  77                 pktptr = (struct in_pktinfo *)(CMSG_DATA(cmsgptr));
  78                 /* pktptr->ipi_ifindex = 0; -- already done by memset(cbuf...) */
  79                 pktptr->ipi_spec_dst = ((struct sockaddr_in*)from)->sin_addr;
  80         }
  81 #if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
  82         else if (to->sa_family == AF_INET6 && from->sa_family == AF_INET6) {
  83                 struct in6_pktinfo *pktptr;
  84                 cmsgptr->cmsg_level = IPPROTO_IPV6;
  85                 cmsgptr->cmsg_type = IPV6_PKTINFO;
  86                 cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
  87                 pktptr = (struct in6_pktinfo *)(CMSG_DATA(cmsgptr));
  88                 /* pktptr->ipi6_ifindex = 0; -- already done by memset(cbuf...) */
  89                 pktptr->ipi6_addr = ((struct sockaddr_in6*)from)->sin6_addr;
  90         }
  91 #endif
  92         return sendmsg(fd, &msg, flags);
  93 #endif
  94 }
  95
  96 /* NB: this will never set port# in 'to'!
  97  * _Only_ IP/IPv6 address part of 'to' is _maybe_ modified.
  98  * Typical usage is to preinit 'to' with "default" value
  99  * before calling recv_from_to(). */
 100 ssize_t
 101 recv_from_to(int fd, void *buf, size_t len, int flags,
 102                 struct sockaddr *from, struct sockaddr *to,
 103                 socklen_t sa_size)
 104 {
 105 #ifndef IP_PKTINFO
 106         return recvfrom(fd, buf, len, flags, from, &sa_size);
 107 #else
 108         /* man recvmsg and man cmsg is needed to make sense of code below */
 109         struct iovec iov[1];
 110         union {
 111                 char cmsg[CMSG_SPACE(sizeof(struct in_pktinfo))];
 112                 char cmsg6[CMSG_SPACE(sizeof(struct in6_pktinfo))];
 113         } u;
 114         struct cmsghdr *cmsgptr;
 115         struct msghdr msg;
 116         socklen_t recv_length;
 117
 118         iov[0].iov_base = buf;
 119         iov[0].iov_len = len;
 120
 121         memset(&msg, 0, sizeof(msg));
 122         msg.msg_name = (struct sockaddr *)from;
 123         msg.msg_namelen = sa_size;
 124         msg.msg_iov = iov;
 125         msg.msg_iovlen = 1;
 126         msg.msg_control = &u;
 127         msg.msg_controllen = sizeof(u);
 128
 129         recv_length = recvmsg(fd, &msg, flags);
 130         if (recv_length < 0)
 131                 return recv_length;
 132
 133         /* Here we try to retrieve destination IP and memorize it */
 134         for (cmsgptr = CMSG_FIRSTHDR(&msg);
 135                         cmsgptr != NULL;
 136                         cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)
 137         ) {
 138                 if (cmsgptr->cmsg_level == IPPROTO_IP
 139                  && cmsgptr->cmsg_type == IP_PKTINFO
 140                 ) {
 141 #define pktinfo(cmsgptr) ( (struct in_pktinfo*)(CMSG_DATA(cmsgptr)) )
 142                         to->sa_family = AF_INET;
 143                         ((struct sockaddr_in*)to)->sin_addr = pktinfo(cmsgptr)->ipi_addr;
 144                         /* ((struct sockaddr_in*)to)->sin_port = 123; */
 145 #undef pktinfo
 146                         break;
 147                 }
 148 #if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
 149                 if (cmsgptr->cmsg_level == IPPROTO_IPV6
 150                  && cmsgptr->cmsg_type == IPV6_PKTINFO
 151                 ) {
 152 #define pktinfo(cmsgptr) ( (struct in6_pktinfo*)(CMSG_DATA(cmsgptr)) )
 153                         to->sa_family = AF_INET6;
 154                         ((struct sockaddr_in6*)to)->sin6_addr = pktinfo(cmsgptr)->ipi6_addr;
 155                         /* ((struct sockaddr_in6*)to)->sin6_port = 123; */
 156 #undef pktinfo
 157                         break;
 158                 }
 159 #endif
 160         }
 161         return recv_length;
 162 #endif
 163 }