release/src/router/busybox/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 GPLv2, see file LICENSE in this source tree.
   8  */
   9 #include "libbb.h"
  10
  11 /*
  12  * This asks kernel to let us know dst addr/port of incoming packets
  13  * We don't check for errors here. Not supported == won't be used
  14  */
  15 void FAST_FUNC
  16 socket_want_pktinfo(int fd UNUSED_PARAM)
  17 {
  18 #ifdef IP_PKTINFO
  19         setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &const_int_1, sizeof(int));
  20 #endif
  21 #if ENABLE_FEATURE_IPV6
  22 # ifdef IPV6_RECVPKTINFO
  23         setsockopt(fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &const_int_1, sizeof(int));
  24         setsockopt(fd, IPPROTO_IPV6, IPV6_2292PKTINFO, &const_int_1, sizeof(int));
  25 # elif defined(IPV6_PKTINFO)
  26         setsockopt(fd, IPPROTO_IPV6, IPV6_PKTINFO, &const_int_1, sizeof(int));
  27 # endif
  28 #endif
  29 }
  30
  31
  32 ssize_t FAST_FUNC
  33 send_to_from(int fd, void *buf, size_t len, int flags,
  34                 const struct sockaddr *to,
  35                 const struct sockaddr *from,
  36                 socklen_t tolen)
  37 {
  38 #ifndef IP_PKTINFO
  39         (void)from; /* suppress "unused from" warning */
  40         return sendto(fd, buf, len, flags, to, tolen);
  41 #else
  42         struct iovec iov[1];
  43         struct msghdr msg;
  44         union {
  45                 char cmsg[CMSG_SPACE(sizeof(struct in_pktinfo))];
  46 # if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
  47                 char cmsg6[CMSG_SPACE(sizeof(struct in6_pktinfo))];
  48 # endif
  49         } u;
  50         struct cmsghdr* cmsgptr;
  51
  52         if (from->sa_family != AF_INET
  53 # if ENABLE_FEATURE_IPV6
  54          && from->sa_family != AF_INET6
  55 # endif
  56         ) {
  57                 /* ANY local address */
  58                 return sendto(fd, buf, len, flags, to, tolen);
  59         }
  60
  61         /* man recvmsg and man cmsg is needed to make sense of code below */
  62
  63         iov[0].iov_base = buf;
  64         iov[0].iov_len = len;
  65
  66         memset(&u, 0, sizeof(u));
  67
  68         memset(&msg, 0, sizeof(msg));
  69         msg.msg_name = (void *)(struct sockaddr *)to; /* or compiler will annoy us */
  70         msg.msg_namelen = tolen;
  71         msg.msg_iov = iov;
  72         msg.msg_iovlen = 1;
  73         msg.msg_control = &u;
  74         msg.msg_controllen = sizeof(u);
  75         msg.msg_flags = flags;
  76
  77         cmsgptr = CMSG_FIRSTHDR(&msg);
  78         if (to->sa_family == AF_INET && from->sa_family == AF_INET) {
  79                 struct in_pktinfo *pktptr;
  80                 cmsgptr->cmsg_level = IPPROTO_IP;
  81                 cmsgptr->cmsg_type = IP_PKTINFO;
  82                 cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
  83                 pktptr = (struct in_pktinfo *)(CMSG_DATA(cmsgptr));
  84                 /*pktptr->ipi_ifindex = 0; -- already done by memset(u...) */
  85                 /* In general, CMSG_DATA() can be unaligned, but in this case
  86                  * we know for sure it is sufficiently aligned:
  87                  * CMSG_FIRSTHDR simply returns &u above,
  88                  * and CMSG_DATA returns &u + size_t + int + int.
  89                  * Thus direct assignment is ok:
  90                  */
  91                 pktptr->ipi_spec_dst = ((struct sockaddr_in*)from)->sin_addr;
  92         }
  93 # if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
  94         else if (to->sa_family == AF_INET6 && from->sa_family == AF_INET6) {
  95                 struct in6_pktinfo *pktptr;
  96                 cmsgptr->cmsg_level = IPPROTO_IPV6;
  97                 cmsgptr->cmsg_type = IPV6_PKTINFO;
  98                 cmsgptr->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
  99                 pktptr = (struct in6_pktinfo *)(CMSG_DATA(cmsgptr));
 100                 /* pktptr->ipi6_ifindex = 0; -- already done by memset(u...) */
 101                 pktptr->ipi6_addr = ((struct sockaddr_in6*)from)->sin6_addr;
 102         }
 103 # endif
 104         msg.msg_controllen = cmsgptr->cmsg_len;
 105
 106         return sendmsg(fd, &msg, flags);
 107 #endif
 108 }
 109
 110 /* NB: this will never set port# in 'to'!
 111  * _Only_ IP/IPv6 address part of 'to' is _maybe_ modified.
 112  * Typical usage is to preinit 'to' with "default" value
 113  * before calling recv_from_to(). */
 114 ssize_t FAST_FUNC
 115 recv_from_to(int fd, void *buf, size_t len, int flags,
 116                 struct sockaddr *from, struct sockaddr *to,
 117                 socklen_t sa_size)
 118 {
 119 #ifndef IP_PKTINFO
 120         (void)to; /* suppress "unused to" warning */
 121         return recvfrom(fd, buf, len, flags, from, &sa_size);
 122 #else
 123         /* man recvmsg and man cmsg is needed to make sense of code below */
 124         struct iovec iov[1];
 125         union {
 126                 char cmsg[CMSG_SPACE(sizeof(struct in_pktinfo))];
 127 # if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
 128                 char cmsg6[CMSG_SPACE(sizeof(struct in6_pktinfo))];
 129 # endif
 130         } u;
 131         struct cmsghdr *cmsgptr;
 132         struct msghdr msg;
 133         ssize_t recv_length;
 134
 135         iov[0].iov_base = buf;
 136         iov[0].iov_len = len;
 137
 138         memset(&msg, 0, sizeof(msg));
 139         msg.msg_name = (struct sockaddr *)from;
 140         msg.msg_namelen = sa_size;
 141         msg.msg_iov = iov;
 142         msg.msg_iovlen = 1;
 143         msg.msg_control = &u;
 144         msg.msg_controllen = sizeof(u);
 145
 146         recv_length = recvmsg(fd, &msg, flags);
 147         if (recv_length < 0)
 148                 return recv_length;
 149
 150 # define to4 ((struct sockaddr_in*)to)
 151 # define to6 ((struct sockaddr_in6*)to)
 152         /* Here we try to retrieve destination IP and memorize it */
 153         for (cmsgptr = CMSG_FIRSTHDR(&msg);
 154                         cmsgptr != NULL;
 155                         cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)
 156         ) {
 157                 if (cmsgptr->cmsg_level == IPPROTO_IP
 158                  && cmsgptr->cmsg_type == IP_PKTINFO
 159                 ) {
 160                         const int IPI_ADDR_OFF = offsetof(struct in_pktinfo, ipi_addr);
 161                         to->sa_family = AF_INET;
 162                         /*# define pktinfo(cmsgptr) ( (struct in_pktinfo*)(CMSG_DATA(cmsgptr)) )*/
 163                         /*to4->sin_addr = pktinfo(cmsgptr)->ipi_addr; - may be unaligned */
 164                         memcpy(&to4->sin_addr, (char*)(CMSG_DATA(cmsgptr)) + IPI_ADDR_OFF, sizeof(to4->sin_addr));
 165                         /*to4->sin_port = 123; - this data is not supplied by kernel */
 166                         break;
 167                 }
 168 # if ENABLE_FEATURE_IPV6 && defined(IPV6_PKTINFO)
 169                 if (cmsgptr->cmsg_level == IPPROTO_IPV6
 170                  && (cmsgptr->cmsg_type == IPV6_PKTINFO
 171 #if defined(IPV6_2292PKTINFO) && defined(IPV6_RECVPKTINFO)
 172                          || cmsgptr->cmsg_type == IPV6_2292PKTINFO
 173 #endif
 174                 )) {
 175                         const int IPI6_ADDR_OFF = offsetof(struct in6_pktinfo, ipi6_addr);
 176                         to->sa_family = AF_INET6;
 177                         /*#  define pktinfo(cmsgptr) ( (struct in6_pktinfo*)(CMSG_DATA(cmsgptr)) )*/
 178                         /*to6->sin6_addr = pktinfo(cmsgptr)->ipi6_addr; - may be unaligned */
 179                         memcpy(&to6->sin6_addr, (char*)(CMSG_DATA(cmsgptr)) + IPI6_ADDR_OFF, sizeof(to6->sin6_addr));
 180                         /*to6->sin6_port = 123; */
 181                         break;
 182                 }
 183 # endif
 184         }
 185         return recv_length;
 186 #endif
 187 }