source: trunk/packages/xen-3.1/xen-3.1/tools/vnet/vnetd/vnetd.c @ 34

Last change on this file since 34 was 34, checked in by hartmans, 18 years ago

Add xen and xen-common

File size: 31.8 KB
Line 
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>
33
34#include <signal.h>
35#include <sys/wait.h>
36#include <sys/select.h>
37
38#include <asm/types.h> // For __u32 etc.
39
40#include <linux/ip.h>  // For struct iphdr.
41#include <linux/udp.h> // For struct udphdr.
42
43#include <linux/if.h>
44#include <linux/if_ether.h>
45#include <linux/if_tun.h>
46
47#include "sys_kernel.h"
48#include "skbuff.h"
49#include "spinlock.h"
50
51#include "allocate.h"
52
53#include "file_stream.h"
54#include "string_stream.h"
55#include "socket_stream.h"
56#include "sys_net.h"
57
58#include "enum.h"
59#include "sxpr.h"
60#include "sxpr_parser.h"
61
62#include "connection.h"
63#include "select.h"
64#include "timer.h"
65
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"
76
77#define MODULE_NAME "VNETD"
78#define DEBUG 1
79#undef DEBUG
80#include "debug.h"
81
82#define PROGRAM "vnetd"
83#define VERSION "1.0"
84
85typedef struct Vnetd {
86    unsigned long port;
87    int ttl;
88    int verbose;
89    int etherip;
90
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;
99
100    int raw_sock;
101
102    struct sockaddr_in ucast_addr;
103    struct sockaddr_in mcast_addr;
104
105    HashTable *vnet_table;
106
107    ConnList *conns;
108
109} Vnetd;
110
111Vnetd _vnetd = {}, *vnetd = &_vnetd;
112
113uint32_t vnetd_intf_addr(Vnetd *vnetd){
114    return vnetd->ucast_addr.sin_addr.s_addr;
115}
116
117uint32_t vnetd_mcast_addr(Vnetd *vnetd){
118    return vnetd->mcast_addr.sin_addr.s_addr;
119}
120
121void vnetd_set_mcast_addr(Vnetd *vnetd, uint32_t addr){
122    varp_mcast_addr = addr;
123    vnetd->mcast_addr.sin_addr.s_addr = addr;
124}
125
126uint16_t vnetd_mcast_port(Vnetd *vnetd){
127    return vnetd->mcast_addr.sin_port;
128}
129
130uint32_t vnetd_addr(void){
131    return vnetd_intf_addr(vnetd);
132}
133
134/** Open tap device.
135 */
136int 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);
148
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);
167
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}
176
177/** Close tap device.
178 */
179int tap_close(struct net_device *dev){
180    int err = 0;
181
182    if(dev->tapfd >= 0){
183        err = close(dev->tapfd);
184        dev->tapfd = -1;
185    }
186    return err;
187}
188
189/** Open vnif tap device for a vnet.
190 */
191int 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}
207
208/** Close vnif tap device for a vnet.
209 */
210void 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}
217
218/** Receive decapsulated ethernet packet on skb->dev.
219 * Always succeeds. The skb must not be referred to after
220 * this is called.
221 */
222int 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}
241
242static const int SKB_SIZE = 1700;
243
244struct sk_buff *skb_new(void){
245    return alloc_skb(SKB_SIZE, GFP_ATOMIC);
246}
247
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 */
263int 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;
273
274    //dest_in->sin_family = AF_INET;
275    //dest_in->sin_port   = 0;
276    getsockname(sock, dest, destlen);
277
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);
286   
287    ret = recvmsg(sock, &msg, flags);
288    if(ret < 0) goto exit;
289    *fromlen = msg.msg_namelen;
290   
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    }
298
299  exit:
300    return ret;
301}
302
303/** Read an skb from a udp socket and fill in its headers.
304 */
305int 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();
311
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}
343
344/** Read an skb fom a raw socket and fill in its headers.
345 */
346int 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();
352
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}
380
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 */
385int skb_read(int fd, struct sk_buff **pskb){
386    int err = 0, n;
387    struct sk_buff *skb = skb_new();
388
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}
409
410/** Read an skb from the tap device for a vnet and send it.
411 */
412int vnet_read(Vnet *vnet){
413    int err;
414    struct sk_buff *skb = NULL;
415
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}
423
424/** Transmit an skb to the network.
425 */
426int _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;
432
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    }
471
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);
477
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}
493
494int varp_open(uint32_t mcaddr, uint16_t port){
495    return 0;
496}
497
498void varp_close(void){
499}
500
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 */
508int vnetd_raw_socket(Vnetd *vnetd, int protocol, int flags,
509                     uint32_t mcaddr, int *sock){
510    int err;
511    int bcast = (flags & VSOCK_BROADCAST);
512
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}
531
532int 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;
537
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}
556
557int get_intf_address(unsigned long *addr){
558    int err = 0;
559    char *devs[] = { "xen-br0", "eth0", "eth1", "eth2", NULL };
560    char **dev;
561
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}
570
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 */
577int get_self_addr(struct sockaddr_in *addr){
578    int err = 0;
579    char hostname[1024] = {};
580    unsigned long saddr;
581 
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}
600
601static 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;
608
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}
618
619static 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;
624
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}
644
645static 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 } };
654
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}
667
668#define OPT_MCADDR   'a'
669#define KEY_MCADDR   "varp_mcaddr"
670#define DOC_MCADDR   "<addr>\n\t VARP multicast address"
671
672#define OPT_FILE     'f'
673#define KEY_FILE     "file"
674#define DOC_FILE     "<file>\n\t Configuration file to load"
675
676#define OPT_HELP     'h'
677#define KEY_HELP     "help"
678#define DOC_HELP     "\n\tprint help"
679
680#define OPT_VERSION  'v'
681#define KEY_VERSION  "version"
682#define DOC_VERSION  "\n\tprint version"
683
684#define OPT_VERBOSE  'V'
685#define KEY_VERBOSE  "verbose"
686#define DOC_VERBOSE  "\n\tverbose flag"
687
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 */
694static void usage(int err){
695    FILE *out = (err ? stderr : stdout);
696
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}
705
706/** Short options. Options followed by ':' take an argument. */
707static char *short_opts = (char[]){
708    OPT_MCADDR,   ':',
709    OPT_FILE,     ':',
710    OPT_HELP,
711    OPT_VERSION,
712    OPT_VERBOSE,
713    0 };
714
715/** Long options. */
716static 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};
724
725static int vnetd_getopts(Vnetd *vnetd, int argc, char *argv[]){
726    int err = 0;
727    int key = 0;
728    int long_index = 0;
729
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}
762
763/** Initialise vnetd params.
764 *
765 * @param vnetd vnetd
766 */
767static int vnetd_init(Vnetd *vnetd, int argc, char *argv[]){
768    int err = 0;
769
770    // Use etherip-in-udp encapsulation.
771    etherip_in_udp = true;
772
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";
784
785    vnetd_getopts(vnetd, argc, argv);
786   
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}
793
794static void vnet_select(Vnetd *vnetd, SelectSet *set){
795    HashTable_for_decl(entry);
796
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}
805
806static void vnet_handle(Vnetd *vnetd, SelectSet *set){
807    HashTable_for_decl(entry);
808
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}
822
823static 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;
829
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);
848
849  exit:
850    if(skb) kfree_skb(skb);
851    return err;
852}
853
854static 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;
860
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}
880
881typedef struct ConnClient {
882    Vnetd *vnetd;
883    Parser *parser;
884} ConnClient;
885
886static 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;
892
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}
925
926static 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;
933
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}
958
959static 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}
969
970static 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;
976
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}
993
994/** Counter for timer alarms.
995 */
996static unsigned timer_alarms = 0;
997
998static int vnetd_main(Vnetd *vnetd){
999    int err = 0;
1000    SelectSet _set = {}, *set = &_set;
1001    struct timeval _timeout = {}, *timeout = &_timeout;
1002
1003    vnetd->vnet_table = vnet_table;
1004
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();
1018                }
1019                continue;
1020            case EBADF:
1021                continue;
1022            default:
1023                perror("select");
1024                goto exit;
1025            }
1026        }
1027        vnetd_handle(vnetd, set);
1028    }
1029  exit:
1030    return err;
1031}
1032
1033static 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);
1036}
1037
1038static int vnetd_etherip_sock(Vnetd *vnetd){
1039    int err = 0;
1040
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;
1052}
1053
1054static int vnetd_udp_sock(Vnetd *vnetd){
1055    int err;
1056    uint32_t mcaddr = vnetd_mcast_addr(vnetd);
1057
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);
1066
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;
1079
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;
1086    }
1087    return err;
1088}
1089
1090static int vnetd_raw_sock(Vnetd *vnetd){
1091    int err;
1092
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;
1100    }
1101    return err;
1102}
1103
1104static int vnetd_unix_sock(Vnetd *vnetd){
1105    int err = 0;
1106    struct sockaddr_un addr = { .sun_family = AF_UNIX };
1107    socklen_t addr_n;
1108   
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;
1114    }
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;
1123    }
1124    err = listen(vnetd->unix_sock, 5);
1125    if(err < 0){
1126        err = -errno;
1127        perror("unix listen");
1128    }
1129  exit:
1130    return err;
1131}
1132   
1133/** Handle SIGPIPE.
1134 *
1135 * @param code signal code
1136 * @param info signal info
1137 * @param data
1138 */
1139static void sigaction_SIGPIPE(int code, siginfo_t *info, void *data){
1140    dprintf("> SIGPIPE\n");
1141}
1142
1143/** Handle SIGALRM.
1144 *
1145 * @param code signal code
1146 * @param info signal info
1147 * @param data
1148 */
1149static void sigaction_SIGALRM(int code, siginfo_t *info, void *data){
1150    timer_alarms++;
1151}
1152
1153/** Type for signal handling functions. */
1154typedef void SignalAction(int code, siginfo_t *info, void *data);
1155
1156/** Install a handler for a signal.
1157 *
1158 * @param signum signal
1159 * @param action handler
1160 * @return 0 on success, error code otherwise
1161 */
1162static 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");
1172    }
1173    return err;
1174}   
1175
1176int main(int argc, char *argv[]){
1177    int err = 0;
1178
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);
1198exit:
1199    return (err ? 1 : 0);
1200}
Note: See TracBrowser for help on using the repository browser.