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 static void *uip_udp_socket_thread(void *p) 102 { 103 struct epoll_event events[UIP_UDP_MAX_EVENTS]; 104 struct uip_udp_socket *sk; 105 struct uip_info *info; 106 struct uip_eth *eth2; 107 struct uip_udp *udp2; 108 struct uip_buf *buf; 109 struct uip_ip *ip2; 110 u8 *payload; 111 int nfds; 112 int ret; 113 int i; 114 115 info = p; 116 117 do { 118 payload = malloc(UIP_MAX_UDP_PAYLOAD); 119 } while (!payload); 120 121 while (1) { 122 nfds = epoll_wait(info->udp_epollfd, events, UIP_UDP_MAX_EVENTS, -1); 123 124 if (nfds == -1) 125 continue; 126 127 for (i = 0; i < nfds; i++) { 128 129 sk = events[i].data.ptr; 130 ret = recvfrom(sk->fd, payload, UIP_MAX_UDP_PAYLOAD, 0, NULL, NULL); 131 if (ret < 0) 132 continue; 133 134 /* 135 * Get free buffer to send data to guest 136 */ 137 buf = uip_buf_get_free(info); 138 139 /* 140 * Cook a ethernet frame 141 */ 142 udp2 = (struct uip_udp *)(buf->eth); 143 eth2 = (struct uip_eth *)buf->eth; 144 ip2 = (struct uip_ip *)(buf->eth); 145 146 eth2->src = info->host_mac; 147 eth2->dst = info->guest_mac; 148 eth2->type = htons(UIP_ETH_P_IP); 149 150 ip2->vhl = UIP_IP_VER_4 | UIP_IP_HDR_LEN; 151 ip2->tos = 0; 152 ip2->id = 0; 153 ip2->flgfrag = 0; 154 ip2->ttl = UIP_IP_TTL; 155 ip2->proto = UIP_IP_P_UDP; 156 ip2->csum = 0; 157 ip2->sip = sk->dip; 158 ip2->dip = sk->sip; 159 160 udp2->sport = sk->dport; 161 udp2->dport = sk->sport; 162 udp2->len = htons(ret + uip_udp_hdrlen(udp2)); 163 udp2->csum = 0; 164 165 memcpy(udp2->payload, payload, ret); 166 167 ip2->len = udp2->len + htons(uip_ip_hdrlen(ip2)); 168 ip2->csum = uip_csum_ip(ip2); 169 udp2->csum = uip_csum_udp(udp2); 170 171 /* 172 * virtio_net_hdr 173 */ 174 buf->vnet_len = sizeof(struct virtio_net_hdr); 175 memset(buf->vnet, 0, buf->vnet_len); 176 177 buf->eth_len = ntohs(ip2->len) + uip_eth_hdrlen(&ip2->eth); 178 179 /* 180 * Send data received from socket to guest 181 */ 182 uip_buf_set_used(info, buf); 183 } 184 } 185 186 free(payload); 187 pthread_exit(NULL); 188 return NULL; 189 } 190 191 int uip_tx_do_ipv4_udp(struct uip_tx_arg *arg) 192 { 193 struct uip_udp_socket *sk; 194 struct uip_info *info; 195 struct uip_udp *udp; 196 struct uip_ip *ip; 197 int ret; 198 199 udp = (struct uip_udp *)(arg->eth); 200 ip = (struct uip_ip *)(arg->eth); 201 info = arg->info; 202 203 /* 204 * Find socket we have allocated before, otherwise allocate one 205 */ 206 sk = uip_udp_socket_find(arg, ip->sip, ip->dip, udp->sport, udp->dport); 207 if (!sk) 208 return -1; 209 210 /* 211 * Send out UDP data to remote host 212 */ 213 ret = uip_udp_socket_send(sk, udp); 214 if (ret) 215 return -1; 216 217 if (!info->udp_thread) 218 pthread_create(&info->udp_thread, NULL, uip_udp_socket_thread, (void *)info); 219 220 return 0; 221 } 222