util.c

   1 /* Misc util functions */
   2
   3 #include <unistd.h>
   4 #include <errno.h>
   5 #include <sched.h>
   6 #include <sys/socket.h>
   7 #include <netinet/in.h>
   8 #include <arpa/inet.h>
   9 #include <string.h>
  10 #include <sys/mman.h>
  11 #include <linux/rtnetlink.h>
  12
  13 #include "dhcp6.h"
  14 #include "l2tpns.h"
  15 #ifdef BGP
  16 #include "bgp.h"
  17 #endif
  18
  19 // format ipv4 addr as a dotted-quad; n chooses one of 4 static buffers
  20 // to use
  21 char *fmtaddr(in_addr_t addr, int n)
  22 {
  23     static char addrs[4][16];
  24     struct in_addr in;
  25
  26     if (n < 0 || n >= 4)
  27         return "";
  28
  29     in.s_addr = addr;
  30     return strcpy(addrs[n], inet_ntoa(in));
  31 }
  32
  33 char *fmtMacAddr(uint8_t *pMacAddr)
  34 {
  35         static char strMAC[2*ETH_ALEN];
  36
  37         sprintf(strMAC, "%02X:%02X:%02X:%02X:%02X:%02X",
  38                         pMacAddr[0], pMacAddr[1], pMacAddr[2],
  39                         pMacAddr[3], pMacAddr[4], pMacAddr[5]);
  40
  41   return strMAC;
  42 }
  43
  44 void *shared_malloc(unsigned int size)
  45 {
  46     void * p;
  47     p = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS, 0, 0);
  48
  49     if (p == MAP_FAILED)
  50         p = NULL;
  51
  52     return p;
  53 }
  54
  55 extern int forked;
  56 extern int cluster_sockfd, tunfd, controlfd, daefd, snoopfd, ifrfd, ifr6fd, rand_fd;
  57 extern int pppoediscfd, pppoesessfd;
  58 extern int *radfds;
  59 extern int udpfd[MAX_UDPFD + 1];
  60
  61 pid_t fork_and_close()
  62 {
  63         pid_t pid = fork();
  64         int i;
  65
  66         if (pid)
  67                 return pid;
  68
  69         forked++;
  70         if (config->scheduler_fifo)
  71         {
  72                 struct sched_param params = {0};
  73                 params.sched_priority = 0;
  74                 if (sched_setscheduler(0, SCHED_OTHER, &params))
  75                 {
  76                         LOG(0, 0, 0, "Error setting scheduler to OTHER after fork: %s\n", strerror(errno));
  77                         LOG(0, 0, 0, "This is probably really really bad.\n");
  78                 }
  79         }
  80
  81         signal(SIGPIPE, SIG_DFL);
  82         signal(SIGCHLD, SIG_DFL);
  83         signal(SIGHUP,  SIG_DFL);
  84         signal(SIGUSR1, SIG_DFL);
  85         signal(SIGQUIT, SIG_DFL);
  86         signal(SIGKILL, SIG_DFL);
  87         signal(SIGTERM, SIG_DFL);
  88
  89         // Close sockets
  90         if (clifd != -1)        close(clifd);
  91         if (cluster_sockfd != -1)       close(cluster_sockfd);
  92         if (tunfd != -1)        close(tunfd);
  93
  94         for (i = 0; i < config->nbudpfd; i++)
  95         {
  96                 if (udpfd[i] != -1)     close(udpfd[i]);
  97         }
  98
  99         if (pppoediscfd != -1)  close(pppoediscfd);
 100         if (pppoediscfd != -1)  close(pppoediscfd);
 101         if (controlfd != -1)    close(controlfd);
 102         if (daefd != -1)        close(daefd);
 103         if (snoopfd != -1)      close(snoopfd);
 104         if (rand_fd != -1)      close(rand_fd);
 105         if (epollfd != -1)      close(epollfd);
 106
 107         for (i = 0; radfds && i < RADIUS_FDS; i++)
 108                 close(radfds[i]);
 109
 110 #ifdef BGP
 111         for (i = 0; i < BGP_NUM_PEERS; i++)
 112                 if (bgp_peers[i].sock != -1)
 113                         close(bgp_peers[i].sock);
 114 #endif /* BGP */
 115
 116     return pid;
 117 }
 118
 119 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
 120         struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr)
 121 {
 122         ssize_t r;
 123         struct msghdr msg;
 124         struct cmsghdr *cmsg;
 125         struct iovec vec;
 126         char cbuf[128];
 127
 128         memset(&msg, 0, sizeof(msg));
 129         msg.msg_name = from;
 130         msg.msg_namelen = *fromlen;
 131
 132         vec.iov_base = buf;
 133         vec.iov_len = len;
 134         msg.msg_iov = &vec;
 135         msg.msg_iovlen = 1;
 136         msg.msg_flags = 0;
 137
 138         msg.msg_control = cbuf;
 139         msg.msg_controllen = sizeof(cbuf);
 140
 141         if ((r = recvmsg(s, &msg, flags)) < 0)
 142                 return r;
 143
 144         if (fromlen)
 145                 *fromlen = msg.msg_namelen;
 146
 147         memset(toaddr, 0, sizeof(*toaddr));
 148         for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
 149         {
 150                 if (cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_PKTINFO)
 151                 {
 152                         struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
 153                         memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
 154                         break;
 155                 }
 156         }
 157
 158         return r;
 159 }
 160
 161 ssize_t sendtofrom(int s, void const *buf, size_t len, int flags,
 162     struct sockaddr const *to, socklen_t tolen, struct in_addr const *from)
 163 {
 164     struct msghdr msg;
 165     struct cmsghdr *cmsg;
 166     struct iovec vec;
 167     struct in_pktinfo pktinfo;
 168     char cbuf[CMSG_SPACE(sizeof(pktinfo))];
 169
 170     memset(&msg, 0, sizeof(msg));
 171     msg.msg_name = (struct sockaddr *) to;
 172     msg.msg_namelen = tolen;
 173
 174     vec.iov_base = (void *) buf;
 175     vec.iov_len = len;
 176     msg.msg_iov = &vec;
 177     msg.msg_iovlen = 1;
 178     msg.msg_flags = 0;
 179
 180     msg.msg_control = cbuf;
 181     msg.msg_controllen = sizeof(cbuf);
 182
 183     cmsg = CMSG_FIRSTHDR(&msg);
 184     cmsg->cmsg_level = SOL_IP;
 185     cmsg->cmsg_type = IP_PKTINFO;
 186     cmsg->cmsg_len = CMSG_LEN(sizeof(pktinfo));
 187
 188     memset(&pktinfo, 0, sizeof(pktinfo));
 189     memcpy(&pktinfo.ipi_spec_dst, from, sizeof(*from));
 190     memcpy(CMSG_DATA(cmsg), &pktinfo, sizeof(pktinfo));
 191
 192     return sendmsg(s, &msg, flags);
 193 }