kvm tools: Add UDP support for uip

- Implement uip_tx_do_ipv4_udp() to send UDP package to remote host.

- Implement uip_udp_socket_thread() to receive UDP package from
  remote host.

Signed-off-by: Asias He <asias.hejun@gmail.com>
Signed-off-by: Pekka Enberg <penberg@kernel.org>

diff --git a/tools/kvm/Makefile b/tools/kvm/Makefile
index 9ad09a43b2968e8dac4ab029bc44946ad1f8de2d..f92cfdb9da8df8114f856bb568b7494ee0477adb 100644 (file)
--- a/tools/kvm/Makefile
+++ b/tools/kvm/Makefile
@@ -48,6 +48,7 @@ OBJS  += irq.o
 OBJS   += uip/arp.o
 OBJS   += uip/icmp.o
 OBJS   += uip/ipv4.o
+OBJS   += uip/udp.o
 OBJS   += uip/buf.o
 OBJS   += uip/csum.o
 OBJS   += kvm-cmd.o

diff --git a/tools/kvm/include/kvm/uip.h b/tools/kvm/include/kvm/uip.h
index e2ad66aaf99f6ae97faa3c3a3a94d7d98d7493e9..ccc16985c981a3788afa5979c324b8ce1e31bd51 100644 (file)
--- a/tools/kvm/include/kvm/uip.h
+++ b/tools/kvm/include/kvm/uip.h
@@ -11,6 +11,20 @@
 #define UIP_BUF_STATUS_INUSE   1
 #define UIP_BUF_STATUS_USED    2
 
+#define UIP_ETH_P_IP           0X0800
+
+#define UIP_IP_VER_4           0X40
+#define UIP_IP_HDR_LEN         0X05
+#define UIP_IP_TTL             0X40
+#define UIP_IP_P_UDP           0X11
+
+/*
+ * IP package maxium len == 64 KBytes
+ * IP header == 20 Bytes
+ * UDP header == 8 Bytes
+ */
+#define UIP_MAX_UDP_PAYLOAD    (64*1024 - 20 -  8 - 1)
+
 struct uip_eth_addr {
        u8 addr[6];
 };
@@ -157,6 +171,7 @@ static inline u16 uip_eth_hdrlen(struct uip_eth *eth)
 }
 
 int uip_tx_do_ipv4_icmp(struct uip_tx_arg *arg);
+int uip_tx_do_ipv4_udp(struct uip_tx_arg *arg);
 int uip_tx_do_ipv4(struct uip_tx_arg *arg);
 int uip_tx_do_arp(struct uip_tx_arg *arg);
 

diff --git a/tools/kvm/uip/ipv4.c b/tools/kvm/uip/ipv4.c
index 6175992f4d5e51139aacc309e7d499526b191455..75058cdc8c4a09c6ce56660b5ec71ec40f0df0c1 100644 (file)
--- a/tools/kvm/uip/ipv4.c
+++ b/tools/kvm/uip/ipv4.c
@@ -15,6 +15,9 @@ int uip_tx_do_ipv4(struct uip_tx_arg *arg)
        case 0x01: /* ICMP */
                uip_tx_do_ipv4_icmp(arg);
                break;
+       case 0x11: /* UDP */
+               uip_tx_do_ipv4_udp(arg);
+               break;
        default:
                break;
        }

