1 /*
2 * Copyright (c) 2015 Intel Corporation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 /* Listen UDP (unicast or multicast) and TCP messages from tun device and send
18 * them back to client that is running in qemu.
19 */
20
21 #include <stdio.h>
22 #include <sys/socket.h>
23 #include <netinet/in.h>
24 #include <poll.h>
25 #include <errno.h>
26 #include <arpa/inet.h>
27 #include <stdlib.h>
28 #include <string.h>
29 #include <stdbool.h>
30 #include <sys/ioctl.h>
31 #include <net/if.h>
32 #include <linux/ipv6.h>
33 #include <ifaddrs.h>
34 #include <unistd.h>
35 #include <fcntl.h>
36
37 #define SERVER_PORT 4242
38 #define MAX_BUF_SIZE 1280 /* min IPv6 MTU, the actual data is smaller */
39 #define MAX_TIMEOUT 3 /* in seconds */
40
41 static bool do_reverse;
42
reverse(unsigned char * buf,int len)43 static inline void reverse(unsigned char *buf, int len)
44 {
45 int i, last = len - 1;
46
47 for(i = 0; i < len/2; i++) {
48 unsigned char tmp = buf[i];
49 buf[i] = buf[last - i];
50 buf[last - i] = tmp;
51 }
52 }
53
get_ifindex(const char * name)54 static int get_ifindex(const char *name)
55 {
56 struct ifreq ifr;
57 int sk, err;
58
59 if (!name)
60 return -1;
61
62 sk = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0);
63 if (sk < 0)
64 return -1;
65
66 memset(&ifr, 0, sizeof(ifr));
67 strncpy(ifr.ifr_name, name, sizeof(ifr.ifr_name) - 1);
68
69 err = ioctl(sk, SIOCGIFINDEX, &ifr);
70
71 close(sk);
72
73 if (err < 0)
74 return -1;
75
76 return ifr.ifr_ifindex;
77 }
78
get_socket(int family,int proto)79 static int get_socket(int family, int proto)
80 {
81 int fd;
82
83 fd = socket(family, proto == IPPROTO_TCP ? SOCK_STREAM : SOCK_DGRAM,
84 proto);
85 if (fd < 0) {
86 perror("socket");
87 exit(-errno);
88 }
89 return fd;
90 }
91
bind_device(int fd,const char * interface,void * addr,int len,int family)92 static int bind_device(int fd, const char *interface, void *addr, int len,
93 int family)
94 {
95 struct ifreq ifr;
96 int ret, val = 1;
97
98 memset(&ifr, 0, sizeof(ifr));
99 snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%s", interface);
100
101 if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE,
102 (void *)&ifr, sizeof(ifr)) < 0) {
103 perror("SO_BINDTODEVICE");
104 exit(-errno);
105 }
106
107 setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
108
109 switch (family) {
110 struct sockaddr_in6 *addr6;
111 struct sockaddr_in *addr4;
112 char addr_buf[INET6_ADDRSTRLEN];
113
114 case AF_INET:
115 addr4 = ((struct sockaddr_in *)addr);
116 printf("Socket %d binding to %s\n", fd,
117 inet_ntop(AF_INET, &addr4->sin_addr,
118 addr_buf, sizeof(addr_buf)));
119 break;
120 case AF_INET6:
121 addr6 = ((struct sockaddr_in6 *)addr);
122 printf("Socket %d binding to %s\n", fd,
123 inet_ntop(AF_INET6, &addr6->sin6_addr,
124 addr_buf, sizeof(addr_buf)));
125 break;
126 }
127
128 ret = bind(fd, (struct sockaddr *)addr, len);
129 if (ret < 0) {
130 perror("bind");
131 }
132 }
133
receive(int fd,unsigned char * buf,int buflen,struct sockaddr * addr,socklen_t * addrlen,int proto)134 static int receive(int fd, unsigned char *buf, int buflen,
135 struct sockaddr *addr, socklen_t *addrlen, int proto)
136 {
137 int ret;
138
139 if (proto == IPPROTO_UDP) {
140 ret = recvfrom(fd, buf, buflen, 0, addr, addrlen);
141 if (ret < 0) {
142 perror("recv");
143 return ret;
144 }
145 } else if (proto == IPPROTO_TCP) {
146 ret = read(fd, buf, buflen);
147 if (ret < 0) {
148 perror("read");
149 return ret;
150 }
151 } else {
152 printf("Invalid protocol %d\n", proto);
153 return -EPROTONOSUPPORT;
154 }
155
156 return ret;
157 }
158
reply(int fd,unsigned char * buf,int buflen,struct sockaddr * addr,socklen_t addrlen,int proto)159 static int reply(int fd, unsigned char *buf, int buflen,
160 struct sockaddr *addr, socklen_t addrlen, int proto)
161 {
162 int ret;
163
164 if (proto == IPPROTO_UDP) {
165 ret = sendto(fd, buf, buflen, 0, addr, addrlen);
166 if (ret < 0)
167 perror("send");
168
169 } else if (proto == IPPROTO_TCP) {
170 int sent = 0;
171
172 do {
173 ret = write(fd, buf + sent, buflen - sent);
174 if (ret <= 0)
175 break;
176
177 sent += ret;
178 } while (sent < buflen);
179
180 } else {
181 printf("Invalid protocol %d\n", proto);
182 return -EPROTONOSUPPORT;
183 }
184
185 return ret;
186 }
187
udp_receive_and_reply(fd_set * rfds,int fd_recv,int fd_send,unsigned char * buf,int buflen,int proto,bool do_reverse)188 static int udp_receive_and_reply(fd_set *rfds, int fd_recv, int fd_send,
189 unsigned char *buf, int buflen, int proto,
190 bool do_reverse)
191 {
192 if (FD_ISSET(fd_recv, rfds)) {
193 struct sockaddr_in6 from = { 0 };
194 socklen_t fromlen = sizeof(from);
195 int ret;
196
197 ret = receive(fd_recv, buf, buflen,
198 (struct sockaddr *)&from, &fromlen, proto);
199 if (ret < 0)
200 return ret;
201
202 if (do_reverse)
203 reverse(buf, ret);
204
205 ret = reply(fd_send, buf, ret,
206 (struct sockaddr *)&from, fromlen, proto);
207 if (ret < 0)
208 return ret;
209
210 fprintf(stderr, ".");
211 }
212
213 return 0;
214 }
215
tcp_receive_and_reply(fd_set * rfds,fd_set * errfds,int fd_recv,int fd_send,unsigned char * buf,int buflen,int proto)216 static int tcp_receive_and_reply(fd_set *rfds, fd_set *errfds,
217 int fd_recv, int fd_send,
218 unsigned char *buf, int buflen, int proto)
219 {
220 struct sockaddr_in6 from = { 0 };
221 socklen_t fromlen = sizeof(from);
222 int ret = 0;
223
224 if (FD_ISSET(fd_recv, rfds)) {
225 ret = receive(fd_recv, buf, buflen,
226 (struct sockaddr *)&from, &fromlen, proto);
227 if (ret < 0)
228 return ret;
229
230 ret = reply(fd_send, buf, ret,
231 (struct sockaddr *)&from, fromlen, proto);
232 if (ret < 0) {
233 return ret;
234 } else if (ret == 0) {
235 close(fd_recv);
236 printf("Connection closed fd %d\n", fd_recv);
237 }
238
239 fprintf(stderr, ".");
240 }
241
242 return ret;
243 }
244
245 #define MY_MCAST_ADDR6 \
246 { { { 0xff,0x84,0,0,0,0,0,0,0,0,0,0,0,0,0,0x2 } } } /* ff84::2 */
247
248 #define MY_MCAST_ADDR4 "239.192.0.2"
249
family_to_level(int family)250 int family_to_level(int family)
251 {
252 switch (family) {
253 case AF_INET:
254 return IPPROTO_IP;
255 case AF_INET6:
256 return IPPROTO_IPV6;
257 default:
258 return -1;
259 }
260 }
261
join_mc_group(int sock,int ifindex,int family,void * addr,int addr_len)262 static int join_mc_group(int sock, int ifindex, int family, void *addr,
263 int addr_len)
264 {
265 struct group_req req;
266 int ret, off = 0;
267
268 memset(&req, 0, sizeof(req));
269
270 req.gr_interface = ifindex;
271 memcpy(&req.gr_group, addr, addr_len);
272
273 ret = setsockopt(sock, family_to_level(family), MCAST_JOIN_GROUP,
274 &req, sizeof(req));
275 if (ret < 0)
276 perror("setsockopt(MCAST_JOIN_GROUP)");
277
278 switch (family) {
279 case AF_INET:
280 ret = setsockopt(sock, family_to_level(family),
281 IP_MULTICAST_LOOP, &off, sizeof(off));
282 break;
283 case AF_INET6:
284 ret = setsockopt(sock, family_to_level(family),
285 IPV6_MULTICAST_LOOP, &off, sizeof(off));
286 break;
287 }
288 return ret;
289 }
290
find_address(int family,struct ifaddrs * if_address,const char * if_name,void * address)291 static int find_address(int family, struct ifaddrs *if_address,
292 const char *if_name, void *address)
293 {
294 struct ifaddrs *tmp;
295 int error = -ENOENT;
296
297 for (tmp = if_address; tmp; tmp = tmp->ifa_next) {
298 if (tmp->ifa_addr &&
299 !strncmp(tmp->ifa_name, if_name, IF_NAMESIZE) &&
300 tmp->ifa_addr->sa_family == family) {
301
302 switch (family) {
303 case AF_INET: {
304 struct sockaddr_in *in4 =
305 (struct sockaddr_in *)tmp->ifa_addr;
306 if (in4->sin_addr.s_addr == INADDR_ANY)
307 continue;
308 if ((in4->sin_addr.s_addr & IN_CLASSB_NET) ==
309 ((in_addr_t)0xa9fe0000))
310 continue;
311 memcpy(address, &in4->sin_addr,
312 sizeof(struct in_addr));
313 error = 0;
314 goto out;
315 }
316 case AF_INET6: {
317 struct sockaddr_in6 *in6 =
318 (struct sockaddr_in6 *)tmp->ifa_addr;
319 if (!memcmp(&in6->sin6_addr, &in6addr_any,
320 sizeof(struct in6_addr)))
321 continue;
322 if (IN6_IS_ADDR_LINKLOCAL(&in6->sin6_addr))
323 continue;
324
325 memcpy(address, &in6->sin6_addr,
326 sizeof(struct in6_addr));
327 error = 0;
328 goto out;
329 }
330 default:
331 error = -EINVAL;
332 goto out;
333 }
334 }
335 }
336
337 out:
338 return error;
339 }
340
get_address(const char * if_name,int family,void * address)341 static int get_address(const char *if_name, int family, void *address)
342 {
343 struct ifaddrs *if_address;
344 int err;
345
346 if (getifaddrs(&if_address) < 0) {
347 err = -errno;
348 fprintf(stderr, "Cannot get interface addresses for "
349 "interface %s error %d/%s",
350 if_name, err, strerror(-err));
351 return err;
352 }
353
354 err = find_address(family, if_address, if_name, address);
355
356 freeifaddrs(if_address);
357
358 return err;
359 }
360
361 extern int optind, opterr, optopt;
362 extern char *optarg;
363
364 /* The application returns:
365 * < 0 : connection or similar error
366 * 0 : no errors, all tests passed
367 * > 0 : could not send all the data to server
368 */
main(int argc,char ** argv)369 int main(int argc, char**argv)
370 {
371 int c, ret, fd4 = 0, fd6 = 0, fd4m = 0, fd6m = 0, tcp4 = 0, tcp6 = 0,
372 i = 0, timeout = 0;
373 int port = SERVER_PORT;
374 int accepted4 = -1, accepted6 = -1;
375 struct sockaddr_in6 addr6_recv = { 0 }, maddr6 = { 0 };
376 struct in6_addr mcast6_addr = MY_MCAST_ADDR6;
377 struct in_addr mcast4_addr = { 0 };
378 struct sockaddr_in addr4_recv = { 0 }, maddr4 = { 0 };
379 int family;
380 unsigned char buf[MAX_BUF_SIZE];
381 char addr_buf[INET6_ADDRSTRLEN];
382 const struct in6_addr any = IN6ADDR_ANY_INIT;
383 const char *interface = NULL;
384 fd_set rfds, errfds;
385 struct timeval tv = {};
386 int ifindex = -1;
387 int opt = 1;
388
389 opterr = 0;
390
391 while ((c = getopt(argc, argv, "i:p:r")) != -1) {
392 switch (c) {
393 case 'i':
394 interface = optarg;
395 break;
396 case 'p':
397 port = atoi(optarg);
398 break;
399 case 'r':
400 do_reverse = true;
401 break;
402 }
403 }
404
405 if (!interface) {
406 printf("usage: %s [-r] -i <iface> [-p <port>]\n", argv[0]);
407 printf("\t-r Reverse the sent UDP data.\n");
408 printf("\t-i Use this network interface.\n");
409 printf("\t-p Use this port (default is %d)\n", SERVER_PORT);
410 exit(-EINVAL);
411 }
412
413 ifindex = get_ifindex(interface);
414 if (ifindex < 0) {
415 printf("Invalid interface %s\n", interface);
416 exit(-EINVAL);
417 }
418
419 addr4_recv.sin_family = AF_INET;
420 addr4_recv.sin_port = htons(port);
421
422 /* We want to bind to global unicast address here so that
423 * we can listen correct addresses. We do not want to listen
424 * link local addresses in this test.
425 */
426 get_address(interface, AF_INET, &addr4_recv.sin_addr);
427 printf("IPv4: binding to %s\n",
428 inet_ntop(AF_INET, &addr4_recv.sin_addr,
429 addr_buf, sizeof(addr_buf)));
430
431 addr6_recv.sin6_family = AF_INET6;
432 addr6_recv.sin6_port = htons(port);
433
434 /* Bind to global unicast address instead of ll address */
435 get_address(interface, AF_INET6, &addr6_recv.sin6_addr);
436 printf("IPv6: binding to %s\n",
437 inet_ntop(AF_INET6, &addr6_recv.sin6_addr,
438 addr_buf, sizeof(addr_buf)));
439
440 memcpy(&maddr6.sin6_addr, &mcast6_addr, sizeof(struct in6_addr));
441 maddr6.sin6_family = AF_INET6;
442 maddr6.sin6_port = htons(port);
443
444 mcast4_addr.s_addr = inet_addr(MY_MCAST_ADDR4);
445 memcpy(&maddr4.sin_addr, &mcast4_addr, sizeof(struct in_addr));
446 maddr4.sin_family = AF_INET;
447 maddr4.sin_port = htons(port);
448
449 restart:
450 if (fd4)
451 close(fd4);
452 if (fd6)
453 close(fd6);
454 if (fd4m)
455 close(fd4m);
456 if (fd6m)
457 close(fd6m);
458 if (tcp4)
459 close(tcp4);
460 if (tcp6)
461 close(tcp6);
462
463 fd4 = get_socket(AF_INET, IPPROTO_UDP);
464 fd6 = get_socket(AF_INET6, IPPROTO_UDP);
465 fd4m = get_socket(AF_INET, IPPROTO_UDP);
466 fd6m = get_socket(AF_INET6, IPPROTO_UDP);
467 tcp4 = get_socket(AF_INET, IPPROTO_TCP);
468 tcp6 = get_socket(AF_INET6, IPPROTO_TCP);
469
470 printf("Sockets: UPD IPv4 %d IPv6 %d, mcast IPv4 %d IPv6 %d, "
471 "TCP IPv4 %d IPv6 %d\n",
472 fd4, fd6, fd4m, fd6m, tcp4, tcp6);
473
474 bind_device(fd4, interface, &addr4_recv, sizeof(addr4_recv), AF_INET);
475 bind_device(fd6, interface, &addr6_recv, sizeof(addr6_recv), AF_INET6);
476
477 bind_device(fd4m, interface, &maddr4, sizeof(maddr4), AF_INET);
478 bind_device(fd6m, interface, &maddr6, sizeof(maddr6), AF_INET6);
479
480 join_mc_group(fd4m, ifindex, AF_INET, &maddr4, sizeof(maddr4));
481 join_mc_group(fd6m, ifindex, AF_INET6, &maddr6, sizeof(maddr6));
482
483 ret = setsockopt(tcp4, SOL_SOCKET, SO_REUSEPORT, &opt, sizeof(opt));
484 if (ret < 0) {
485 perror("setsockopt TCP v4");
486 }
487 ret = setsockopt(tcp6, SOL_SOCKET, SO_REUSEPORT, &opt, sizeof(opt));
488 if (ret < 0) {
489 perror("setsockopt TCP v6");
490 }
491 bind_device(tcp4, interface, &addr4_recv, sizeof(addr4_recv), AF_INET);
492 bind_device(tcp6, interface, &addr6_recv, sizeof(addr6_recv), AF_INET6);
493
494 if (listen(tcp4, 0) < 0) {
495 perror("IPv4 TCP listen");
496 }
497 if (listen(tcp6, 0) < 0) {
498 perror("IPv6 TCP listen");
499 }
500
501 if (fcntl(tcp4, F_SETFL, O_NONBLOCK) < 0) {
502 perror("IPv4 TCP non blocking");
503 }
504
505 if (fcntl(tcp6, F_SETFL, O_NONBLOCK) < 0) {
506 perror("IPv6 TCP non blocking");
507 }
508
509 #define MAX(a,b) ((a) > (b) ? (a) : (b))
510
511 while (1) {
512 int addr4len = sizeof(addr4_recv);
513 int addr6len = sizeof(addr6_recv);
514 int fd;
515
516 FD_ZERO(&rfds);
517 FD_SET(fd4, &rfds);
518 FD_SET(fd6, &rfds);
519 FD_SET(fd4m, &rfds);
520 FD_SET(fd6m, &rfds);
521 FD_SET(tcp4, &rfds);
522 FD_SET(tcp6, &rfds);
523
524 FD_ZERO(&errfds);
525 FD_SET(tcp4, &errfds);
526 FD_SET(tcp6, &errfds);
527
528 fd = MAX(tcp4, tcp6);
529
530 if (accepted4 >= 0) {
531 FD_SET(accepted4, &rfds);
532 FD_SET(accepted4, &errfds);
533 fd = MAX(fd, accepted4);
534 }
535 if (accepted6 >= 0) {
536 FD_SET(accepted6, &rfds);
537 FD_SET(accepted6, &errfds);
538 fd = MAX(fd, accepted6);
539 }
540
541 ret = select(fd + 1, &rfds, NULL,
542 &errfds, NULL);
543 if (ret < 0) {
544 perror("select");
545 break;
546 } else if (ret == 0) {
547 continue;
548 }
549
550 if (accepted4 < 0) {
551 accepted4 = accept(tcp4, (struct sockaddr *)&addr4_recv,
552 &addr4len);
553 if (accepted4 < 0 &&
554 (errno != EAGAIN && errno != EWOULDBLOCK)) {
555 perror("accept IPv4");
556 break;
557 } else if (accepted4 >= 0) {
558 FD_SET(accepted4, &errfds);
559 FD_SET(accepted4, &rfds);
560 }
561 }
562
563 if (accepted6 < 0) {
564 accepted6 = accept(tcp6, (struct sockaddr *)&addr6_recv,
565 &addr6len);
566 if (accepted6 < 0 &&
567 (errno != EAGAIN && errno != EWOULDBLOCK)) {
568 perror("accept IPv6");
569 break;
570 } else if (accepted6 >= 0) {
571 FD_SET(accepted6, &errfds);
572 FD_SET(accepted6, &rfds);
573
574 printf("New connection fd %d\n", accepted6);
575 }
576 }
577
578 /* Unicast IPv4 */
579 if (udp_receive_and_reply(&rfds, fd4, fd4, buf, sizeof(buf),
580 IPPROTO_UDP, do_reverse) < 0)
581 goto restart;
582
583 /* Unicast IPv6 */
584 if (udp_receive_and_reply(&rfds, fd6, fd6, buf, sizeof(buf),
585 IPPROTO_UDP, do_reverse) < 0)
586 goto restart;
587
588 /* Multicast IPv4 */
589 if (udp_receive_and_reply(&rfds, fd4m, fd4, buf, sizeof(buf),
590 IPPROTO_UDP, do_reverse) < 0)
591 goto restart;
592
593 /* Multicast IPv6 */
594 if (udp_receive_and_reply(&rfds, fd6m, fd6, buf, sizeof(buf),
595 IPPROTO_UDP, do_reverse) < 0)
596 goto restart;
597
598 /* TCP IPv4 */
599 ret = tcp_receive_and_reply(&rfds, &errfds,
600 accepted4, accepted4,
601 buf, sizeof(buf),
602 IPPROTO_TCP);
603 if (ret < 0)
604 goto restart;
605 else if (ret == 0)
606 accepted4 = -1;
607
608 /* TCP IPv6 */
609 ret = tcp_receive_and_reply(&rfds, &errfds,
610 accepted6, accepted6,
611 buf, sizeof(buf),
612 IPPROTO_TCP);
613 if (ret < 0)
614 goto restart;
615 else if (ret == 0)
616 accepted6 = -1;
617 }
618
619 close(fd4);
620 close(fd6);
621 close(fd4m);
622 close(fd6m);
623 close(tcp4);
624 close(tcp6);
625
626 printf("\n");
627
628 exit(0);
629 }
630
