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 } 237