1 /*
2 * Copyright (c) 2016 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_DECLARE(net_zperf, CONFIG_NET_ZPERF_LOG_LEVEL);
9
10 #include <zephyr/linker/sections.h>
11 #include <zephyr/toolchain.h>
12
13 #include <zephyr/kernel.h>
14
15 #include <zephyr/net/socket.h>
16 #include <zephyr/net/zperf.h>
17
18 #include "zperf_internal.h"
19 #include "zperf_session.h"
20
21 /* To get net_sprint_ipv{4|6}_addr() */
22 #define NET_LOG_ENABLED 1
23 #include "net_private.h"
24
25 static struct sockaddr_in6 *in6_addr_my;
26 static struct sockaddr_in *in4_addr_my;
27
28 #if defined(CONFIG_NET_TC_THREAD_COOPERATIVE)
29 #define UDP_RECEIVER_THREAD_PRIORITY K_PRIO_COOP(8)
30 #else
31 #define UDP_RECEIVER_THREAD_PRIORITY K_PRIO_PREEMPT(8)
32 #endif
33
34 #define UDP_RECEIVER_STACK_SIZE 2048
35
36 #define SOCK_ID_IPV4 0
37 #define SOCK_ID_IPV6 1
38 #define SOCK_ID_MAX 2
39
40 #define UDP_RECEIVER_BUF_SIZE 1500
41 #define POLL_TIMEOUT_MS 100
42
43 static K_THREAD_STACK_DEFINE(udp_receiver_stack_area, UDP_RECEIVER_STACK_SIZE);
44 static struct k_thread udp_receiver_thread_data;
45
46 static zperf_callback udp_session_cb;
47 static void *udp_user_data;
48 static bool udp_server_running;
49 static bool udp_server_stop;
50 static uint16_t udp_server_port;
51 static struct sockaddr udp_server_addr;
52 static K_SEM_DEFINE(udp_server_run, 0, 1);
53
build_reply(struct zperf_udp_datagram * hdr,struct zperf_server_hdr * stat,uint8_t * buf)54 static inline void build_reply(struct zperf_udp_datagram *hdr,
55 struct zperf_server_hdr *stat,
56 uint8_t *buf)
57 {
58 int pos = 0;
59 struct zperf_server_hdr *stat_hdr;
60
61 memcpy(&buf[pos], hdr, sizeof(struct zperf_udp_datagram));
62 pos += sizeof(struct zperf_udp_datagram);
63
64 stat_hdr = (struct zperf_server_hdr *)&buf[pos];
65
66 stat_hdr->flags = htonl(stat->flags);
67 stat_hdr->total_len1 = htonl(stat->total_len1);
68 stat_hdr->total_len2 = htonl(stat->total_len2);
69 stat_hdr->stop_sec = htonl(stat->stop_sec);
70 stat_hdr->stop_usec = htonl(stat->stop_usec);
71 stat_hdr->error_cnt = htonl(stat->error_cnt);
72 stat_hdr->outorder_cnt = htonl(stat->outorder_cnt);
73 stat_hdr->datagrams = htonl(stat->datagrams);
74 stat_hdr->jitter1 = htonl(stat->jitter1);
75 stat_hdr->jitter2 = htonl(stat->jitter2);
76 }
77
78 /* Send statistics to the remote client */
79 #define BUF_SIZE sizeof(struct zperf_udp_datagram) + \
80 sizeof(struct zperf_server_hdr)
81
zperf_receiver_send_stat(int sock,const struct sockaddr * addr,struct zperf_udp_datagram * hdr,struct zperf_server_hdr * stat)82 static int zperf_receiver_send_stat(int sock, const struct sockaddr *addr,
83 struct zperf_udp_datagram *hdr,
84 struct zperf_server_hdr *stat)
85 {
86 uint8_t reply[BUF_SIZE];
87 int ret;
88
89 build_reply(hdr, stat, reply);
90
91 ret = zsock_sendto(sock, reply, sizeof(reply), 0, addr,
92 addr->sa_family == AF_INET6 ?
93 sizeof(struct sockaddr_in6) :
94 sizeof(struct sockaddr_in));
95 if (ret < 0) {
96 NET_ERR("Cannot send data to peer (%d)", errno);
97 }
98
99 return ret;
100 }
101
udp_received(int sock,const struct sockaddr * addr,uint8_t * data,size_t datalen)102 static void udp_received(int sock, const struct sockaddr *addr, uint8_t *data,
103 size_t datalen)
104 {
105 struct zperf_udp_datagram *hdr;
106 struct session *session;
107 int32_t transit_time;
108 int64_t time;
109 int32_t id;
110
111 if (datalen < sizeof(struct zperf_udp_datagram)) {
112 NET_WARN("Short iperf packet!");
113 return;
114 }
115
116 hdr = (struct zperf_udp_datagram *)data;
117 time = k_uptime_ticks();
118
119 session = get_session(addr, SESSION_UDP);
120 if (!session) {
121 NET_ERR("Cannot get a session!");
122 return;
123 }
124
125 id = ntohl(hdr->id);
126
127 switch (session->state) {
128 case STATE_COMPLETED:
129 case STATE_NULL:
130 if (id < 0) {
131 /* Session is already completed: Resend the stat packet
132 * and continue
133 */
134 if (zperf_receiver_send_stat(sock, addr, hdr,
135 &session->stat) < 0) {
136 NET_ERR("Failed to send the packet");
137 }
138 } else {
139 zperf_reset_session_stats(session);
140 session->state = STATE_ONGOING;
141 session->start_time = time;
142
143 /* Start a new session! */
144 if (udp_session_cb != NULL) {
145 udp_session_cb(ZPERF_SESSION_STARTED, NULL,
146 udp_user_data);
147 }
148 }
149 break;
150 case STATE_ONGOING:
151 if (id < 0) { /* Negative id means session end. */
152 struct zperf_results results = { 0 };
153 uint32_t duration;
154
155 duration = k_ticks_to_us_ceil32(time -
156 session->start_time);
157
158 /* Update state machine */
159 session->state = STATE_COMPLETED;
160
161 /* Fill statistics */
162 session->stat.flags = 0x80000000;
163 session->stat.total_len1 = session->length >> 32;
164 session->stat.total_len2 =
165 session->length % 0xFFFFFFFF;
166 session->stat.stop_sec = duration / USEC_PER_SEC;
167 session->stat.stop_usec = duration % USEC_PER_SEC;
168 session->stat.error_cnt = session->error;
169 session->stat.outorder_cnt = session->outorder;
170 session->stat.datagrams = session->counter;
171 session->stat.jitter1 = 0;
172 session->stat.jitter2 = session->jitter;
173
174 if (zperf_receiver_send_stat(sock, addr, hdr,
175 &session->stat) < 0) {
176 NET_ERR("Failed to send the packet");
177 }
178
179 results.nb_packets_rcvd = session->counter;
180 results.nb_packets_lost = session->error;
181 results.nb_packets_outorder = session->outorder;
182 results.total_len = session->length;
183 results.time_in_us = duration;
184 results.jitter_in_us = session->jitter;
185 results.packet_size = session->length / session->counter;
186
187 if (udp_session_cb != NULL) {
188 udp_session_cb(ZPERF_SESSION_FINISHED, &results,
189 udp_user_data);
190 }
191 } else {
192 /* Update counter */
193 session->counter++;
194 session->length += datalen;
195
196 /* Compute jitter */
197 transit_time = time_delta(
198 k_ticks_to_us_ceil32(time),
199 ntohl(hdr->tv_sec) * USEC_PER_SEC +
200 ntohl(hdr->tv_usec));
201 if (session->last_transit_time != 0) {
202 int32_t delta_transit = transit_time -
203 session->last_transit_time;
204
205 delta_transit =
206 (delta_transit < 0) ?
207 -delta_transit : delta_transit;
208
209 session->jitter +=
210 (delta_transit - session->jitter) / 16;
211 }
212
213 session->last_transit_time = transit_time;
214
215 /* Check header id */
216 if (id != session->next_id) {
217 if (id < session->next_id) {
218 session->outorder++;
219 } else {
220 session->error += id - session->next_id;
221 session->next_id = id + 1;
222 }
223 } else {
224 session->next_id++;
225 }
226 }
227 break;
228 default:
229 break;
230 }
231 }
232
udp_server_session(void)233 static void udp_server_session(void)
234 {
235 static uint8_t buf[UDP_RECEIVER_BUF_SIZE];
236 struct zsock_pollfd fds[SOCK_ID_MAX] = { 0 };
237 int ret;
238
239 for (int i = 0; i < ARRAY_SIZE(fds); i++) {
240 fds[i].fd = -1;
241 }
242
243 if (IS_ENABLED(CONFIG_NET_IPV4)) {
244 const struct in_addr *in4_addr = NULL;
245
246 in4_addr_my = zperf_get_sin();
247
248 fds[SOCK_ID_IPV4].fd = zsock_socket(AF_INET, SOCK_DGRAM,
249 IPPROTO_UDP);
250 if (fds[SOCK_ID_IPV4].fd < 0) {
251 NET_ERR("Cannot create IPv4 network socket.");
252 goto error;
253 }
254
255 in4_addr = &net_sin(&udp_server_addr)->sin_addr;
256
257 if (!net_ipv4_is_addr_unspecified(in4_addr)) {
258 memcpy(&in4_addr_my->sin_addr, in4_addr,
259 sizeof(struct in_addr));
260 } else if (strlen(MY_IP4ADDR ? MY_IP4ADDR : "")) {
261 /* Use setting IP */
262 ret = zperf_get_ipv4_addr(MY_IP4ADDR,
263 &in4_addr_my->sin_addr);
264 if (ret < 0) {
265 NET_WARN("Unable to set IPv4");
266 goto use_existing_ipv4;
267 }
268 } else {
269 use_existing_ipv4:
270 /* Use existing IP */
271 in4_addr = zperf_get_default_if_in4_addr();
272 if (!in4_addr) {
273 NET_ERR("Unable to get IPv4 by default");
274 goto error;
275 }
276 memcpy(&in4_addr_my->sin_addr, in4_addr,
277 sizeof(struct in_addr));
278 }
279
280 NET_INFO("Binding to %s",
281 net_sprint_ipv4_addr(&in4_addr_my->sin_addr));
282
283 in4_addr_my->sin_port = htons(udp_server_port);
284
285 ret = zsock_bind(fds[SOCK_ID_IPV4].fd,
286 (struct sockaddr *)in4_addr_my,
287 sizeof(struct sockaddr_in));
288 if (ret < 0) {
289 NET_ERR("Cannot bind IPv4 UDP port %d (%d)",
290 ntohs(in4_addr_my->sin_port),
291 errno);
292 goto error;
293 }
294
295 fds[SOCK_ID_IPV4].events = ZSOCK_POLLIN;
296 }
297
298 if (IS_ENABLED(CONFIG_NET_IPV6)) {
299 const struct in6_addr *in6_addr = NULL;
300
301 in6_addr_my = zperf_get_sin6();
302
303 fds[SOCK_ID_IPV6].fd = zsock_socket(AF_INET6, SOCK_DGRAM,
304 IPPROTO_UDP);
305 if (fds[SOCK_ID_IPV6].fd < 0) {
306 NET_ERR("Cannot create IPv4 network socket.");
307 goto error;
308 }
309
310 in6_addr = &net_sin6(&udp_server_addr)->sin6_addr;
311
312 if (!net_ipv6_is_addr_unspecified(in6_addr)) {
313 memcpy(&in6_addr_my->sin6_addr, in6_addr,
314 sizeof(struct in6_addr));
315 } else if (strlen(MY_IP6ADDR ? MY_IP6ADDR : "")) {
316 /* Use setting IP */
317 ret = zperf_get_ipv6_addr(MY_IP6ADDR,
318 MY_PREFIX_LEN_STR,
319 &in6_addr_my->sin6_addr);
320 if (ret < 0) {
321 NET_WARN("Unable to set IPv6");
322 goto use_existing_ipv6;
323 }
324 } else {
325 use_existing_ipv6:
326 /* Use existing IP */
327 in6_addr = zperf_get_default_if_in6_addr();
328 if (!in6_addr) {
329 NET_ERR("Unable to get IPv4 by default");
330 goto error;
331 }
332 memcpy(&in6_addr_my->sin6_addr, in6_addr,
333 sizeof(struct in6_addr));
334 }
335
336 NET_INFO("Binding to %s",
337 net_sprint_ipv6_addr(&in6_addr_my->sin6_addr));
338
339 in6_addr_my->sin6_port = htons(udp_server_port);
340
341 ret = zsock_bind(fds[SOCK_ID_IPV6].fd,
342 (struct sockaddr *)in6_addr_my,
343 sizeof(struct sockaddr_in6));
344 if (ret < 0) {
345 NET_ERR("Cannot bind IPv6 UDP port %d (%d)",
346 ntohs(in6_addr_my->sin6_port),
347 ret);
348 goto error;
349 }
350
351 fds[SOCK_ID_IPV6].events = ZSOCK_POLLIN;
352 }
353
354 NET_INFO("Listening on port %d", udp_server_port);
355
356 while (true) {
357 ret = zsock_poll(fds, ARRAY_SIZE(fds), POLL_TIMEOUT_MS);
358 if (ret < 0) {
359 NET_ERR("UDP receiver poll error (%d)", errno);
360 goto error;
361 }
362
363 if (udp_server_stop) {
364 goto cleanup;
365 }
366
367 if (ret == 0) {
368 continue;
369 }
370
371 for (int i = 0; i < ARRAY_SIZE(fds); i++) {
372 struct sockaddr addr;
373 socklen_t addrlen = sizeof(addr);
374
375 if ((fds[i].revents & ZSOCK_POLLERR) ||
376 (fds[i].revents & ZSOCK_POLLNVAL)) {
377 NET_ERR("UDP receiver IPv%d socket error",
378 (i == SOCK_ID_IPV4) ? 4 : 6);
379 goto error;
380 }
381
382 if (!(fds[i].revents & ZSOCK_POLLIN)) {
383 continue;
384 }
385
386 ret = zsock_recvfrom(fds[i].fd, buf, sizeof(buf), 0,
387 &addr, &addrlen);
388 if (ret < 0) {
389 NET_ERR("recv failed on IPv%d socket (%d)",
390 (i == SOCK_ID_IPV4) ? 4 : 6, errno);
391 goto error;
392 }
393
394 udp_received(fds[i].fd, &addr, buf, ret);
395 }
396 }
397
398 error:
399 if (udp_session_cb != NULL) {
400 udp_session_cb(ZPERF_SESSION_ERROR, NULL, udp_user_data);
401 }
402
403 cleanup:
404 for (int i = 0; i < ARRAY_SIZE(fds); i++) {
405 if (fds[i].fd >= 0) {
406 zsock_close(fds[i].fd);
407 }
408 }
409 }
410
udp_receiver_thread(void * ptr1,void * ptr2,void * ptr3)411 static void udp_receiver_thread(void *ptr1, void *ptr2, void *ptr3)
412 {
413 ARG_UNUSED(ptr1);
414 ARG_UNUSED(ptr2);
415 ARG_UNUSED(ptr3);
416
417 while (true) {
418 k_sem_take(&udp_server_run, K_FOREVER);
419
420 udp_server_session();
421
422 udp_server_running = false;
423 }
424 }
425
zperf_udp_receiver_init(void)426 void zperf_udp_receiver_init(void)
427 {
428 k_thread_create(&udp_receiver_thread_data,
429 udp_receiver_stack_area,
430 K_THREAD_STACK_SIZEOF(udp_receiver_stack_area),
431 udp_receiver_thread,
432 NULL, NULL, NULL,
433 UDP_RECEIVER_THREAD_PRIORITY,
434 IS_ENABLED(CONFIG_USERSPACE) ? K_USER |
435 K_INHERIT_PERMS : 0,
436 K_NO_WAIT);
437 }
438
zperf_udp_download(const struct zperf_download_params * param,zperf_callback callback,void * user_data)439 int zperf_udp_download(const struct zperf_download_params *param,
440 zperf_callback callback, void *user_data)
441 {
442 if (param == NULL || callback == NULL) {
443 return -EINVAL;
444 }
445
446 if (udp_server_running) {
447 return -EALREADY;
448 }
449
450 udp_session_cb = callback;
451 udp_user_data = user_data;
452 udp_server_port = param->port;
453 udp_server_running = true;
454 udp_server_stop = false;
455 memcpy(&udp_server_addr, ¶m->addr, sizeof(struct sockaddr));
456
457 k_sem_give(&udp_server_run);
458
459 return 0;
460 }
461
zperf_udp_download_stop(void)462 int zperf_udp_download_stop(void)
463 {
464 if (!udp_server_running) {
465 return -EALREADY;
466 }
467
468 udp_server_stop = true;
469 udp_session_cb = NULL;
470
471 return 0;
472 }
473
