vnetd.c

来自「xen虚拟机源代码安装包」· C语言 代码 · 共 1,201 行 · 第 1/3 页

/*
 * Copyright (C) 2005, 2006 Mike Wray <[e-mail]>.
 *
 * This library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation; either version 2.1 of the
 * License, or  (at your option) any later version. This library is 
 * distributed in the  hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
 */
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <getopt.h>
#include <errno.h>
#include <time.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>

#include <signal.h>
#include <sys/wait.h>
#include <sys/select.h>

#include <asm/types.h> // For __u32 etc.

#include <linux/ip.h>  // For struct iphdr.
#include <linux/udp.h> // For struct udphdr.

#include <linux/if.h>
#include <linux/if_ether.h>
#include <linux/if_tun.h> 

#include "sys_kernel.h"
#include "skbuff.h"
#include "spinlock.h"

#include "allocate.h"

#include "file_stream.h"
#include "string_stream.h"
#include "socket_stream.h"
#include "sys_net.h"

#include "enum.h"
#include "sxpr.h"
#include "sxpr_parser.h"

#include "connection.h"
#include "select.h"
#include "timer.h"

#include "if_etherip.h"
#include "if_varp.h"
#include "varp.h"
#include "vnet.h"
#include "vnet_dev.h"
#include "vnet_eval.h"
#include "vnet_forward.h"
#include "tunnel.h"
#include "etherip.h"
#include "sxpr_util.h"

#define MODULE_NAME "VNETD"
#define DEBUG 1
#undef DEBUG
#include "debug.h"

#define PROGRAM "vnetd"
#define VERSION "1.0"

typedef struct Vnetd {
    unsigned long port;
    int ttl;
    int verbose;
    int etherip;

    int udp_sock;
    struct sockaddr_in udp_sock_addr;
    int mcast_sock;
    struct sockaddr_in mcast_sock_addr;
    int etherip_sock;
    struct sockaddr_in etherip_sock_addr;
    int unix_sock;
    char *unix_path;

    int raw_sock;

    struct sockaddr_in ucast_addr;
    struct sockaddr_in mcast_addr;

    HashTable *vnet_table;

    ConnList *conns;

} Vnetd;

Vnetd _vnetd = {}, *vnetd = &_vnetd;

uint32_t vnetd_intf_addr(Vnetd *vnetd){
    return vnetd->ucast_addr.sin_addr.s_addr;
}

uint32_t vnetd_mcast_addr(Vnetd *vnetd){
    return vnetd->mcast_addr.sin_addr.s_addr;
}

void vnetd_set_mcast_addr(Vnetd *vnetd, uint32_t addr){
    varp_mcast_addr = addr;
    vnetd->mcast_addr.sin_addr.s_addr = addr;
}

uint16_t vnetd_mcast_port(Vnetd *vnetd){
    return vnetd->mcast_addr.sin_port;
}

uint32_t vnetd_addr(void){
    return vnetd_intf_addr(vnetd);
}

/** Open tap device.
 */
int tap_open(struct net_device *dev){
    int err;
    /* IFF_TAP      : Ethernet tap device.
     * IFF_NO_PI    : Don't add packet info struct when reading.
     * IFF_ONE_QUEUE: Drop packets when the dev queue is full. The driver uses
     *                the queue size from the device, which defaults to 1000 for etherdev.
     *                If not set the driver stops the device queue when it goes over
     *                TUN_READQ_SIZE, which is 10. Broken - makes the device stall
     *                under load.
     */
    struct ifreq ifr = { };
    ifr.ifr_flags = (IFF_TAP | IFF_NO_PI | IFF_ONE_QUEUE);

    dprintf(">\n");
    dev->tapfd = open("/dev/net/tun", O_RDWR);
    if(dev->tapfd < 0){
        err = -errno;
        perror("open");
        goto exit;
    }
    strcpy(ifr.ifr_name, dev->name);
    err = ioctl(dev->tapfd, TUNSETIFF, (void *)&ifr);
    if(err < 0){
        err = -errno;
        perror("ioctl");
        goto exit;
    }
    strcpy(dev->name, ifr.ifr_name);
    dprintf("> dev=%s\n", dev->name);
    // Make it non-blocking.
    fcntl(dev->tapfd, F_SETFL, O_NONBLOCK);

  exit:
    if(err && (dev->tapfd >= 0)){
        close(dev->tapfd);
        dev->tapfd = -1;
    }
    dprintf("< err=%d\n", err);
    return err;
}

