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 = 0;
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 ret = zsock_recvfrom(pev->event.fd, buf, sizeof(buf), 0,
345 &addr, &addrlen);
346 if (ret < 0) {
347 ret = -errno;
348 (void)zsock_getsockopt(pev->event.fd, SOL_SOCKET,
349 SO_DOMAIN, &family, &optlen);
350 NET_ERR("recv failed on IPv%d socket (%d)",
351 family == AF_INET ? 4 : 6, -ret);
352 goto error;
353 }
354
355 udp_received(pev->event.fd, &addr, buf, ret);
356
357 return ret;
358
359 error:
360 if (udp_session_cb != NULL) {
361 udp_session_cb(ZPERF_SESSION_ERROR, NULL, udp_user_data);
362 }
363
364 return ret;
365 }
366
udp_svc_handler(struct net_socket_service_event * pev)367 static void udp_svc_handler(struct net_socket_service_event *pev)
368 {
369 int ret;
370
371 ret = udp_recv_data(pev);
372 if (ret < 0) {
373 udp_receiver_cleanup();
374 }
375 }
376
zperf_udp_receiver_init(void)377 static int zperf_udp_receiver_init(void)
378 {
379 int ret;
380 int family;
381
382 for (int i = 0; i < ARRAY_SIZE(fds); i++) {
383 fds[i].fd = -1;
384 }
385
386 family = udp_server_addr.sa_family;
387
388 if (IS_ENABLED(CONFIG_NET_IPV4) && (family == AF_INET || family == AF_UNSPEC)) {
389 const struct in_addr *in4_addr = NULL;
390
391 in4_addr_my = zperf_get_sin();
392
393 fds[SOCK_ID_IPV4].fd = zsock_socket(AF_INET, SOCK_DGRAM,
394 IPPROTO_UDP);
395 if (fds[SOCK_ID_IPV4].fd < 0) {
396 ret = -errno;
397 NET_ERR("Cannot create IPv4 network socket.");
398 goto error;
399 }
400
401 in4_addr = &net_sin(&udp_server_addr)->sin_addr;
402
403 if (!net_ipv4_is_addr_unspecified(in4_addr)) {
404 memcpy(&in4_addr_my->sin_addr, in4_addr,
405 sizeof(struct in_addr));
406 } else if (strlen(MY_IP4ADDR ? MY_IP4ADDR : "")) {
407 /* Use setting IP */
408 ret = zperf_get_ipv4_addr(MY_IP4ADDR,
409 &in4_addr_my->sin_addr);
410 if (ret < 0) {
411 NET_WARN("Unable to set IPv4");
412 goto use_any_ipv4;
413 }
414 } else {
415 use_any_ipv4:
416 in4_addr_my->sin_addr.s_addr = INADDR_ANY;
417 }
418
419 if (net_ipv4_is_addr_mcast(&in4_addr_my->sin_addr)) {
420 zperf_udp_join_mcast_ipv4(udp_server_iface_name,
421 &in4_addr_my->sin_addr);
422 }
423
424 NET_INFO("Binding to %s",
425 net_sprint_ipv4_addr(&in4_addr_my->sin_addr));
426
427 in4_addr_my->sin_port = htons(udp_server_port);
428
429 ret = zsock_bind(fds[SOCK_ID_IPV4].fd,
430 (struct sockaddr *)in4_addr_my,
431 sizeof(struct sockaddr_in));
432 if (ret < 0) {
433 NET_ERR("Cannot bind IPv4 UDP port %d (%d)",
434 ntohs(in4_addr_my->sin_port),
435 errno);
436 goto error;
437 }
438
439 fds[SOCK_ID_IPV4].events = ZSOCK_POLLIN;
440 }
441
442 if (IS_ENABLED(CONFIG_NET_IPV6) && (family == AF_INET6 || family == AF_UNSPEC)) {
443 const struct in6_addr *in6_addr = NULL;
444
445 in6_addr_my = zperf_get_sin6();
446
447 fds[SOCK_ID_IPV6].fd = zsock_socket(AF_INET6, SOCK_DGRAM,
448 IPPROTO_UDP);
449 if (fds[SOCK_ID_IPV6].fd < 0) {
450 ret = -errno;
451 NET_ERR("Cannot create IPv4 network socket.");
452 goto error;
453 }
454
455 in6_addr = &net_sin6(&udp_server_addr)->sin6_addr;
456
457 if (!net_ipv6_is_addr_unspecified(in6_addr)) {
458 memcpy(&in6_addr_my->sin6_addr, in6_addr,
459 sizeof(struct in6_addr));
460 } else if (strlen(MY_IP6ADDR ? MY_IP6ADDR : "")) {
461 /* Use setting IP */
462 ret = zperf_get_ipv6_addr(MY_IP6ADDR,
463 MY_PREFIX_LEN_STR,
464 &in6_addr_my->sin6_addr);
465 if (ret < 0) {
466 NET_WARN("Unable to set IPv6");
467 goto use_any_ipv6;
468 }
469 } else {
470 use_any_ipv6:
471 memcpy(&in6_addr_my->sin6_addr,
472 net_ipv6_unspecified_address(),
473 sizeof(struct in6_addr));
474 }
475
476 if (net_ipv6_is_addr_mcast(&in6_addr_my->sin6_addr)) {
477 zperf_udp_join_mcast_ipv6(udp_server_iface_name,
478 &in6_addr_my->sin6_addr);
479 }
480
481 NET_INFO("Binding to %s",
482 net_sprint_ipv6_addr(&in6_addr_my->sin6_addr));
483
484 in6_addr_my->sin6_port = htons(udp_server_port);
485
486 ret = zsock_bind(fds[SOCK_ID_IPV6].fd,
487 (struct sockaddr *)in6_addr_my,
488 sizeof(struct sockaddr_in6));
489 if (ret < 0) {
490 NET_ERR("Cannot bind IPv6 UDP port %d (%d)",
491 ntohs(in6_addr_my->sin6_port),
492 ret);
493 goto error;
494 }
495
496 fds[SOCK_ID_IPV6].events = ZSOCK_POLLIN;
497 }
498
499 NET_INFO("Listening on port %d", udp_server_port);
500
501 ret = net_socket_service_register(&svc_udp, fds,
502 ARRAY_SIZE(fds), NULL);
503 if (ret < 0) {
504 LOG_ERR("Cannot register socket service handler (%d)", ret);
505 }
506
507 error:
508
509 return ret;
510 }
511
zperf_udp_download(const struct zperf_download_params * param,zperf_callback callback,void * user_data)512 int zperf_udp_download(const struct zperf_download_params *param,
513 zperf_callback callback, void *user_data)
514 {
515 int ret;
516
517 if (param == NULL || callback == NULL) {
518 return -EINVAL;
519 }
520
521 if (udp_server_running) {
522 return -EALREADY;
523 }
524
525 udp_session_cb = callback;
526 udp_user_data = user_data;
527 udp_server_port = param->port;
528 memcpy(&udp_server_addr, ¶m->addr, sizeof(struct sockaddr));
529
530 if (param->if_name[0]) {
531 /*
532 * IFNAMSIZ by default CONFIG_NET_INTERFACE_NAME_LEN
533 * is at least 1 so no overflow risk here
534 */
535 (void)memset(udp_server_iface_name, 0, IFNAMSIZ);
536 strncpy(udp_server_iface_name, param->if_name, IFNAMSIZ);
537 udp_server_iface_name[IFNAMSIZ - 1] = 0;
538 } else {
539 udp_server_iface_name[0] = 0;
540 }
541
542 ret = zperf_udp_receiver_init();
543 if (ret < 0) {
544 udp_receiver_cleanup();
545 return ret;
546 }
547
548 udp_server_running = true;
549
550 return 0;
551 }
552
zperf_udp_download_stop(void)553 int zperf_udp_download_stop(void)
554 {
555 if (!udp_server_running) {
556 return -EALREADY;
557 }
558
559 udp_receiver_cleanup();
560
561 return 0;
562 }
563
