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