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