/** Close tap device.
 */
int tap_close(struct net_device *dev){
    int err = 0;

    if(dev->tapfd >= 0){
        err = close(dev->tapfd);
        dev->tapfd = -1;
    }
    return err;
}

/** Open vnif tap device for a vnet.
 */
int vnet_dev_add(struct Vnet *vnet){
    int err = 0;
    struct net_device *dev = ALLOCATE(struct net_device);
    strcpy(dev->name, vnet->device);
    err = tap_open(dev);
    if(err){
        wprintf("> Unable to open tap device.\n"
                "The tun module must be loaded and\n"
                "the vnet kernel module must not be loaded.\n");
        deallocate(dev);
        goto exit;
    }
    vnet->dev = dev;
  exit:
    return err;
}

/** Close vnif tap device for a vnet.
 */
void vnet_dev_remove(struct Vnet *vnet){
    if(vnet->dev){
        tap_close(vnet->dev);
        deallocate(vnet->dev);
        vnet->dev = NULL;
    }
}

/** Receive decapsulated ethernet packet on skb->dev.
 * Always succeeds. The skb must not be referred to after
 * this is called.
 */
int netif_rx(struct sk_buff *skb){
    int err = 0, n, k;
    struct net_device *dev = skb->dev;
    if(!dev){
        err = -ENODEV;
        goto exit;
    }
    n = skb->tail - skb->mac.raw;
    k = write(dev->tapfd, skb->mac.raw, n);
    if(k < 0){
        err = -errno;
        perror("write");
    } else if(k < n){
        //todo: What?
    }
  exit:
    kfree_skb(skb);
    return err;
}

static const int SKB_SIZE = 1700;

struct sk_buff *skb_new(void){
    return alloc_skb(SKB_SIZE, GFP_ATOMIC);
}

/** Receive a packet and fill-in source and destination addresses.
 * Just like recvfrom() but adds the destination address.
 * The socket must have the IP_PKTINFO option set so that the
 * destination address information is available.
 *
 * @param sock socket
 * @param buf receive buffer
 * @param len size of buffer
 * @param flags receive flags
 * @param from source address
 * @param fromlen size of source address
 * @param dest destination address
 * @param destlen size of destination address
 * @return number of bytes read on success, negative otherwise
 */
int recvfromdest(int sock, void *buf, size_t len, int flags,
                 struct sockaddr *from, socklen_t *fromlen,
                 struct sockaddr *dest, socklen_t *destlen){
    int ret = 0;
    struct iovec iov;
    struct msghdr msg;
    struct cmsghdr *cmsg;
    char cbuf[1024];
    struct in_pktinfo *info;
    struct sockaddr_in *dest_in = (struct sockaddr_in *)dest;

    //dest_in->sin_family = AF_INET;
    //dest_in->sin_port   = 0;
    getsockname(sock, dest, destlen);

    iov.iov_base       = buf;
    iov.iov_len        = len;
    msg.msg_name       = from;
    msg.msg_namelen    = *fromlen;
    msg.msg_iov        = &iov;
    msg.msg_iovlen     = 1;
    msg.msg_control    = cbuf;
    msg.msg_controllen = sizeof(cbuf);
    
    ret = recvmsg(sock, &msg, flags);
    if(ret < 0) goto exit;
    *fromlen = msg.msg_namelen;
    
    for(cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)){
        if((cmsg->cmsg_level == SOL_IP) && (cmsg->cmsg_type == IP_PKTINFO)){
            info = (void*)CMSG_DATA(cmsg);
            dest_in->sin_addr = info->ipi_addr;
            break;
        }
    }

  exit:
    return ret;
}

