send nsctl responses back using the correct source address
[l2tpns.git] / util.c
1 /* Misc util functions */
2
3 char const *cvs_id_util = "$Id: util.c,v 1.14 2006-04-05 01:45:57 bodea Exp $";
4
5 #include <unistd.h>
6 #include <errno.h>
7 #include <sched.h>
8 #include <sys/socket.h>
9 #include <netinet/in.h>
10 #include <arpa/inet.h>
11 #include <string.h>
12 #include <sys/mman.h>
13
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 void *shared_malloc(unsigned int size)
34 {
35 void * p;
36 p = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS, 0, 0);
37
38 if (p == MAP_FAILED)
39 p = NULL;
40
41 return p;
42 }
43
44 extern int forked;
45 extern int cluster_sockfd, tunfd, udpfd, controlfd, daefd, snoopfd, ifrfd, ifr6fd, rand_fd;
46 extern int *radfds;
47
48 pid_t fork_and_close()
49 {
50 pid_t pid = fork();
51 int i;
52
53 if (pid)
54 return pid;
55
56 forked++;
57 if (config->scheduler_fifo)
58 {
59 struct sched_param params = {0};
60 params.sched_priority = 0;
61 if (sched_setscheduler(0, SCHED_OTHER, &params))
62 {
63 LOG(0, 0, 0, "Error setting scheduler to OTHER after fork: %s\n", strerror(errno));
64 LOG(0, 0, 0, "This is probably really really bad.\n");
65 }
66 }
67
68 signal(SIGPIPE, SIG_DFL);
69 signal(SIGCHLD, SIG_DFL);
70 signal(SIGHUP, SIG_DFL);
71 signal(SIGUSR1, SIG_DFL);
72 signal(SIGQUIT, SIG_DFL);
73 signal(SIGKILL, SIG_DFL);
74 signal(SIGTERM, SIG_DFL);
75
76 // Close sockets
77 if (clifd != -1) close(clifd);
78 if (cluster_sockfd != -1) close(cluster_sockfd);
79 if (tunfd != -1) close(tunfd);
80 if (udpfd != -1) close(udpfd);
81 if (controlfd != -1) close(controlfd);
82 if (daefd != -1) close(daefd);
83 if (snoopfd != -1) close(snoopfd);
84 if (ifrfd != -1) close(ifrfd);
85 if (ifr6fd != -1) close(ifr6fd);
86 if (rand_fd != -1) close(rand_fd);
87 if (epollfd != -1) close(epollfd);
88
89 for (i = 0; radfds && i < RADIUS_FDS; i++)
90 close(radfds[i]);
91
92 #ifdef BGP
93 for (i = 0; i < BGP_NUM_PEERS; i++)
94 if (bgp_peers[i].sock != -1)
95 close(bgp_peers[i].sock);
96 #endif /* BGP */
97
98 return pid;
99 }
100
101 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
102 struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr)
103 {
104 ssize_t r;
105 struct msghdr msg;
106 struct cmsghdr *cmsg;
107 struct iovec vec;
108 char cbuf[128];
109
110 memset(&msg, 0, sizeof(msg));
111 msg.msg_name = from;
112 msg.msg_namelen = *fromlen;
113
114 vec.iov_base = buf;
115 vec.iov_len = len;
116 msg.msg_iov = &vec;
117 msg.msg_iovlen = 1;
118 msg.msg_flags = 0;
119
120 msg.msg_control = cbuf;
121 msg.msg_controllen = sizeof(cbuf);
122
123 if ((r = recvmsg(s, &msg, flags)) < 0)
124 return r;
125
126 if (fromlen)
127 *fromlen = msg.msg_namelen;
128
129 memset(toaddr, 0, sizeof(*toaddr));
130 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
131 {
132 if (cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_PKTINFO)
133 {
134 struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
135 memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
136 break;
137 }
138 }
139
140 return r;
141 }
142
143 ssize_t sendtofrom(int s, void const *buf, size_t len, int flags,
144 struct sockaddr const *to, socklen_t tolen, struct in_addr const *from)
145 {
146 struct msghdr msg;
147 struct cmsghdr *cmsg;
148 struct iovec vec;
149 struct in_pktinfo pktinfo;
150 char cbuf[CMSG_SPACE(sizeof(pktinfo))];
151
152 memset(&msg, 0, sizeof(msg));
153 msg.msg_name = (struct sockaddr *) to;
154 msg.msg_namelen = tolen;
155
156 vec.iov_base = (void *) buf;
157 vec.iov_len = len;
158 msg.msg_iov = &vec;
159 msg.msg_iovlen = 1;
160 msg.msg_flags = 0;
161
162 msg.msg_control = cbuf;
163 msg.msg_controllen = sizeof(cbuf);
164
165 cmsg = CMSG_FIRSTHDR(&msg);
166 cmsg->cmsg_level = SOL_IP;
167 cmsg->cmsg_type = IP_PKTINFO;
168 cmsg->cmsg_len = CMSG_LEN(sizeof(pktinfo));
169
170 memset(&pktinfo, 0, sizeof(pktinfo));
171 memcpy(&pktinfo.ipi_spec_dst, from, sizeof(*from));
172 memcpy(CMSG_DATA(cmsg), &pktinfo, sizeof(pktinfo));
173
174 return sendmsg(s, &msg, flags);
175 }