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/mld.h>
16 #include <zephyr/net/socket.h>
17 #include <zephyr/net/socket_service.h>
18 #include <zephyr/net/zperf.h>
19
20 #include "zperf_internal.h"
21 #include "zperf_session.h"
22
23 /* To get net_sprint_ipv{4|6}_addr() */
24 #define NET_LOG_ENABLED 1
25 #include "net_private.h"
26
27 /* To support multicast */
28 #include "ipv6.h"
29 #include "zephyr/net/igmp.h"
30
31 static struct sockaddr_in6 *in6_addr_my;
32 static struct sockaddr_in *in4_addr_my;
33
34 #define SOCK_ID_IPV4 0
35 #define SOCK_ID_IPV6 1
36 #define SOCK_ID_MAX 2
37
38 #define UDP_RECEIVER_BUF_SIZE 1500
39 #define POLL_TIMEOUT_MS 100
40
41 static zperf_callback udp_session_cb;
42 static void *udp_user_data;
43 static bool udp_server_running;
44 static uint16_t udp_server_port;
45 static struct sockaddr udp_server_addr;
46
47 struct zsock_pollfd fds[SOCK_ID_MAX] = { 0 };
48
49 static void udp_svc_handler(struct net_socket_service_event *pev);
50
51 NET_SOCKET_SERVICE_SYNC_DEFINE_STATIC(svc_udp, udp_svc_handler,
52 SOCK_ID_MAX);
53 static char udp_server_iface_name[IFNAMSIZ];
54
build_reply(struct zperf_udp_datagram * hdr,struct zperf_server_hdr * stat,uint8_t * buf)55 static inline void build_reply(struct zperf_udp_datagram *hdr,
56 struct zperf_server_hdr *stat,
57 uint8_t *buf)
58 {
59 int pos = 0;
60 struct zperf_server_hdr *stat_hdr;
61
62 memcpy(&buf[pos], hdr, sizeof(struct zperf_udp_datagram));
63 pos += sizeof(struct zperf_udp_datagram);
64
65 stat_hdr = (struct zperf_server_hdr *)&buf[pos];
66
67 stat_hdr->flags = htonl(stat->flags);
68 stat_hdr->total_len1 = htonl(stat->total_len1);
69 stat_hdr->total_len2 = htonl(stat->total_len2);
70 stat_hdr->stop_sec = htonl(stat->stop_sec);
71 stat_hdr->stop_usec = htonl(stat->stop_usec);
72 stat_hdr->error_cnt = htonl(stat->error_cnt);
73 stat_hdr->outorder_cnt = htonl(stat->outorder_cnt);
74 stat_hdr->datagrams = htonl(stat->datagrams);
75 stat_hdr->jitter1 = htonl(stat->jitter1);
76 stat_hdr->jitter2 = htonl(stat->jitter2);
77 }
78
79 /* Send statistics to the remote client */
80 #define BUF_SIZE sizeof(struct zperf_udp_datagram) + \
81 sizeof(struct zperf_server_hdr)
82
zperf_receiver_send_stat(int sock,const struct sockaddr * addr,struct zperf_udp_datagram * hdr,struct zperf_server_hdr * stat)83 static int zperf_receiver_send_stat(int sock, const struct sockaddr *addr,
84 struct zperf_udp_datagram *hdr,
85 struct zperf_server_hdr *stat)
86 {
87 uint8_t reply[BUF_SIZE];
88 int ret;
89
90 build_reply(hdr, stat, reply);
91
92 ret = zsock_sendto(sock, reply, sizeof(reply), 0, addr,
93 addr->sa_family == AF_INET6 ?
94 sizeof(struct sockaddr_in6) :
95 sizeof(struct sockaddr_in));
96 if (ret < 0) {
97 NET_ERR("Cannot send data to peer (%d)", errno);
98 }
99
100 return ret;
101 }
102
udp_received(int sock,const struct sockaddr * addr,uint8_t * data,size_t datalen)103 static void udp_received(int sock, const struct sockaddr *addr, uint8_t *data,
104 size_t datalen)
105 {
106 struct zperf_udp_datagram *hdr;
107 struct session *session;
108 int32_t transit_time;
109 int64_t time;
110 int32_t id;
111
112 if (datalen < sizeof(struct zperf_udp_datagram)) {
113 NET_WARN("Short iperf packet!");
114 return;
115 }
116
117 hdr = (struct zperf_udp_datagram *)data;
118 time = k_uptime_ticks();
119
120 session = get_session(addr, SESSION_UDP);
121 if (!session) {
122 NET_ERR("Cannot get a session!");
123 return;
124 }
125
126 id = ntohl(hdr->id);
127
128 switch (session->state) {
129 case STATE_COMPLETED:
130 case STATE_NULL:
131 if (id < 0) {
132 /* Session is already completed: Resend the stat packet
133 * and continue
134 */
135 if (zperf_receiver_send_stat(sock, addr, hdr,
136 &session->stat) < 0) {
137 NET_ERR("Failed to send the packet");
138 }
139 } else {
140 zperf_reset_session_stats(session);
141 session->state = STATE_ONGOING;
142 session->start_time = time;
143
144 /* Start a new session! */
145 if (udp_session_cb != NULL) {
146 udp_session_cb(ZPERF_SESSION_STARTED, NULL,
147 udp_user_data);
148 }
149 }
150 break;
151 case STATE_ONGOING:
152 if (id < 0) { /* Negative id means session end. */
153 struct zperf_results results = { 0 };
154 uint64_t duration;
155
156 duration = k_ticks_to_us_ceil64(time -
157 session->start_time);
158
159 /* Update state machine */
160 session->state = STATE_COMPLETED;
161
162 /* Fill statistics */
163 session->stat.flags = 0x80000000;
164 session->stat.total_len1 = session->length >> 32;
165 session->stat.total_len2 =
166 session->length % 0xFFFFFFFF;
167 session->stat.stop_sec = duration / USEC_PER_SEC;
168 session->stat.stop_usec = duration % USEC_PER_SEC;
169 session->stat.error_cnt = session->error;
170 session->stat.outorder_cnt = session->outorder;
171 session->stat.datagrams = session->counter;
172 session->stat.jitter1 = 0;
173 session->stat.jitter2 = session->jitter;
174
175 if (zperf_receiver_send_stat(sock, addr, hdr,
176 &session->stat) < 0) {
177 NET_ERR("Failed to send the packet");
178 }
179
180 results.nb_packets_rcvd = session->counter;
181 results.nb_packets_lost = session->error;
182 results.nb_packets_outorder = session->outorder;
183 results.total_len = session->length;
184 results.time_in_us = duration;
185 results.jitter_in_us = session->jitter;
186 results.packet_size = session->length / session->counter;
187
188 if (udp_session_cb != NULL) {
189 udp_session_cb(ZPERF_SESSION_FINISHED, &results,
190 udp_user_data);
191 }
192 } else {
193 /* Update counter */
194 session->counter++;
195 session->length += datalen;
196
197 /* Compute jitter */
198 transit_time = time_delta(
199 k_ticks_to_us_ceil32(time),
200 ntohl(hdr->tv_sec) * USEC_PER_SEC +
201 ntohl(hdr->tv_usec));
202 if (session->last_transit_time != 0) {
203 int32_t delta_transit = transit_time -
204 session->last_transit_time;
205
206 delta_transit =
207 (delta_transit < 0) ?
208 -delta_transit : delta_transit;
209
210 session->jitter +=
211 (delta_transit - session->jitter) / 16;
212 }
213
214 session->last_transit_time = transit_time;
215
216 /* Check header id */
217 if (id != session->next_id) {
218 if (id < session->next_id) {
219 session->outorder++;
220 } else {
221 session->error += id - session->next_id;
222 session->next_id = id + 1;
223 }
224 } else {
225 session->next_id++;
226 }
227 }
228 break;
229 default:
230 break;
231 }
232 }
233
zperf_udp_join_mcast_ipv4(char * if_name,struct in_addr * addr)234 static void zperf_udp_join_mcast_ipv4(char *if_name, struct in_addr *addr)
235 {
236 struct net_if *iface = NULL;
237
238 if (if_name[0]) {
239 iface = net_if_get_by_index(net_if_get_by_name(if_name));
240 if (iface == NULL) {
241 iface = net_if_get_default();
242 }
243 } else {
244 iface = net_if_get_default();
245 }
246
247 if (iface != NULL) {
248 net_ipv4_igmp_join(iface, addr, NULL);
249 }
250 }
251
zperf_udp_join_mcast_ipv6(char * if_name,struct in6_addr * addr)252 static void zperf_udp_join_mcast_ipv6(char *if_name, struct in6_addr *addr)
253 {
254 struct net_if *iface = NULL;
255
256 if (if_name[0]) {
257 iface = net_if_get_by_index(net_if_get_by_name(if_name));
258 if (iface == NULL) {
259 iface = net_if_get_default();
260 }
261 } else {
262 iface = net_if_get_default();
263 }
264
265 if (iface != NULL) {
266 net_ipv6_mld_join(iface, addr);
267 }
268 }
269
zperf_udp_leave_mcast(int sock)270 static void zperf_udp_leave_mcast(int sock)
271 {
272 struct net_if *iface = NULL;
273 struct sockaddr addr = {0};
274 socklen_t addr_len = NET_IPV6_ADDR_SIZE;
275
276 zsock_getsockname(sock, &addr, &addr_len);
277
278 if (IS_ENABLED(CONFIG_NET_IPV4) && addr.sa_family == AF_INET) {
279 struct sockaddr_in *addr4 = (struct sockaddr_in *)&addr;
280
281 if (net_ipv4_is_addr_mcast(&addr4->sin_addr)) {
282 net_ipv4_igmp_leave(iface, &addr4->sin_addr);
283 }
284 }
285
286 if (IS_ENABLED(CONFIG_NET_IPV6) && addr.sa_family == AF_INET6) {
287 struct sockaddr_in6 *addr6 = (struct sockaddr_in6 *)&addr;
288
289 if (net_ipv6_is_addr_mcast(&addr6->sin6_addr)) {
290 net_ipv6_mld_leave(iface, &addr6->sin6_addr);
291 }
292 }
293 }
294
udp_receiver_cleanup(void)295 static void udp_receiver_cleanup(void)
296 {
297 int i;
298
299 (void)net_socket_service_unregister(&svc_udp);
300
301 for (i = 0; i < ARRAY_SIZE(fds); i++) {
302 if (fds[i].fd >= 0) {
303 zperf_udp_leave_mcast(fds[i].fd);
304 zsock_close(fds[i].fd);
305 fds[i].fd = -1;
306 }
307 }
308
309 udp_server_running = false;
310 udp_session_cb = NULL;
311
312 zperf_session_reset(SESSION_UDP);
313 }
314
udp_recv_data(struct net_socket_service_event * pev)315 static int udp_recv_data(struct net_socket_service_event *pev)
316 {
317 static uint8_t buf[UDP_RECEIVER_BUF_SIZE];
318 int ret = 1;
319 int family, sock_error;
320 struct sockaddr addr;
321 socklen_t optlen = sizeof(int);
322 socklen_t addrlen = sizeof(addr);
323
324 if (!udp_server_running) {
325 return -ENOENT;
326 }
327
328 if ((pev->event.revents & ZSOCK_POLLERR) ||
329 (pev->event.revents & ZSOCK_POLLNVAL)) {
330 (void)zsock_getsockopt(pev->event.fd, SOL_SOCKET,
331 SO_DOMAIN, &family, &optlen);
332 (void)zsock_getsockopt(pev->event.fd, SOL_SOCKET,
333 SO_ERROR, &sock_error, &optlen);
334 NET_ERR("UDP receiver IPv%d socket error (%d)",
335 family == AF_INET ? 4 : 6, sock_error);
336 ret = -sock_error;
337 goto error;
338 }
339
340 if (!(pev->event.revents & ZSOCK_POLLIN)) {
341 return 0;
342 }
343
344 while (ret > 0) {
345 ret = zsock_recvfrom(pev->event.fd, buf, sizeof(buf), ZSOCK_MSG_DONTWAIT,
346 &addr, &addrlen);
347 if ((ret < 0) && (errno == EAGAIN)) {
348 ret = 0;
349 break;
350 }
351
352 if (ret < 0) {
353 ret = -errno;
354 (void)zsock_getsockopt(pev->event.fd, SOL_SOCKET,
355 SO_DOMAIN, &family, &optlen);
356 NET_ERR("recv failed on IPv%d socket (%d)",
357 family == AF_INET ? 4 : 6, -ret);
358 goto error;
359 }
360
361 udp_received(pev->event.fd, &addr, buf, ret);
362 }
363 return ret;
364
365 error:
366 if (udp_session_cb != NULL) {
367 udp_session_cb(ZPERF_SESSION_ERROR, NULL, udp_user_data);
368 }
369
370 return ret;
371 }
372
udp_svc_handler(struct net_socket_service_event * pev)373 static void udp_svc_handler(struct net_socket_service_event *pev)
374 {
375 int ret;
376
377 ret = udp_recv_data(pev);
378 if (ret < 0) {
379 udp_receiver_cleanup();
380 }
381 }
382
zperf_udp_receiver_init(void)383 static int zperf_udp_receiver_init(void)
384 {
385 int ret;
386 int family;
387
388 for (int i = 0; i < ARRAY_SIZE(fds); i++) {
389 fds[i].fd = -1;
390 }
391
392 family = udp_server_addr.sa_family;
393
394 if (IS_ENABLED(CONFIG_NET_IPV4) && (family == AF_INET || family == AF_UNSPEC)) {
395 const struct in_addr *in4_addr = NULL;
396
397 in4_addr_my = zperf_get_sin();
398
399 fds[SOCK_ID_IPV4].fd = zsock_socket(AF_INET, SOCK_DGRAM,
400 IPPROTO_UDP);
401 if (fds[SOCK_ID_IPV4].fd < 0) {
402 ret = -errno;
403 NET_ERR("Cannot create IPv4 network socket.");
404 goto error;
405 }
406
407 in4_addr = &net_sin(&udp_server_addr)->sin_addr;
408
409 if (!net_ipv4_is_addr_unspecified(in4_addr)) {
410 memcpy(&in4_addr_my->sin_addr, in4_addr,
411 sizeof(struct in_addr));
412 } else if (strlen(MY_IP4ADDR ? MY_IP4ADDR : "")) {
413 /* Use setting IP */
414 ret = zperf_get_ipv4_addr(MY_IP4ADDR,
415 &in4_addr_my->sin_addr);
416 if (ret < 0) {
417 NET_WARN("Unable to set IPv4");
418 goto use_any_ipv4;
419 }
420 } else {
421 use_any_ipv4:
422 in4_addr_my->sin_addr.s_addr = INADDR_ANY;
423 }
424
425 if (net_ipv4_is_addr_mcast(&in4_addr_my->sin_addr)) {
426 zperf_udp_join_mcast_ipv4(udp_server_iface_name,
427 &in4_addr_my->sin_addr);
428 }
429
430 NET_INFO("Binding to %s",
431 net_sprint_ipv4_addr(&in4_addr_my->sin_addr));
432
433 in4_addr_my->sin_port = htons(udp_server_port);
434
435 ret = zsock_bind(fds[SOCK_ID_IPV4].fd,
436 (struct sockaddr *)in4_addr_my,
437 sizeof(struct sockaddr_in));
438 if (ret < 0) {
439 NET_ERR("Cannot bind IPv4 UDP port %d (%d)",
440 ntohs(in4_addr_my->sin_port),
441 errno);
442 goto error;
443 }
444
445 fds[SOCK_ID_IPV4].events = ZSOCK_POLLIN;
446 }
447
448 if (IS_ENABLED(CONFIG_NET_IPV6) && (family == AF_INET6 || family == AF_UNSPEC)) {
449 const struct in6_addr *in6_addr = NULL;
450
451 in6_addr_my = zperf_get_sin6();
452
453 fds[SOCK_ID_IPV6].fd = zsock_socket(AF_INET6, SOCK_DGRAM,
454 IPPROTO_UDP);
455 if (fds[SOCK_ID_IPV6].fd < 0) {
456 ret = -errno;
457 NET_ERR("Cannot create IPv4 network socket.");
458 goto error;
459 }
460
461 in6_addr = &net_sin6(&udp_server_addr)->sin6_addr;
462
463 if (!net_ipv6_is_addr_unspecified(in6_addr)) {
464 memcpy(&in6_addr_my->sin6_addr, in6_addr,
465 sizeof(struct in6_addr));
466 } else if (strlen(MY_IP6ADDR ? MY_IP6ADDR : "")) {
467 /* Use setting IP */
468 ret = zperf_get_ipv6_addr(MY_IP6ADDR,
469 MY_PREFIX_LEN_STR,
470 &in6_addr_my->sin6_addr);
471 if (ret < 0) {
472 NET_WARN("Unable to set IPv6");
473 goto use_any_ipv6;
474 }
475 } else {
476 use_any_ipv6:
477 memcpy(&in6_addr_my->sin6_addr,
478 net_ipv6_unspecified_address(),
479 sizeof(struct in6_addr));
480 }
481
482 if (net_ipv6_is_addr_mcast(&in6_addr_my->sin6_addr)) {
483 zperf_udp_join_mcast_ipv6(udp_server_iface_name,
484 &in6_addr_my->sin6_addr);
485 }
486
487 NET_INFO("Binding to %s",
488 net_sprint_ipv6_addr(&in6_addr_my->sin6_addr));
489
490 in6_addr_my->sin6_port = htons(udp_server_port);
491
492 ret = zsock_bind(fds[SOCK_ID_IPV6].fd,
493 (struct sockaddr *)in6_addr_my,
494 sizeof(struct sockaddr_in6));
495 if (ret < 0) {
496 NET_ERR("Cannot bind IPv6 UDP port %d (%d)",
497 ntohs(in6_addr_my->sin6_port),
498 ret);
499 goto error;
500 }
501
502 fds[SOCK_ID_IPV6].events = ZSOCK_POLLIN;
503 }
504
505 NET_INFO("Listening on port %d", udp_server_port);
506
507 ret = net_socket_service_register(&svc_udp, fds,
508 ARRAY_SIZE(fds), NULL);
509 if (ret < 0) {
510 LOG_ERR("Cannot register socket service handler (%d)", ret);
511 }
512
513 error:
514
515 return ret;
516 }
517
zperf_udp_download(const struct zperf_download_params * param,zperf_callback callback,void * user_data)518 int zperf_udp_download(const struct zperf_download_params *param,
519 zperf_callback callback, void *user_data)
520 {
521 int ret;
522
523 if (param == NULL || callback == NULL) {
524 return -EINVAL;
525 }
526
527 if (udp_server_running) {
528 return -EALREADY;
529 }
530
531 udp_session_cb = callback;
532 udp_user_data = user_data;
533 udp_server_port = param->port;
534 memcpy(&udp_server_addr, ¶m->addr, sizeof(struct sockaddr));
535
536 if (param->if_name[0]) {
537 /*
538 * IFNAMSIZ by default CONFIG_NET_INTERFACE_NAME_LEN
539 * is at least 1 so no overflow risk here
540 */
541 (void)memset(udp_server_iface_name, 0, IFNAMSIZ);
542 strncpy(udp_server_iface_name, param->if_name, IFNAMSIZ);
543 udp_server_iface_name[IFNAMSIZ - 1] = 0;
544 } else {
545 udp_server_iface_name[0] = 0;
546 }
547
548 ret = zperf_udp_receiver_init();
549 if (ret < 0) {
550 udp_receiver_cleanup();
551 return ret;
552 }
553
554 udp_server_running = true;
555
556 return 0;
557 }
558
zperf_udp_download_stop(void)559 int zperf_udp_download_stop(void)
560 {
561 if (!udp_server_running) {
562 return -EALREADY;
563 }
564
565 udp_receiver_cleanup();
566
567 return 0;
568 }
569