tools/kvm/net/uip/udp.c

   1 #include "kvm/uip.h"
   2
   3 #include <linux/virtio_net.h>
   4 #include <linux/kernel.h>
   5 #include <linux/list.h>
   6 #include <sys/socket.h>
   7 #include <sys/epoll.h>
   8 #include <fcntl.h>
   9
  10 #define UIP_UDP_MAX_EVENTS 1000
  11
  12 static struct uip_udp_socket *uip_udp_socket_find(struct uip_tx_arg *arg, u32 sip, u32 dip, u16 sport, u16 dport)
  13 {
  14         struct list_head *sk_head;
  15         struct uip_udp_socket *sk;
  16         pthread_mutex_t *sk_lock;
  17         struct epoll_event ev;
  18         int flags;
  19         int ret;
  20
  21         sk_head = &arg->info->udp_socket_head;
  22         sk_lock = &arg->info->udp_socket_lock;
  23
  24         /*
  25          * Find existing sk
  26          */
  27         mutex_lock(sk_lock);
  28         list_for_each_entry(sk, sk_head, list) {
  29                 if (sk->sip == sip && sk->dip == dip && sk->sport == sport && sk->dport == dport) {
  30                         mutex_unlock(sk_lock);
  31                         return sk;
  32                 }
  33         }
  34         mutex_unlock(sk_lock);
  35
  36         /*
  37          * Allocate new one
  38          */
  39         sk = malloc(sizeof(*sk));
  40         memset(sk, 0, sizeof(*sk));
  41
  42         sk->lock = sk_lock;
  43
  44         sk->fd = socket(AF_INET, SOCK_DGRAM, 0);
  45         if (sk->fd < 0)
  46                 goto out;
  47
  48         /*
  49          * Set non-blocking
  50          */
  51         flags = fcntl(sk->fd, F_GETFL, 0);
  52         flags |= O_NONBLOCK;
  53         fcntl(sk->fd, F_SETFL, flags);
  54
  55         /*
  56          * Add sk->fd to epoll_wait
  57          */
  58         ev.events       = EPOLLIN;
  59         ev.data.fd      = sk->fd;
  60         ev.data.ptr     = sk;
  61         if (arg->info->udp_epollfd <= 0)
  62                 arg->info->udp_epollfd = epoll_create(UIP_UDP_MAX_EVENTS);
  63         ret = epoll_ctl(arg->info->udp_epollfd, EPOLL_CTL_ADD, sk->fd, &ev);
  64         if (ret == -1)
  65                 pr_warning("epoll_ctl error");
  66
  67         sk->addr.sin_family      = AF_INET;
  68         sk->addr.sin_addr.s_addr = dip;
  69         sk->addr.sin_port        = dport;
  70
  71         sk->sip                  = sip;
  72         sk->dip                  = dip;
  73         sk->sport                = sport;
  74         sk->dport                = dport;
  75
  76         mutex_lock(sk_lock);
  77         list_add_tail(&sk->list, sk_head);
  78         mutex_unlock(sk_lock);
  79
  80         return sk;
  81
  82 out:
  83         free(sk);
  84         return NULL;
  85 }
  86
  87 static int uip_udp_socket_send(struct uip_udp_socket *sk, struct uip_udp *udp)
  88 {
  89         int len;
  90         int ret;
  91
  92         len = ntohs(udp->len) - uip_udp_hdrlen(udp);
  93
  94         ret = sendto(sk->fd, udp->payload, len, 0, (struct sockaddr *)&sk->addr, sizeof(sk->addr));
  95         if (ret != len)
  96                 return -1;
  97
  98         return 0;
  99 }
 100
 101 int uip_udp_make_pkg(struct uip_info *info, struct uip_udp_socket *sk, struct uip_buf *buf, u8* payload, int payload_len)
 102 {
 103         struct uip_eth *eth2;
 104         struct uip_udp *udp2;
 105         struct uip_ip *ip2;
 106
 107         /*
 108          * Cook a ethernet frame
 109          */
 110         udp2            = (struct uip_udp *)(buf->eth);
 111         eth2            = (struct uip_eth *)buf->eth;
 112         ip2             = (struct uip_ip *)(buf->eth);
 113
 114         eth2->src       = info->host_mac;
 115         eth2->dst       = info->guest_mac;
 116         eth2->type      = htons(UIP_ETH_P_IP);
 117
 118         ip2->vhl        = UIP_IP_VER_4 | UIP_IP_HDR_LEN;
 119         ip2->tos        = 0;
 120         ip2->id         = 0;
 121         ip2->flgfrag    = 0;
 122         ip2->ttl        = UIP_IP_TTL;
 123         ip2->proto      = UIP_IP_P_UDP;
 124         ip2->csum       = 0;
 125
 126         ip2->sip        = sk->dip;
 127         ip2->dip        = sk->sip;
 128         udp2->sport     = sk->dport;
 129         udp2->dport     = sk->sport;
 130
 131         udp2->len       = htons(payload_len + uip_udp_hdrlen(udp2));
 132         udp2->csum      = 0;
 133
 134         if (payload)
 135                 memcpy(udp2->payload, payload, payload_len);
 136
 137         ip2->len        = udp2->len + htons(uip_ip_hdrlen(ip2));
 138         ip2->csum       = uip_csum_ip(ip2);
 139         udp2->csum      = uip_csum_udp(udp2);
 140
 141         /*
 142          * virtio_net_hdr
 143          */
 144         buf->vnet_len   = sizeof(struct virtio_net_hdr);
 145         memset(buf->vnet, 0, buf->vnet_len);
 146
 147         buf->eth_len    = ntohs(ip2->len) + uip_eth_hdrlen(&ip2->eth);
 148
 149         return 0;
 150 }
 151
 152 static void *uip_udp_socket_thread(void *p)
 153 {
 154         struct epoll_event events[UIP_UDP_MAX_EVENTS];
 155         struct uip_udp_socket *sk;
 156         struct uip_info *info;
 157         struct uip_buf *buf;
 158         int payload_len;
 159         u8 *payload;
 160         int nfds;
 161         int i;
 162
 163         info = p;
 164
 165         do {
 166                 payload = malloc(UIP_MAX_UDP_PAYLOAD);
 167         } while (!payload);
 168
 169         while (1) {
 170                 nfds = epoll_wait(info->udp_epollfd, events, UIP_UDP_MAX_EVENTS, -1);
 171
 172                 if (nfds == -1)
 173                         continue;
 174
 175                 for (i = 0; i < nfds; i++) {
 176
 177                         sk = events[i].data.ptr;
 178                         payload_len = recvfrom(sk->fd, payload, UIP_MAX_UDP_PAYLOAD, 0, NULL, NULL);
 179                         if (payload_len < 0)
 180                                 continue;
 181
 182                         /*
 183                          * Get free buffer to send data to guest
 184                          */
 185                         buf             = uip_buf_get_free(info);
 186
 187                         uip_udp_make_pkg(info, sk, buf, payload, payload_len);
 188
 189                         /*
 190                          * Send data received from socket to guest
 191                          */
 192                         uip_buf_set_used(info, buf);
 193                 }
 194         }
 195
 196         free(payload);
 197         pthread_exit(NULL);
 198         return NULL;
 199 }
 200
 201 int uip_tx_do_ipv4_udp(struct uip_tx_arg *arg)
 202 {
 203         struct uip_udp_socket *sk;
 204         struct uip_info *info;
 205         struct uip_udp *udp;
 206         struct uip_ip *ip;
 207         int ret;
 208
 209         udp     = (struct uip_udp *)(arg->eth);
 210         ip      = (struct uip_ip *)(arg->eth);
 211         info    = arg->info;
 212
 213         if (uip_udp_is_dhcp(udp)) {
 214                 uip_tx_do_ipv4_udp_dhcp(arg);
 215                 return 0;
 216         }
 217
 218         /*
 219          * Find socket we have allocated before, otherwise allocate one
 220          */
 221         sk = uip_udp_socket_find(arg, ip->sip, ip->dip, udp->sport, udp->dport);
 222         if (!sk)
 223                 return -1;
 224
 225         /*
 226          * Send out UDP data to remote host
 227          */
 228         ret = uip_udp_socket_send(sk, udp);
 229         if (ret)
 230                 return -1;
 231
 232         if (!info->udp_thread)
 233                 pthread_create(&info->udp_thread, NULL, uip_udp_socket_thread, (void *)info);
 234
 235         return 0;
 236 }