debuggers.hg

view tools/vnet/vnetd/vnetd.c @ 0:7d21f7218375

Exact replica of unstable on 051908 + README-this
author Mukesh Rathor
date Mon May 19 15:34:57 2008 -0700 (2008-05-19)
parents
children
line source
1 /*
2 * Copyright (C) 2005, 2006 Mike Wray <mike.wray@hp.com>.
3 *
4 * This library is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU Lesser General Public License as
6 * published by the Free Software Foundation; either version 2.1 of the
7 * License, or (at your option) any later version. This library is
8 * distributed in the hope that it will be useful, but WITHOUT ANY
9 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
10 * FITNESS FOR A PARTICULAR PURPOSE.
11 * See the GNU Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public License
14 * along with this library; if not, write to the Free Software Foundation,
15 * Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
16 */
17 #include <stdlib.h>
18 #include <stdbool.h>
19 #include <stdint.h>
20 #include <unistd.h>
21 #include <stdio.h>
22 #include <getopt.h>
23 #include <errno.h>
24 #include <time.h>
25 #include <fcntl.h>
26 #include <sys/types.h>
27 #include <sys/ioctl.h>
28 #include <sys/socket.h>
29 #include <sys/un.h>
30 #include <netinet/in.h>
31 #include <arpa/inet.h>
32 #include <string.h>
34 #include <signal.h>
35 #include <sys/wait.h>
36 #include <sys/select.h>
38 #include <asm/types.h> // For __u32 etc.
40 #include <linux/ip.h> // For struct iphdr.
41 #include <linux/udp.h> // For struct udphdr.
43 #include <linux/if.h>
44 #include <linux/if_ether.h>
45 #include <linux/if_tun.h>
47 #include "sys_kernel.h"
48 #include "skbuff.h"
49 #include "spinlock.h"
51 #include "allocate.h"
53 #include "file_stream.h"
54 #include "string_stream.h"
55 #include "socket_stream.h"
56 #include "sys_net.h"
58 #include "enum.h"
59 #include "sxpr.h"
60 #include "sxpr_parser.h"
62 #include "connection.h"
63 #include "select.h"
64 #include "timer.h"
66 #include "if_etherip.h"
67 #include "if_varp.h"
68 #include "varp.h"
69 #include "vnet.h"
70 #include "vnet_dev.h"
71 #include "vnet_eval.h"
72 #include "vnet_forward.h"
73 #include "tunnel.h"
74 #include "etherip.h"
75 #include "sxpr_util.h"
77 #define MODULE_NAME "VNETD"
78 #define DEBUG 1
79 #undef DEBUG
80 #include "debug.h"
82 #define PROGRAM "vnetd"
83 #define VERSION "1.0"
85 typedef struct Vnetd {
86 unsigned long port;
87 int ttl;
88 int verbose;
89 int etherip;
91 int udp_sock;
92 struct sockaddr_in udp_sock_addr;
93 int mcast_sock;
94 struct sockaddr_in mcast_sock_addr;
95 int etherip_sock;
96 struct sockaddr_in etherip_sock_addr;
97 int unix_sock;
98 char *unix_path;
100 int raw_sock;
102 struct sockaddr_in ucast_addr;
103 struct sockaddr_in mcast_addr;
105 HashTable *vnet_table;
107 ConnList *conns;
109 } Vnetd;
111 Vnetd _vnetd = {}, *vnetd = &_vnetd;
113 uint32_t vnetd_intf_addr(Vnetd *vnetd){
114 return vnetd->ucast_addr.sin_addr.s_addr;
115 }
117 uint32_t vnetd_mcast_addr(Vnetd *vnetd){
118 return vnetd->mcast_addr.sin_addr.s_addr;
119 }
121 void vnetd_set_mcast_addr(Vnetd *vnetd, uint32_t addr){
122 varp_mcast_addr = addr;
123 vnetd->mcast_addr.sin_addr.s_addr = addr;
124 }
126 uint16_t vnetd_mcast_port(Vnetd *vnetd){
127 return vnetd->mcast_addr.sin_port;
128 }
130 uint32_t vnetd_addr(void){
131 return vnetd_intf_addr(vnetd);
132 }
134 /** Open tap device.
135 */
136 int tap_open(struct net_device *dev){
137 int err;
138 /* IFF_TAP : Ethernet tap device.
139 * IFF_NO_PI : Don't add packet info struct when reading.
140 * IFF_ONE_QUEUE: Drop packets when the dev queue is full. The driver uses
141 * the queue size from the device, which defaults to 1000 for etherdev.
142 * If not set the driver stops the device queue when it goes over
143 * TUN_READQ_SIZE, which is 10. Broken - makes the device stall
144 * under load.
145 */
146 struct ifreq ifr = { };
147 ifr.ifr_flags = (IFF_TAP | IFF_NO_PI | IFF_ONE_QUEUE);
149 dprintf(">\n");
150 dev->tapfd = open("/dev/net/tun", O_RDWR);
151 if(dev->tapfd < 0){
152 err = -errno;
153 perror("open");
154 goto exit;
155 }
156 strcpy(ifr.ifr_name, dev->name);
157 err = ioctl(dev->tapfd, TUNSETIFF, (void *)&ifr);
158 if(err < 0){
159 err = -errno;
160 perror("ioctl");
161 goto exit;
162 }
163 strcpy(dev->name, ifr.ifr_name);
164 dprintf("> dev=%s\n", dev->name);
165 // Make it non-blocking.
166 fcntl(dev->tapfd, F_SETFL, O_NONBLOCK);
168 exit:
169 if(err && (dev->tapfd >= 0)){
170 close(dev->tapfd);
171 dev->tapfd = -1;
172 }
173 dprintf("< err=%d\n", err);
174 return err;
175 }
177 /** Close tap device.
178 */
179 int tap_close(struct net_device *dev){
180 int err = 0;
182 if(dev->tapfd >= 0){
183 err = close(dev->tapfd);
184 dev->tapfd = -1;
185 }
186 return err;
187 }
189 /** Open vnif tap device for a vnet.
190 */
191 int vnet_dev_add(struct Vnet *vnet){
192 int err = 0;
193 struct net_device *dev = ALLOCATE(struct net_device);
194 strcpy(dev->name, vnet->device);
195 err = tap_open(dev);
196 if(err){
197 wprintf("> Unable to open tap device.\n"
198 "The tun module must be loaded and\n"
199 "the vnet kernel module must not be loaded.\n");
200 deallocate(dev);
201 goto exit;
202 }
203 vnet->dev = dev;
204 exit:
205 return err;
206 }
208 /** Close vnif tap device for a vnet.
209 */
210 void vnet_dev_remove(struct Vnet *vnet){
211 if(vnet->dev){
212 tap_close(vnet->dev);
213 deallocate(vnet->dev);
214 vnet->dev = NULL;
215 }
216 }
218 /** Receive decapsulated ethernet packet on skb->dev.
219 * Always succeeds. The skb must not be referred to after
220 * this is called.
221 */
222 int netif_rx(struct sk_buff *skb){
223 int err = 0, n, k;
224 struct net_device *dev = skb->dev;
225 if(!dev){
226 err = -ENODEV;
227 goto exit;
228 }
229 n = skb->tail - skb->mac.raw;
230 k = write(dev->tapfd, skb->mac.raw, n);
231 if(k < 0){
232 err = -errno;
233 perror("write");
234 } else if(k < n){
235 //todo: What?
236 }
237 exit:
238 kfree_skb(skb);
239 return err;
240 }
242 static const int SKB_SIZE = 1700;
244 struct sk_buff *skb_new(void){
245 return alloc_skb(SKB_SIZE, GFP_ATOMIC);
246 }
248 /** Receive a packet and fill-in source and destination addresses.
249 * Just like recvfrom() but adds the destination address.
250 * The socket must have the IP_PKTINFO option set so that the
251 * destination address information is available.
252 *
253 * @param sock socket
254 * @param buf receive buffer
255 * @param len size of buffer
256 * @param flags receive flags
257 * @param from source address
258 * @param fromlen size of source address
259 * @param dest destination address
260 * @param destlen size of destination address
261 * @return number of bytes read on success, negative otherwise
262 */
263 int recvfromdest(int sock, void *buf, size_t len, int flags,
264 struct sockaddr *from, socklen_t *fromlen,
265 struct sockaddr *dest, socklen_t *destlen){
266 int ret = 0;
267 struct iovec iov;
268 struct msghdr msg;
269 struct cmsghdr *cmsg;
270 char cbuf[1024];
271 struct in_pktinfo *info;
272 struct sockaddr_in *dest_in = (struct sockaddr_in *)dest;
274 //dest_in->sin_family = AF_INET;
275 //dest_in->sin_port = 0;
276 getsockname(sock, dest, destlen);
278 iov.iov_base = buf;
279 iov.iov_len = len;
280 msg.msg_name = from;
281 msg.msg_namelen = *fromlen;
282 msg.msg_iov = &iov;
283 msg.msg_iovlen = 1;
284 msg.msg_control = cbuf;
285 msg.msg_controllen = sizeof(cbuf);
287 ret = recvmsg(sock, &msg, flags);
288 if(ret < 0) goto exit;
289 *fromlen = msg.msg_namelen;
291 for(cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)){
292 if((cmsg->cmsg_level == SOL_IP) && (cmsg->cmsg_type == IP_PKTINFO)){
293 info = (void*)CMSG_DATA(cmsg);
294 dest_in->sin_addr = info->ipi_addr;
295 break;
296 }
297 }
299 exit:
300 return ret;
301 }
303 /** Read an skb from a udp socket and fill in its headers.
304 */
305 int skb_recv_udp(int sock, int flags,
306 struct sockaddr_in *peer, socklen_t *peer_n,
307 struct sockaddr_in *dest, socklen_t *dest_n,
308 struct sk_buff **pskb){
309 int err = 0, n;
310 struct sk_buff *skb = skb_new();
312 skb->mac.raw = skb->data;
313 skb_reserve(skb, ETH_HLEN);
314 skb->nh.raw = skb->data;
315 skb_reserve(skb, sizeof(struct iphdr));
316 // Rcvr wants skb->data pointing at the udphdr.
317 skb->h.raw = skb_put(skb, sizeof(struct udphdr));
318 n = recvfromdest(sock, skb->tail, skb_tailroom(skb), flags,
319 (struct sockaddr *)peer, peer_n,
320 (struct sockaddr *)dest, dest_n);
321 if(n < 0){
322 err = -errno;
323 //perror("recvfrom");
324 goto exit;
325 }
326 dprintf("> peer=%s:%d\n", inet_ntoa(peer->sin_addr), ntohs(peer->sin_port));
327 dprintf("> dest=%s:%d\n", inet_ntoa(dest->sin_addr), ntohs(dest->sin_port));
328 skb_put(skb, n);
329 skb->protocol = skb->nh.iph->protocol = IPPROTO_UDP;
330 skb->nh.iph->saddr = peer->sin_addr.s_addr;
331 skb->h.uh->source = peer->sin_port;
332 skb->nh.iph->daddr = dest->sin_addr.s_addr;
333 skb->h.uh->dest = dest->sin_port;
334 exit:
335 if(err < 0){
336 kfree_skb(skb);
337 *pskb = NULL;
338 } else {
339 *pskb = skb;
340 }
341 return (err < 0 ? err : n);
342 }
344 /** Read an skb fom a raw socket and fill in its headers.
345 */
346 int skb_recv_raw(int sock, int flags,
347 struct sockaddr_in *peer, socklen_t *peer_n,
348 struct sockaddr_in *dest, socklen_t *dest_n,
349 struct sk_buff **pskb){
350 int err = 0, n;
351 struct sk_buff *skb = skb_new();
353 skb->mac.raw = skb->data;
354 skb_reserve(skb, ETH_HLEN);
355 skb->nh.raw = skb->data;
356 skb_reserve(skb, sizeof(struct iphdr));
357 // Rcvr wants skb->data pointing after ip hdr, at raw protocol hdr.
358 n = recvfromdest(sock, skb->tail, skb_tailroom(skb), flags,
359 (struct sockaddr *)peer, peer_n,
360 (struct sockaddr *)dest, dest_n);
361 if(n < 0){
362 err = -errno;
363 //perror("recvfrom");
364 goto exit;
365 }
366 skb_put(skb, n);
367 // On a raw socket the port in the address is the protocol.
368 skb->protocol = skb->nh.iph->protocol = peer->sin_port;
369 skb->nh.iph->saddr = peer->sin_addr.s_addr;
370 skb->nh.iph->daddr = dest->sin_addr.s_addr;
371 exit:
372 if(err < 0){
373 kfree_skb(skb);
374 *pskb = NULL;
375 } else {
376 *pskb = skb;
377 }
378 return (err < 0 ? err : n);
379 }
381 /** Read an skb from a file descriptor.
382 * Used for skbs coming to us from the tap device.
383 * The skb content is an ethernet frame.
384 */
385 int skb_read(int fd, struct sk_buff **pskb){
386 int err = 0, n;
387 struct sk_buff *skb = skb_new();
389 // Reserve space for the headers we will add.
390 skb_reserve(skb, 100);
391 // Rcvr will want ethhdr on the skb.
392 skb->mac.raw = skb->tail;
393 n = read(fd, skb->tail, skb_tailroom(skb));
394 if(n < 0){
395 err = -errno;
396 //perror("read");
397 goto exit;
398 }
399 skb_put(skb, n);
400 exit:
401 if(err < 0){
402 kfree_skb(skb);
403 *pskb = NULL;
404 } else {
405 *pskb = skb;
406 }
407 return (err < 0 ? err : n);
408 }
410 /** Read an skb from the tap device for a vnet and send it.
411 */
412 int vnet_read(Vnet *vnet){
413 int err;
414 struct sk_buff *skb = NULL;
416 err = skb_read(vnet->dev->tapfd, &skb);
417 if(err < 0) goto exit;
418 err = vnet_skb_send(skb, &vnet->vnet);
419 exit:
420 if(skb) kfree_skb(skb);
421 return (err < 0 ? err : 0);
422 }
424 /** Transmit an skb to the network.
425 */
426 int _skb_xmit(struct sk_buff *skb, uint32_t saddr){
427 int err = 0;
428 int sock;
429 unsigned char *data;
430 struct sockaddr_in addr = { .sin_family = AF_INET };
431 int flags = 0;
433 if(saddr){
434 dprintf("> Raw IP send\n");
435 sock = vnetd->raw_sock;
436 skb->nh.iph->saddr = saddr;
437 addr.sin_addr.s_addr = skb->nh.iph->daddr;
438 // Should be the protocol, but is ignored. See raw(7) man page.
439 addr.sin_port = 0;
440 // Data includes the ip header.
441 data = (void*)(skb->nh.iph);
442 } else {
443 switch(skb->nh.iph->protocol){
444 case IPPROTO_UDP:
445 dprintf("> protocol=UDP\n");
446 sock = vnetd->udp_sock;
447 // Data comes after the udp header.
448 data = (void*)(skb->h.uh + 1);
449 addr.sin_addr.s_addr = skb->nh.iph->daddr;
450 addr.sin_port = skb->h.uh->dest;
451 break;
452 case IPPROTO_ETHERIP:
453 dprintf("> protocol=ETHERIP\n");
454 if(vnetd->etherip_sock < 0){
455 err = -ENOSYS;
456 goto exit;
457 }
458 sock = vnetd->etherip_sock;
459 // Data comes after the ip header.
460 data = (void*)(skb->nh.iph + 1);
461 addr.sin_addr.s_addr = skb->nh.iph->daddr;
462 // Should be the protocol, but is ignored. See raw(7) man page.
463 addr.sin_port = 0;
464 break;
465 default:
466 err = -ENOSYS;
467 wprintf("> protocol=%d, %d\n", skb->nh.iph->protocol, skb->protocol);
468 goto exit;
469 }
470 }
472 dprintf("> sending %d bytes to %s:%d protocol=%d\n",
473 skb->tail - data,
474 inet_ntoa(addr.sin_addr),
475 ntohs(addr.sin_port),
476 skb->nh.iph->protocol);
478 err = sendto(sock, data, skb->tail - data, flags,
479 (struct sockaddr *)&addr, sizeof(addr));
480 if(err < 0){
481 err = -errno;
482 perror("sendto");
483 }
484 exit:
485 if(err >= 0){
486 // Caller will assume skb freed if no error.
487 kfree_skb(skb);
488 err = 0;
489 }
490 dprintf("< err=%d\n", err);
491 return err;
492 }
494 int varp_open(uint32_t mcaddr, uint16_t port){
495 return 0;
496 }
498 void varp_close(void){
499 }
501 /** Create a raw socket.
502 *
503 * @param protocol protocol
504 * @param flags flags (VSOCK_*)
505 * @param mcaddr multicast addr used with flag VSOCK_MULTICAST
506 * @param sock return value for the socket
507 */
508 int vnetd_raw_socket(Vnetd *vnetd, int protocol, int flags,
509 uint32_t mcaddr, int *sock){
510 int err;
511 int bcast = (flags & VSOCK_BROADCAST);
513 err = *sock = socket(AF_INET, SOCK_RAW, protocol);
514 if(err < 0){
515 err = -errno;
516 perror("socket");
517 goto exit;
518 }
519 if(bcast){
520 err = setsock_broadcast(*sock, bcast);
521 if(err < 0) goto exit;
522 }
523 if(flags & VSOCK_MULTICAST){
524 err = setsock_multicast(*sock, INADDR_ANY, mcaddr);
525 if(err < 0) goto exit;
526 }
527 //todo ?? fcntl(*sock, F_SETFL, O_NONBLOCK);
528 exit:
529 return err;
530 }
532 int get_dev_address(char *dev, unsigned long *addr){
533 int err = 0;
534 int sock = -1;
535 struct ifreq ifreq = {};
536 struct sockaddr_in *in_addr;
538 sock = socket(AF_INET, SOCK_DGRAM, 0);
539 if(sock < 0){
540 err = -errno;
541 goto exit;
542 }
543 strncpy(ifreq.ifr_name, dev, IFNAMSIZ);
544 err = ioctl(sock, SIOCGIFADDR, &ifreq);
545 if(err){
546 err = -errno;
547 goto exit;
548 }
549 in_addr = (struct sockaddr_in *) &ifreq.ifr_addr;
550 *addr = in_addr->sin_addr.s_addr;
551 //iprintf("> dev=%s addr=%s\n", dev, inet_ntoa(in_addr->sin_addr));
552 exit:
553 if(sock >= 0) close(sock);
554 return err;
555 }
557 int get_intf_address(unsigned long *addr){
558 int err = 0;
559 char *devs[] = { "xen-br0", "eth0", "eth1", "eth2", NULL };
560 char **dev;
562 for(dev = devs; *dev; dev++){
563 err = get_dev_address(*dev, addr);
564 if(err == 0) goto exit;
565 }
566 err = -ENOSYS;
567 exit:
568 return err;
569 }
571 /** Get our own address. So we can ignore broadcast traffic
572 * we sent ourselves.
573 *
574 * @param addr
575 * @return 0 on success, error code otherwise
576 */
577 int get_self_addr(struct sockaddr_in *addr){
578 int err = 0;
579 char hostname[1024] = {};
580 unsigned long saddr;
582 err = gethostname(hostname, sizeof(hostname) - 1);
583 if(err){
584 err = -errno;
585 perror("gethostname");
586 goto exit;
587 }
588 err = get_host_address(hostname, &saddr);
589 if(err) goto exit;
590 addr->sin_addr.s_addr = saddr;
591 if(saddr == htonl(INADDR_LOOPBACK)){
592 err = get_intf_address(&saddr);
593 if(err) goto exit;
594 }
595 addr->sin_addr.s_addr = saddr;
596 err = 0;
597 exit:
598 return err;
599 }
601 static int eval_vnetd_mcaddr(Sxpr exp, IOStream *out, void *data){
602 int err = 0;
603 Vnetd *vnetd = data;
604 Sxpr oaddr = intern("addr");
605 Sxpr ottl = intern("ttl");
606 uint32_t addr;
607 int ttl;
609 err = child_addr(exp, oaddr, &addr);
610 if(err < 0) goto exit;
611 vnetd_set_mcast_addr(vnetd, addr);
612 if(child_int(exp, ottl, &ttl) == 0){
613 vnetd->ttl = ttl;
614 }
615 exit:
616 return err;
617 }
619 static int vnetd_eval_io(Vnetd *vnetd, Parser *parser, SxprEval *defs,
620 IOStream *in, IOStream *out){
621 int err = 0;
622 char buf[1024];
623 int k, n = sizeof(buf) - 1;
625 for( ; ; ){
626 k = IOStream_read(in, buf, n);
627 if(k < 0){
628 err = k;
629 goto exit;
630 }
631 err = Parser_input(parser, buf, k);
632 if(err < 0) goto exit;
633 while(Parser_ready(parser)){
634 Sxpr exp = Parser_get_val(parser);
635 if(NONEP(exp)) break;
636 err = vnet_eval_defs(defs, exp, out, vnetd);
637 if(err) goto exit;
638 }
639 if(Parser_at_eof(parser)) break;
640 }
641 exit:
642 return err;
643 }
645 static int vnetd_configure(Vnetd *vnetd, char *file){
646 int err = 0;
647 Parser *parser = NULL;
648 IOStream *io = NULL;
649 SxprEval defs[] = {
650 { .name = intern("peer.add"), .fn = eval_peer_add },
651 { .name = intern("varp.mcaddr"), .fn = eval_vnetd_mcaddr },
652 { .name = intern("vnet.add"), .fn = eval_vnet_add },
653 { .name = ONONE, .fn = NULL } };
655 parser = Parser_new();
656 io = file_stream_fopen(file, "rb");
657 if(!io){
658 err = -errno;
659 goto exit;
660 }
661 vnetd_eval_io(vnetd, parser, defs, io, iostdout);
662 exit:
663 if(io) IOStream_close(io);
664 Parser_free(parser);
665 return err;
666 }
668 #define OPT_MCADDR 'a'
669 #define KEY_MCADDR "varp_mcaddr"
670 #define DOC_MCADDR "<addr>\n\t VARP multicast address"
672 #define OPT_FILE 'f'
673 #define KEY_FILE "file"
674 #define DOC_FILE "<file>\n\t Configuration file to load"
676 #define OPT_HELP 'h'
677 #define KEY_HELP "help"
678 #define DOC_HELP "\n\tprint help"
680 #define OPT_VERSION 'v'
681 #define KEY_VERSION "version"
682 #define DOC_VERSION "\n\tprint version"
684 #define OPT_VERBOSE 'V'
685 #define KEY_VERBOSE "verbose"
686 #define DOC_VERBOSE "\n\tverbose flag"
688 /** Print a usage message.
689 * Prints to stdout if err is zero, and exits with 0.
690 * Prints to stderr if err is non-zero, and exits with 1.
691 *
692 * @param err error code
693 */
694 static void usage(int err){
695 FILE *out = (err ? stderr : stdout);
697 fprintf(out, "Usage: %s [options]\n", PROGRAM);
698 fprintf(out, "-%c, --%s %s\n", OPT_MCADDR, KEY_MCADDR, DOC_MCADDR);
699 fprintf(out, "-%c, --%s %s\n", OPT_FILE, KEY_FILE, DOC_FILE);
700 fprintf(out, "-%c, --%s %s\n", OPT_VERBOSE, KEY_VERBOSE, DOC_VERBOSE);
701 fprintf(out, "-%c, --%s %s\n", OPT_VERSION, KEY_VERSION, DOC_VERSION);
702 fprintf(out, "-%c, --%s %s\n", OPT_HELP, KEY_HELP, DOC_HELP);
703 exit(err ? 1 : 0);
704 }
706 /** Short options. Options followed by ':' take an argument. */
707 static char *short_opts = (char[]){
708 OPT_MCADDR, ':',
709 OPT_FILE, ':',
710 OPT_HELP,
711 OPT_VERSION,
712 OPT_VERBOSE,
713 0 };
715 /** Long options. */
716 static struct option const long_opts[] = {
717 { KEY_MCADDR, required_argument, NULL, OPT_MCADDR },
718 { KEY_FILE, required_argument, NULL, OPT_FILE },
719 { KEY_HELP, no_argument, NULL, OPT_HELP },
720 { KEY_VERSION, no_argument, NULL, OPT_VERSION },
721 { KEY_VERBOSE, no_argument, NULL, OPT_VERBOSE },
722 { NULL, 0, NULL, 0 }
723 };
725 static int vnetd_getopts(Vnetd *vnetd, int argc, char *argv[]){
726 int err = 0;
727 int key = 0;
728 int long_index = 0;
730 while(1){
731 key = getopt_long(argc, argv, short_opts, long_opts, &long_index);
732 if(key == -1) break;
733 switch(key){
734 case OPT_MCADDR: {
735 unsigned long addr;
736 err = get_inet_addr(optarg, &addr);
737 if(err) goto exit;
738 vnetd_set_mcast_addr(vnetd, addr);
739 break; }
740 case OPT_FILE:
741 err = vnetd_configure(vnetd, optarg);
742 if(err) goto exit;
743 break;
744 case OPT_HELP:
745 usage(0);
746 break;
747 case OPT_VERBOSE:
748 vnetd->verbose = true;
749 break;
750 case OPT_VERSION:
751 iprintf("> %s %s\n", PROGRAM, VERSION);
752 exit(0);
753 break;
754 default:
755 usage(EINVAL);
756 break;
757 }
758 }
759 exit:
760 return err;
761 }
763 /** Initialise vnetd params.
764 *
765 * @param vnetd vnetd
766 */
767 static int vnetd_init(Vnetd *vnetd, int argc, char *argv[]){
768 int err = 0;
770 // Use etherip-in-udp encapsulation.
771 etherip_in_udp = true;
773 *vnetd = (Vnetd){};
774 vnetd->port = htons(VARP_PORT);
775 vnetd->verbose = false;
776 vnetd->ttl = 1; // Default multicast ttl.
777 vnetd->etherip = true;
778 vnetd->udp_sock = -1;
779 vnetd->mcast_sock = -1;
780 vnetd->etherip_sock = -1;
781 vnetd_set_mcast_addr(vnetd, htonl(VARP_MCAST_ADDR));
782 vnetd->mcast_addr.sin_port = vnetd->port;
783 vnetd->unix_path = "/tmp/vnetd";
785 vnetd_getopts(vnetd, argc, argv);
787 err = get_self_addr(&vnetd->ucast_addr);
788 vnetd->ucast_addr.sin_port = vnetd->port;
789 dprintf("> mcaddr=%s\n", inet_ntoa(vnetd->mcast_addr.sin_addr));
790 dprintf("> addr =%s\n", inet_ntoa(vnetd->ucast_addr.sin_addr));
791 return err;
792 }
794 static void vnet_select(Vnetd *vnetd, SelectSet *set){
795 HashTable_for_decl(entry);
797 HashTable_for_each(entry, vnetd->vnet_table){
798 Vnet *vnet = entry->value;
799 struct net_device *dev = vnet->dev;
800 if(!dev) continue;
801 if(dev->tapfd < 0) continue;
802 SelectSet_add(set, dev->tapfd, SELECT_READ);
803 }
804 }
806 static void vnet_handle(Vnetd *vnetd, SelectSet *set){
807 HashTable_for_decl(entry);
809 HashTable_for_each(entry, vnetd->vnet_table){
810 Vnet *vnet = entry->value;
811 struct net_device *dev = vnet->dev;
812 if(!dev) continue;
813 if(dev->tapfd < 0) continue;
814 if(SelectSet_in_read(set, dev->tapfd)){
815 int n;
816 for(n = 64; n > 0; --n){
817 if(vnet_read(vnet) < 0) break;
818 }
819 }
820 }
821 }
823 static int vnetd_handle_udp(Vnetd *vnetd, struct sockaddr_in *addr, int sock){
824 int err = 0, n = 0;
825 struct sockaddr_in peer, dest;
826 socklen_t peer_n = sizeof(peer), dest_n = sizeof(dest);
827 int flags = MSG_DONTWAIT;
828 struct sk_buff *skb = NULL;
830 dest = *addr;
831 n = skb_recv_udp(sock, flags, &peer, &peer_n, &dest, &dest_n, &skb);
832 if(n < 0){
833 err = n;
834 goto exit;
835 }
836 dprintf("> Received %d bytes from=%s:%d dest=%s:%d\n",
837 n,
838 inet_ntoa(peer.sin_addr), htons(peer.sin_port),
839 inet_ntoa(dest.sin_addr), htons(dest.sin_port));
840 if(peer.sin_addr.s_addr == vnetd_intf_addr(vnetd)){
841 dprintf("> Ignoring message from self.\n");
842 goto exit;
843 }
844 if(dest.sin_addr.s_addr == vnetd_mcast_addr(vnetd)){
845 vnet_forward_send(skb);
846 }
847 err = varp_handle_message(skb);
849 exit:
850 if(skb) kfree_skb(skb);
851 return err;
852 }
854 static int vnetd_handle_etherip(Vnetd *vnetd, struct sockaddr_in *addr, int sock){
855 int err = 0, n = 0;
856 struct sockaddr_in peer, dest;
857 socklen_t peer_n = sizeof(peer), dest_n = sizeof(dest);
858 int flags = 0;
859 struct sk_buff *skb = NULL;
861 dest = *addr;
862 n = skb_recv_raw(sock, flags, &peer, &peer_n, &dest, &dest_n, &skb);
863 if(n < 0){
864 err = n;
865 goto exit;
866 }
867 dprintf("> Received %d bytes from=%s:%d dest=%s:%d\n",
868 n,
869 inet_ntoa(peer.sin_addr), htons(peer.sin_port),
870 inet_ntoa(dest.sin_addr), htons(dest.sin_port));
871 if(peer.sin_addr.s_addr == vnetd_intf_addr(vnetd)){
872 dprintf("> Ignoring message from self.\n");
873 goto exit;
874 }
875 err = etherip_protocol_recv(skb);
876 exit:
877 if(skb) kfree_skb(skb);
878 return err;
879 }
881 typedef struct ConnClient {
882 Vnetd *vnetd;
883 Parser *parser;
884 } ConnClient;
886 static int conn_handle_fn(Conn *conn, int mode){
887 int err;
888 ConnClient *client = conn->data;
889 char data[1024] = {};
890 int k;
891 int done = false;
893 k = IOStream_read(conn->in, data, sizeof(data));
894 if(k < 0){
895 err = k;
896 goto exit;
897 }
898 if(!client->parser){
899 err = -ENOSYS;
900 goto exit;
901 }
902 if((k == 0) && Parser_at_eof(client->parser)){
903 err = -EINVAL;
904 goto exit;
905 }
906 err = Parser_input(client->parser, data, k);
907 if(err < 0) goto exit;
908 while(Parser_ready(client->parser)){
909 Sxpr sxpr = Parser_get_val(client->parser);
910 err = vnet_eval(sxpr, conn->out, NULL);
911 if(err) goto exit;
912 done = true;
913 }
914 if(done || Parser_at_eof(client->parser)){
915 // Close at EOF.
916 err = -EIO;
917 }
918 exit:
919 if(err < 0){
920 Parser_free(client->parser);
921 client->parser = NULL;
922 }
923 return (err < 0 ? err : 0);
924 }
926 static int vnetd_handle_unix(Vnetd *vnetd, int sock){
927 int err;
928 ConnClient *client = NULL;
929 Conn *conn = NULL;
930 struct sockaddr_un peer = {};
931 socklen_t peer_n = sizeof(peer);
932 int peersock;
934 peersock = accept(sock, (struct sockaddr *)&peer, &peer_n);
935 if(peersock < 0){
936 perror("accept");
937 err = -errno;
938 goto exit;
939 }
940 // We want non-blocking i/o.
941 fcntl(peersock, F_SETFL, O_NONBLOCK);
942 client = ALLOCATE(ConnClient);
943 client->vnetd = vnetd;
944 client->parser = Parser_new();
945 conn = Conn_new(conn_handle_fn, client);
946 err = Conn_init(conn, peersock, SOCK_STREAM, SELECT_READ,
947 (struct sockaddr_in){});
948 if(err) goto exit;
949 vnetd->conns = ConnList_add(vnetd->conns, conn);
950 exit:
951 if(err){
952 Conn_close(conn);
953 close(peersock);
954 }
955 if(err < 0) wprintf("< err=%d\n", err);
956 return err;
957 }
959 static void vnetd_select(Vnetd *vnetd, SelectSet *set){
960 SelectSet_add(set, vnetd->unix_sock, SELECT_READ);
961 SelectSet_add(set, vnetd->udp_sock, SELECT_READ);
962 SelectSet_add(set, vnetd->mcast_sock, SELECT_READ);
963 if(vnetd->etherip_sock >= 0){
964 SelectSet_add(set, vnetd->etherip_sock, SELECT_READ);
965 }
966 vnet_select(vnetd, set);
967 ConnList_select(vnetd->conns, set);
968 }
970 static void vnetd_handle(Vnetd *vnetd, SelectSet *set){
971 if(SelectSet_in_read(set, vnetd->unix_sock)){
972 vnetd_handle_unix(vnetd, vnetd->unix_sock);
973 }
974 if(SelectSet_in_read(set, vnetd->udp_sock)){
975 int n;
977 for(n = 256; n > 0; --n){
978 if(vnetd_handle_udp(vnetd, &vnetd->udp_sock_addr, vnetd->udp_sock) < 0){
979 break;
980 }
981 }
982 }
983 if(SelectSet_in_read(set, vnetd->mcast_sock)){
984 vnetd_handle_udp(vnetd, &vnetd->mcast_sock_addr, vnetd->mcast_sock);
985 }
986 if((vnetd->etherip_sock >= 0) &&
987 SelectSet_in_read(set, vnetd->etherip_sock)){
988 vnetd_handle_etherip(vnetd, &vnetd->etherip_sock_addr, vnetd->etherip_sock);
989 }
990 vnet_handle(vnetd, set);
991 vnetd->conns = ConnList_handle(vnetd->conns, set);
992 }
994 /** Counter for timer alarms.
995 */
996 static unsigned timer_alarms = 0;
998 static int vnetd_main(Vnetd *vnetd){
999 int err = 0;
1000 SelectSet _set = {}, *set = &_set;
1001 struct timeval _timeout = {}, *timeout = &_timeout;
1003 vnetd->vnet_table = vnet_table;
1005 for( ; ; ){
1006 timeout->tv_sec = 0;
1007 timeout->tv_usec = 500000;
1008 SelectSet_zero(set);
1009 vnetd_select(vnetd, set);
1010 err = SelectSet_select(set, timeout);
1011 if(err == 0) continue;
1012 if(err < 0){
1013 switch(errno){
1014 case EINTR:
1015 if(timer_alarms){
1016 timer_alarms = 0;
1017 process_timers();
1019 continue;
1020 case EBADF:
1021 continue;
1022 default:
1023 perror("select");
1024 goto exit;
1027 vnetd_handle(vnetd, set);
1029 exit:
1030 return err;
1033 static int getsockaddr(int sock, struct sockaddr_in *addr){
1034 socklen_t addr_n = sizeof(struct sockaddr_in);
1035 return getsockname(sock, (struct sockaddr*)addr, &addr_n);
1038 static int vnetd_etherip_sock(Vnetd *vnetd){
1039 int err = 0;
1041 if(!vnetd->etherip) goto exit;
1042 err = vnetd_raw_socket(vnetd, IPPROTO_ETHERIP,
1043 (VSOCK_BROADCAST | VSOCK_MULTICAST),
1044 vnetd_mcast_addr(vnetd),
1045 &vnetd->etherip_sock);
1046 if(err < 0) goto exit;
1047 err = setsock_pktinfo(vnetd->etherip_sock, true);
1048 if(err < 0) goto exit;
1049 getsockaddr(vnetd->etherip_sock, &vnetd->etherip_sock_addr);
1050 exit:
1051 return err;
1054 static int vnetd_udp_sock(Vnetd *vnetd){
1055 int err;
1056 uint32_t mcaddr = vnetd_mcast_addr(vnetd);
1058 err = create_socket(SOCK_DGRAM, INADDR_ANY, vnetd->port,
1059 (VSOCK_BIND | VSOCK_REUSE),
1060 &vnetd->udp_sock);
1061 if(err < 0) goto exit;
1062 err = setsock_pktinfo(vnetd->udp_sock, true);
1063 if(err < 0) goto exit;
1064 getsockaddr(vnetd->udp_sock, &vnetd->udp_sock_addr);
1065 vnetd->mcast_sock_addr.sin_addr.s_addr = vnetd_intf_addr(vnetd);
1067 err = create_socket(SOCK_DGRAM, mcaddr, vnetd_mcast_port(vnetd),
1068 (VSOCK_REUSE | VSOCK_BROADCAST | VSOCK_MULTICAST),
1069 &vnetd->mcast_sock);
1070 if(err < 0) goto exit;
1071 err = setsock_pktinfo(vnetd->udp_sock, true);
1072 if(err < 0) goto exit;
1073 err = setsock_multicast(vnetd->mcast_sock, INADDR_ANY, mcaddr);
1074 if(err < 0) goto exit;
1075 err = setsock_multicast_ttl(vnetd->mcast_sock, vnetd->ttl);
1076 if(err < 0) goto exit;
1077 getsockaddr(vnetd->mcast_sock, &vnetd->mcast_sock_addr);
1078 vnetd->mcast_sock_addr.sin_addr.s_addr = mcaddr;
1080 exit:
1081 if(err < 0){
1082 close(vnetd->udp_sock);
1083 close(vnetd->mcast_sock);
1084 vnetd->udp_sock = -1;
1085 vnetd->mcast_sock = -1;
1087 return err;
1090 static int vnetd_raw_sock(Vnetd *vnetd){
1091 int err;
1093 err = vnetd_raw_socket(vnetd, IPPROTO_RAW,
1094 (VSOCK_BROADCAST),
1095 vnetd_mcast_addr(vnetd),
1096 &vnetd->raw_sock);
1097 if(err){
1098 close(vnetd->raw_sock);
1099 vnetd->raw_sock = -1;
1101 return err;
1104 static int vnetd_unix_sock(Vnetd *vnetd){
1105 int err = 0;
1106 struct sockaddr_un addr = { .sun_family = AF_UNIX };
1107 socklen_t addr_n;
1109 vnetd->unix_sock = socket(addr.sun_family, SOCK_STREAM, 0);
1110 if(vnetd->unix_sock < 0){
1111 err = -errno;
1112 perror("unix socket");
1113 goto exit;
1115 unlink(vnetd->unix_path);
1116 strcpy(addr.sun_path, vnetd->unix_path);
1117 addr_n = sizeof(addr) - sizeof(addr.sun_path) + strlen(vnetd->unix_path) + 1;
1118 err = bind(vnetd->unix_sock, (struct sockaddr *)&addr, addr_n);
1119 if(err < 0){
1120 err = -errno;
1121 perror("unix bind");
1122 goto exit;
1124 err = listen(vnetd->unix_sock, 5);
1125 if(err < 0){
1126 err = -errno;
1127 perror("unix listen");
1129 exit:
1130 return err;
1133 /** Handle SIGPIPE.
1135 * @param code signal code
1136 * @param info signal info
1137 * @param data
1138 */
1139 static void sigaction_SIGPIPE(int code, siginfo_t *info, void *data){
1140 dprintf("> SIGPIPE\n");
1143 /** Handle SIGALRM.
1145 * @param code signal code
1146 * @param info signal info
1147 * @param data
1148 */
1149 static void sigaction_SIGALRM(int code, siginfo_t *info, void *data){
1150 timer_alarms++;
1153 /** Type for signal handling functions. */
1154 typedef void SignalAction(int code, siginfo_t *info, void *data);
1156 /** Install a handler for a signal.
1158 * @param signum signal
1159 * @param action handler
1160 * @return 0 on success, error code otherwise
1161 */
1162 static int catch_signal(int signum, SignalAction *action){
1163 int err = 0;
1164 struct sigaction sig = {};
1165 dprintf(">\n");
1166 sig.sa_sigaction = action;
1167 sig.sa_flags = SA_SIGINFO;
1168 err = sigaction(signum, &sig, NULL);
1169 if(err){
1170 err = -errno;
1171 perror("sigaction");
1173 return err;
1176 int main(int argc, char *argv[]){
1177 int err = 0;
1179 err = tunnel_module_init();
1180 if(err < 0) goto exit;
1181 err = vnet_init();
1182 if(err < 0) goto exit;
1183 err = vnetd_init(vnetd, argc, argv);
1184 if(err < 0) goto exit;
1185 err = catch_signal(SIGPIPE, sigaction_SIGPIPE);
1186 if(err < 0) goto exit;
1187 err = catch_signal(SIGALRM, sigaction_SIGALRM);
1188 if(err < 0) goto exit;
1189 err = vnetd_etherip_sock(vnetd);
1190 if(err < 0) goto exit;
1191 err = vnetd_udp_sock(vnetd);
1192 if(err < 0) goto exit;
1193 err = vnetd_raw_sock(vnetd);
1194 if(err < 0) goto exit;
1195 err = vnetd_unix_sock(vnetd);
1196 if(err < 0) goto exit;
1197 err = vnetd_main(vnetd);
1198 exit:
1199 return (err ? 1 : 0);