/** Read an skb from a udp socket and fill in its headers.
 */
int skb_recv_udp(int sock, int flags,
                 struct sockaddr_in *peer, socklen_t *peer_n,
                 struct sockaddr_in *dest, socklen_t *dest_n,
                 struct sk_buff **pskb){
    int err = 0, n;
    struct sk_buff *skb = skb_new();

    skb->mac.raw = skb->data;
    skb_reserve(skb, ETH_HLEN);
    skb->nh.raw = skb->data;
    skb_reserve(skb, sizeof(struct iphdr));
    // Rcvr wants skb->data pointing at the udphdr.
    skb->h.raw = skb_put(skb, sizeof(struct udphdr));
    n = recvfromdest(sock, skb->tail, skb_tailroom(skb), flags,
                     (struct sockaddr *)peer, peer_n,
                     (struct sockaddr *)dest, dest_n);
    if(n < 0){
        err = -errno;
        //perror("recvfrom");
        goto exit;
    }
    dprintf("> peer=%s:%d\n", inet_ntoa(peer->sin_addr), ntohs(peer->sin_port));
    dprintf("> dest=%s:%d\n", inet_ntoa(dest->sin_addr), ntohs(dest->sin_port));
    skb_put(skb, n);
    skb->protocol = skb->nh.iph->protocol = IPPROTO_UDP;
    skb->nh.iph->saddr = peer->sin_addr.s_addr;
    skb->h.uh->source  = peer->sin_port;
    skb->nh.iph->daddr = dest->sin_addr.s_addr;
    skb->h.uh->dest    = dest->sin_port;
  exit:
    if(err < 0){
        kfree_skb(skb);
        *pskb = NULL;
    } else {
        *pskb = skb;
    }
    return (err < 0 ? err : n);
}

/** Read an skb fom a raw socket and fill in its headers.
 */
int skb_recv_raw(int sock, int flags,
                 struct sockaddr_in *peer, socklen_t *peer_n,
                 struct sockaddr_in *dest, socklen_t *dest_n,
                 struct sk_buff **pskb){
    int err = 0, n;
    struct sk_buff *skb = skb_new();

    skb->mac.raw = skb->data;
    skb_reserve(skb, ETH_HLEN);
    skb->nh.raw = skb->data;
    skb_reserve(skb, sizeof(struct iphdr));
    // Rcvr wants skb->data pointing after ip hdr, at raw protocol hdr.
    n = recvfromdest(sock, skb->tail, skb_tailroom(skb), flags,
                     (struct sockaddr *)peer, peer_n,
                     (struct sockaddr *)dest, dest_n);
    if(n < 0){
        err = -errno;
        //perror("recvfrom");
        goto exit;
    }
    skb_put(skb, n);
    // On a raw socket the port in the address is the protocol.
    skb->protocol = skb->nh.iph->protocol = peer->sin_port;
    skb->nh.iph->saddr = peer->sin_addr.s_addr;
    skb->nh.iph->daddr = dest->sin_addr.s_addr;
  exit:
    if(err < 0){
        kfree_skb(skb);
        *pskb = NULL;
    } else {
        *pskb = skb;
    }
    return (err < 0 ? err : n);
}

/** Read an skb from a file descriptor.
 * Used for skbs coming to us from the tap device.
 * The skb content is an ethernet frame.
 */
int skb_read(int fd, struct sk_buff **pskb){
    int err = 0, n;
    struct sk_buff *skb = skb_new();

    // Reserve space for the headers we will add.
    skb_reserve(skb, 100);
    // Rcvr will want ethhdr on the skb.
    skb->mac.raw = skb->tail;
    n = read(fd, skb->tail, skb_tailroom(skb));
    if(n < 0){
        err = -errno;
        //perror("read");
        goto exit;
    }
    skb_put(skb, n);
  exit:
    if(err < 0){