improved load balancing algorithm.
[l2tpns.git] / 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, controlfd, daefd, snoopfd, ifrfd, ifr6fd, rand_fd;
56 extern int pppoediscfd, pppoesessfd;
57 extern int *radfds;
58 extern int udpfd[MAX_UDPFD + 1];
59
60 pid_t fork_and_close()
61 {
62 pid_t pid = fork();
63 int i;
64
65 if (pid)
66 return pid;
67
68 forked++;
69 if (config->scheduler_fifo)
70 {
71 struct sched_param params = {0};
72 params.sched_priority = 0;
73 if (sched_setscheduler(0, SCHED_OTHER, &params))
74 {
75 LOG(0, 0, 0, "Error setting scheduler to OTHER after fork: %s\n", strerror(errno));
76 LOG(0, 0, 0, "This is probably really really bad.\n");
77 }
78 }
79
80 signal(SIGPIPE, SIG_DFL);
81 signal(SIGCHLD, SIG_DFL);
82 signal(SIGHUP, SIG_DFL);
83 signal(SIGUSR1, SIG_DFL);
84 signal(SIGQUIT, SIG_DFL);
85 signal(SIGKILL, SIG_DFL);
86 signal(SIGTERM, SIG_DFL);
87
88 // Close sockets
89 if (clifd != -1) close(clifd);
90 if (cluster_sockfd != -1) close(cluster_sockfd);
91 if (tunfd != -1) close(tunfd);
92
93 for (i = 0; i < config->nbudpfd; i++)
94 {
95 if (udpfd[i] != -1) close(udpfd[i]);
96 }
97
98 if (pppoediscfd != -1) close(pppoediscfd);
99 if (pppoediscfd != -1) close(pppoediscfd);
100 if (controlfd != -1) close(controlfd);
101 if (daefd != -1) close(daefd);
102 if (snoopfd != -1) close(snoopfd);
103 if (rand_fd != -1) close(rand_fd);
104 if (epollfd != -1) close(epollfd);
105
106 for (i = 0; radfds && i < RADIUS_FDS; i++)
107 close(radfds[i]);
108
109 #ifdef BGP
110 for (i = 0; i < BGP_NUM_PEERS; i++)
111 if (bgp_peers[i].sock != -1)
112 close(bgp_peers[i].sock);
113 #endif /* BGP */
114
115 return pid;
116 }
117
118 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
119 struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr)
120 {
121 ssize_t r;
122 struct msghdr msg;
123 struct cmsghdr *cmsg;
124 struct iovec vec;
125 char cbuf[128];
126
127 memset(&msg, 0, sizeof(msg));
128 msg.msg_name = from;
129 msg.msg_namelen = *fromlen;
130
131 vec.iov_base = buf;
132 vec.iov_len = len;
133 msg.msg_iov = &vec;
134 msg.msg_iovlen = 1;
135 msg.msg_flags = 0;
136
137 msg.msg_control = cbuf;
138 msg.msg_controllen = sizeof(cbuf);
139
140 if ((r = recvmsg(s, &msg, flags)) < 0)
141 return r;
142
143 if (fromlen)
144 *fromlen = msg.msg_namelen;
145
146 memset(toaddr, 0, sizeof(*toaddr));
147 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
148 {
149 if (cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_PKTINFO)
150 {
151 struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
152 memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
153 break;
154 }
155 }
156
157 return r;
158 }
159
160 ssize_t sendtofrom(int s, void const *buf, size_t len, int flags,
161 struct sockaddr const *to, socklen_t tolen, struct in_addr const *from)
162 {
163 struct msghdr msg;
164 struct cmsghdr *cmsg;
165 struct iovec vec;
166 struct in_pktinfo pktinfo;
167 char cbuf[CMSG_SPACE(sizeof(pktinfo))];
168
169 memset(&msg, 0, sizeof(msg));
170 msg.msg_name = (struct sockaddr *) to;
171 msg.msg_namelen = tolen;
172
173 vec.iov_base = (void *) buf;
174 vec.iov_len = len;
175 msg.msg_iov = &vec;
176 msg.msg_iovlen = 1;
177 msg.msg_flags = 0;
178
179 msg.msg_control = cbuf;
180 msg.msg_controllen = sizeof(cbuf);
181
182 cmsg = CMSG_FIRSTHDR(&msg);
183 cmsg->cmsg_level = SOL_IP;
184 cmsg->cmsg_type = IP_PKTINFO;
185 cmsg->cmsg_len = CMSG_LEN(sizeof(pktinfo));
186
187 memset(&pktinfo, 0, sizeof(pktinfo));
188 memcpy(&pktinfo.ipi_spec_dst, from, sizeof(*from));
189 memcpy(CMSG_DATA(cmsg), &pktinfo, sizeof(pktinfo));
190
191 return sendmsg(s, &msg, flags);
192 }