diff --git a/tools/kvm/uip/udp.c b/tools/kvm/uip/udp.c
new file mode 100644 (file)
index 0000000..5f9d7a4
--- /dev/null
+++ b/tools/kvm/uip/udp.c
@@ -0,0 +1,221 @@
+#include "kvm/uip.h"
+
+#include <linux/virtio_net.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <sys/socket.h>
+#include <sys/epoll.h>
+#include <fcntl.h>
+
+#define UIP_UDP_MAX_EVENTS 1000
+
+static struct uip_udp_socket *uip_udp_socket_find(struct uip_tx_arg *arg, u32 sip, u32 dip, u16 sport, u16 dport)
+{
+       struct list_head *sk_head;
+       struct uip_udp_socket *sk;
+       pthread_mutex_t *sk_lock;
+       struct epoll_event ev;
+       int flags;
+       int ret;
+
+       sk_head = &arg->info->udp_socket_head;
+       sk_lock = &arg->info->udp_socket_lock;
+
+       /*
+        * Find existing sk
+        */
+       mutex_lock(sk_lock);
+       list_for_each_entry(sk, sk_head, list) {
+               if (sk->sip == sip && sk->dip == dip && sk->sport == sport && sk->dport == dport) {
+                       mutex_unlock(sk_lock);
+                       return sk;
+               }
+       }
+       mutex_unlock(sk_lock);
+
+       /*
+        * Allocate new one
+        */
+       sk = malloc(sizeof(*sk));
+       memset(sk, 0, sizeof(*sk));
+
+       sk->lock = sk_lock;
+
+       sk->fd = socket(AF_INET, SOCK_DGRAM, 0);
+       if (sk->fd < 0)
+               goto out;
+
+       /*
+        * Set non-blocking
+        */
+       flags = fcntl(sk->fd, F_GETFL, 0);
+       flags |= O_NONBLOCK;
+       fcntl(sk->fd, F_SETFL, flags);
+
+       /*
+        * Add sk->fd to epoll_wait
+        */
+       ev.events       = EPOLLIN;
+       ev.data.fd      = sk->fd;
+       ev.data.ptr     = sk;
+       if (arg->info->udp_epollfd <= 0)
+               arg->info->udp_epollfd = epoll_create(UIP_UDP_MAX_EVENTS);
+       ret = epoll_ctl(arg->info->udp_epollfd, EPOLL_CTL_ADD, sk->fd, &ev);
+       if (ret == -1)
+               pr_warning("epoll_ctl error");
+
+       sk->addr.sin_family      = AF_INET;
+       sk->addr.sin_addr.s_addr = dip;
+       sk->addr.sin_port        = dport;
+
+       sk->sip                  = sip;
+       sk->dip                  = dip;
+       sk->sport                = sport;
+       sk->dport                = dport;
+
+       mutex_lock(sk_lock);
+       list_add_tail(&sk->list, sk_head);
+       mutex_unlock(sk_lock);
+
+       return sk;
+
+out:
+       free(sk);
+       return NULL;
+}
+
+static int uip_udp_socket_send(struct uip_udp_socket *sk, struct uip_udp *udp)
+{
+       int len;
+       int ret;
+
+       len = ntohs(udp->len) - uip_udp_hdrlen(udp);
+
+       ret = sendto(sk->fd, udp->payload, len, 0, (struct sockaddr *)&sk->addr, sizeof(sk->addr));
+       if (ret != len)
+               return -1;
+
+       return 0;
+}
+
+static void *uip_udp_socket_thread(void *p)
+{
+       struct epoll_event events[UIP_UDP_MAX_EVENTS];
+       struct uip_udp_socket *sk;
+       struct uip_info *info;
+       struct uip_eth *eth2;
+       struct uip_udp *udp2;
+       struct uip_buf *buf;
+       struct uip_ip *ip2;
+       u8 *payload;
+       int nfds;
+       int ret;
+       int i;
+
+       info = p;
+
+       do {
+               payload = malloc(UIP_MAX_UDP_PAYLOAD);
+       } while (!payload);
+
+       while (1) {
+               nfds = epoll_wait(info->udp_epollfd, events, UIP_UDP_MAX_EVENTS, -1);
+
+               if (nfds == -1)
+                       continue;
+
+               for (i = 0; i < nfds; i++) {
+
+                       sk = events[i].data.ptr;
+                       ret = recvfrom(sk->fd, payload, UIP_MAX_UDP_PAYLOAD, 0, NULL, NULL);
+                       if (ret < 0)
+                               continue;
+
+                       /*
+                        * Get free buffer to send data to guest
+                        */
+                       buf             = uip_buf_get_free(info);
+
+                       /*
+                        * Cook a ethernet frame
+                        */
+                       udp2            = (struct uip_udp *)(buf->eth);
+                       eth2            = (struct uip_eth *)buf->eth;
+                       ip2             = (struct uip_ip *)(buf->eth);
+
+                       eth2->src       = info->host_mac;
+                       eth2->dst       = info->guest_mac;
+                       eth2->type      = htons(UIP_ETH_P_IP);
+
+                       ip2->vhl        = UIP_IP_VER_4 | UIP_IP_HDR_LEN;
+                       ip2->tos        = 0;
+                       ip2->id         = 0;
+                       ip2->flgfrag    = 0;
+                       ip2->ttl        = UIP_IP_TTL;
+                       ip2->proto      = UIP_IP_P_UDP;
+                       ip2->csum       = 0;
+                       ip2->sip        = sk->dip;
+                       ip2->dip        = sk->sip;
+
+                       udp2->sport     = sk->dport;
+                       udp2->dport     = sk->sport;
+                       udp2->len       = htons(ret + uip_udp_hdrlen(udp2));
+                       udp2->csum      = 0;
+
+                       memcpy(udp2->payload, payload, ret);
+
+                       ip2->len        = udp2->len + htons(uip_ip_hdrlen(ip2));
+                       ip2->csum       = uip_csum_ip(ip2);
+                       udp2->csum      = uip_csum_udp(udp2);
+
+                       /*
+                        * virtio_net_hdr
+                        */
+                       buf->vnet_len   = sizeof(struct virtio_net_hdr);
+                       memset(buf->vnet, 0, buf->vnet_len);
+
+                       buf->eth_len    = ntohs(ip2->len) + uip_eth_hdrlen(&ip2->eth);
+
+                       /*
+                        * Send data received from socket to guest
+                        */
+                       uip_buf_set_used(info, buf);
+               }
+       }
+
+       free(payload);
+       pthread_exit(NULL);
+       return NULL;
+}
+
+int uip_tx_do_ipv4_udp(struct uip_tx_arg *arg)
+{
+       struct uip_udp_socket *sk;
+       struct uip_info *info;
+       struct uip_udp *udp;
+       struct uip_ip *ip;
+       int ret;
+
+       udp     = (struct uip_udp *)(arg->eth);
+       ip      = (struct uip_ip *)(arg->eth);
+       info    = arg->info;
+
+       /*
+        * Find socket we have allocated before, otherwise allocate one
+        */
+       sk = uip_udp_socket_find(arg, ip->sip, ip->dip, udp->sport, udp->dport);
+       if (!sk)
+               return -1;
+
+       /*
+        * Send out UDP data to remote host
+        */
+       ret = uip_udp_socket_send(sk, udp);
+       if (ret)
+               return -1;
+
+       if (!info->udp_thread)
+               pthread_create(&info->udp_thread, NULL, uip_udp_socket_thread, (void *)info);
+
+       return 0;
+}