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
uip_udp_socket_find(struct uip_tx_arg * arg,u32 sip,u32 dip,u16 sport,u16 dport)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 struct mutex *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
uip_udp_socket_send(struct uip_udp_socket * sk,struct uip_udp * udp)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
uip_udp_make_pkg(struct uip_info * info,struct uip_udp_socket * sk,struct uip_buf * buf,u8 * payload,int payload_len)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 = info->vnet_hdr_len;
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
uip_udp_socket_thread(void * p)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 payload = info->udp_buf;
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 mutex_lock(&info->udp_socket_lock);
197 free(info->udp_buf);
198 info->udp_buf = NULL;
199 mutex_unlock(&info->udp_socket_lock);
200
201 pthread_exit(NULL);
202 return NULL;
203 }
204
uip_tx_do_ipv4_udp(struct uip_tx_arg * arg)205 int uip_tx_do_ipv4_udp(struct uip_tx_arg *arg)
206 {
207 struct uip_udp_socket *sk;
208 struct uip_info *info;
209 struct uip_udp *udp;
210 struct uip_ip *ip;
211 int ret;
212
213 udp = (struct uip_udp *)(arg->eth);
214 ip = (struct uip_ip *)(arg->eth);
215 info = arg->info;
216
217 if (uip_udp_is_dhcp(udp)) {
218 uip_tx_do_ipv4_udp_dhcp(arg);
219 return 0;
220 }
221
222 /*
223 * Find socket we have allocated before, otherwise allocate one
224 */
225 sk = uip_udp_socket_find(arg, ip->sip, ip->dip, udp->sport, udp->dport);
226 if (!sk)
227 return -1;
228
229 /*
230 * Send out UDP data to remote host
231 */
232 ret = uip_udp_socket_send(sk, udp);
233 if (ret)
234 return -1;
235
236 if (!info->udp_thread) {
237 info->udp_buf = malloc(UIP_MAX_UDP_PAYLOAD);
238 if (!info->udp_buf)
239 return -1;
240
241 pthread_create(&info->udp_thread, NULL, uip_udp_socket_thread, (void *)info);
242 }
243
244 return 0;
245 }
246
uip_udp_exit(struct uip_info * info)247 void uip_udp_exit(struct uip_info *info)
248 {
249 struct uip_udp_socket *sk, *next;
250
251 mutex_lock(&info->udp_socket_lock);
252 if (info->udp_thread) {
253 pthread_cancel(info->udp_thread);
254 pthread_join(info->udp_thread, NULL);
255 info->udp_thread = 0;
256 free(info->udp_buf);
257 }
258 if (info->udp_epollfd > 0) {
259 close(info->udp_epollfd);
260 info->udp_epollfd = 0;
261 }
262
263 list_for_each_entry_safe(sk, next, &info->udp_socket_head, list) {
264 close(sk->fd);
265 free(sk);
266 }
267 mutex_unlock(&info->udp_socket_lock);
268 }
269