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