1 /*
2 * Copyright (c) 2020 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_txtime_sample, LOG_LEVEL_DBG);
9
10 #include <zephyr/kernel.h>
11 #include <errno.h>
12 #include <stdio.h>
13 #include <inttypes.h>
14 #include <zephyr/drivers/ptp_clock.h>
15 #include <zephyr/shell/shell.h>
16
17 #include <zephyr/net/net_mgmt.h>
18 #include <zephyr/net/net_event.h>
19 #include <zephyr/net/conn_mgr_monitor.h>
20
21 #include <zephyr/net/socket.h>
22 #include <zephyr/net/ethernet.h>
23 #include <zephyr/net/ethernet_mgmt.h>
24
25 #define APP_BANNER "Run SO_TXTIME client"
26
27 #define DHCPV4_MASK (NET_EVENT_IPV4_DHCP_BOUND | \
28 NET_EVENT_IPV4_DHCP_STOP)
29 #define EVENT_MASK (NET_EVENT_L4_CONNECTED | \
30 NET_EVENT_L4_DISCONNECTED)
31
32 #define STACK_SIZE 2048
33 #define THREAD_PRIORITY K_PRIO_COOP(8)
34 #define WAIT_PERIOD (1 * MSEC_PER_SEC)
35 #define MAX_MSG_LEN 64
36
37 static char txtime_str[MAX_MSG_LEN];
38
39 static struct k_sem quit_lock;
40 static struct net_mgmt_event_callback mgmt_cb;
41 static struct net_mgmt_event_callback dhcpv4_cb;
42
43 struct app_data {
44 const struct device *clk;
45 struct sockaddr peer;
46 socklen_t peer_addr_len;
47 int sock;
48 };
49
50 static struct app_data peer_data = {
51 .sock = -1,
52 };
53
54 static k_tid_t tx_tid;
55 static K_THREAD_STACK_DEFINE(tx_stack, STACK_SIZE);
56 static struct k_thread tx_thread;
57
58 static k_tid_t rx_tid;
59 static K_THREAD_STACK_DEFINE(rx_stack, STACK_SIZE);
60 static struct k_thread rx_thread;
61
62 K_SEM_DEFINE(run_app, 0, 1);
63 static bool want_to_quit;
64 static bool connected;
65
66 extern int init_vlan(void);
67
quit(void)68 static void quit(void)
69 {
70 k_sem_give(&quit_lock);
71 }
72
event_handler(struct net_mgmt_event_callback * cb,uint32_t mgmt_event,struct net_if * iface)73 static void event_handler(struct net_mgmt_event_callback *cb,
74 uint32_t mgmt_event, struct net_if *iface)
75 {
76 static bool dhcpv4_done;
77
78 if (want_to_quit) {
79 k_sem_give(&run_app);
80 want_to_quit = false;
81 }
82
83 if (IS_ENABLED(CONFIG_NET_DHCPV4)) {
84 if (mgmt_event == NET_EVENT_IPV4_DHCP_BOUND) {
85 LOG_INF("DHCPv4 bound");
86 dhcpv4_done = true;
87
88 if (connected) {
89 k_sem_give(&run_app);
90 }
91
92 return;
93 }
94
95 if (mgmt_event == NET_EVENT_IPV4_DHCP_STOP) {
96 dhcpv4_done = false;
97 return;
98 }
99 }
100
101 if (mgmt_event == NET_EVENT_L4_CONNECTED) {
102 if (!connected) {
103 LOG_INF("Network connected");
104 }
105
106 connected = true;
107
108 /* Go to connected state only after DHCPv4 is done */
109 if (!IS_ENABLED(CONFIG_NET_DHCPV4) || dhcpv4_done) {
110 k_sem_give(&run_app);
111 }
112
113 return;
114 }
115
116 if (mgmt_event == NET_EVENT_L4_DISCONNECTED) {
117 if (connected == false) {
118 LOG_INF("Waiting network to be connected");
119 } else {
120 LOG_INF("Network disconnected");
121 connected = false;
122 }
123
124 k_sem_reset(&run_app);
125
126 return;
127 }
128 }
129
rx(void * p1,void * p2,void * p3)130 static void rx(void *p1, void *p2, void *p3)
131 {
132 ARG_UNUSED(p2);
133 ARG_UNUSED(p3);
134
135 struct app_data *data = p1;
136 static uint8_t recv_buf[sizeof(txtime_str)];
137 struct sockaddr src;
138 socklen_t addr_len = data->peer_addr_len;
139 ssize_t len = 0;
140
141 while (true) {
142 len += recvfrom(data->sock, recv_buf, sizeof(recv_buf), 0,
143 &src, &addr_len);
144 if (!(len % (100 * 1024))) {
145 LOG_DBG("Received %zd kb data", len / 1024);
146 }
147 }
148 }
149
tx(void * p1,void * p2,void * p3)150 static void tx(void *p1, void *p2, void *p3)
151 {
152 ARG_UNUSED(p2);
153 ARG_UNUSED(p3);
154
155 struct app_data *data = p1;
156 struct net_ptp_time time;
157 struct msghdr msg;
158 struct cmsghdr *cmsg;
159 struct iovec io_vector[1];
160 union {
161 struct cmsghdr hdr;
162 unsigned char buf[CMSG_SPACE(sizeof(uint64_t))];
163 } cmsgbuf;
164 net_time_t txtime, delay, interval;
165 int ret;
166 int print_offset;
167
168 print_offset = IS_ENABLED(CONFIG_NET_SAMPLE_PACKET_SOCKET) ?
169 sizeof(struct net_eth_hdr) : 0;
170
171 interval = CONFIG_NET_SAMPLE_PACKET_INTERVAL * NSEC_PER_MSEC;
172 delay = CONFIG_NET_SAMPLE_PACKET_TXTIME * NSEC_PER_USEC;
173
174 io_vector[0].iov_base = (void *)txtime_str;
175
176 memset(&msg, 0, sizeof(msg));
177 msg.msg_control = &cmsgbuf.buf;
178 msg.msg_controllen = sizeof(cmsgbuf.buf);
179 msg.msg_iov = io_vector;
180 msg.msg_iovlen = 1;
181 msg.msg_name = &data->peer;
182 msg.msg_namelen = data->peer_addr_len;
183
184 cmsg = CMSG_FIRSTHDR(&msg);
185 cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));
186 cmsg->cmsg_level = SOL_SOCKET;
187 cmsg->cmsg_type = SCM_TXTIME;
188
189 LOG_DBG("Sending network packets with SO_TXTIME");
190
191 ptp_clock_get(data->clk, &time);
192 txtime = net_ptp_time_to_ns(&time);
193
194 snprintk(txtime_str + print_offset,
195 sizeof(txtime_str) - print_offset, "%"PRIx64, (uint64_t)txtime);
196 io_vector[0].iov_len = sizeof(txtime_str);
197
198 while (1) {
199 *(net_time_t *)CMSG_DATA(cmsg) = txtime + delay;
200
201 ret = sendmsg(data->sock, &msg, 0);
202 if (ret < 0) {
203 if (errno != ENOMEM) {
204 LOG_DBG("Message send failed (%d)", -errno);
205 quit();
206 break;
207 }
208 }
209
210 txtime += interval;
211 snprintk(txtime_str + print_offset,
212 sizeof(txtime_str) - print_offset, "%"PRIx64, (uint64_t)txtime);
213
214 k_sleep(K_NSEC(interval));
215 }
216 }
217
get_local_ipv6(struct net_if * iface,struct sockaddr * peer,struct sockaddr * local,socklen_t * addrlen)218 static int get_local_ipv6(struct net_if *iface, struct sockaddr *peer,
219 struct sockaddr *local, socklen_t *addrlen)
220 {
221 const struct in6_addr *addr;
222
223 if (peer->sa_family != AF_INET6) {
224 return 0;
225 }
226
227 addr = net_if_ipv6_select_src_addr(iface, &net_sin6(peer)->sin6_addr);
228 if (!addr) {
229 LOG_ERR("Cannot get local %s address", "IPv6");
230 return -EINVAL;
231 }
232
233 memcpy(&net_sin6(local)->sin6_addr, addr, sizeof(*addr));
234 local->sa_family = AF_INET6;
235 *addrlen = sizeof(struct sockaddr_in6);
236
237 return 0;
238 }
239
get_local_ipv4(struct net_if * iface,struct sockaddr * peer,struct sockaddr * local,socklen_t * addrlen)240 static int get_local_ipv4(struct net_if *iface, struct sockaddr *peer,
241 struct sockaddr *local, socklen_t *addrlen)
242 {
243 const struct in_addr *addr;
244
245 if (peer->sa_family != AF_INET) {
246 return 0;
247 }
248
249 addr = net_if_ipv4_select_src_addr(iface, &net_sin(peer)->sin_addr);
250 if (!addr) {
251 LOG_ERR("Cannot get local %s address", "IPv4");
252 return -EINVAL;
253 }
254
255 memcpy(&net_sin(local)->sin_addr, addr, sizeof(*addr));
256 local->sa_family = AF_INET;
257 *addrlen = sizeof(struct sockaddr_in);
258
259 return 0;
260 }
261
create_socket(struct net_if * iface,struct sockaddr * peer)262 static int create_socket(struct net_if *iface, struct sockaddr *peer)
263 {
264 struct sockaddr local;
265 socklen_t addrlen;
266 int optval;
267 uint8_t priority;
268 int sock;
269 int ret;
270
271 memset(&local, 0, sizeof(local));
272
273 if (IS_ENABLED(CONFIG_NET_SAMPLE_PACKET_SOCKET)) {
274 struct sockaddr_ll *addr;
275
276 sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
277 if (sock < 0) {
278 LOG_ERR("Cannot create %s socket (%d)", "packet",
279 -errno);
280 return -errno;
281 }
282
283 addr = (struct sockaddr_ll *)&local;
284 addr->sll_ifindex = net_if_get_by_iface(net_if_get_default());
285 addr->sll_family = AF_PACKET;
286 addrlen = sizeof(struct sockaddr_ll);
287
288 LOG_DBG("Binding to interface %d (%p)", addr->sll_ifindex,
289 net_if_get_by_index(addr->sll_ifindex));
290 }
291
292 if (IS_ENABLED(CONFIG_NET_SAMPLE_UDP_SOCKET)) {
293 char addr_str[INET6_ADDRSTRLEN];
294
295 sock = socket(peer->sa_family, SOCK_DGRAM, IPPROTO_UDP);
296 if (sock < 0) {
297 LOG_ERR("Cannot create %s socket (%d)", "UDP", -errno);
298 return -errno;
299 }
300
301 if (IS_ENABLED(CONFIG_NET_IPV6) &&
302 peer->sa_family == AF_INET6) {
303 ret = get_local_ipv6(iface, peer, &local, &addrlen);
304 if (ret < 0) {
305 return ret;
306 }
307
308 net_addr_ntop(AF_INET6, &net_sin6(&local)->sin6_addr,
309 addr_str, sizeof(addr_str));
310 } else if (IS_ENABLED(CONFIG_NET_IPV4) &&
311 peer->sa_family == AF_INET) {
312 ret = get_local_ipv4(iface, peer, &local, &addrlen);
313 if (ret < 0) {
314 return ret;
315 }
316
317 net_addr_ntop(AF_INET, &net_sin(&local)->sin_addr,
318 addr_str, sizeof(addr_str));
319 } else {
320 LOG_ERR("Invalid socket family %d", peer->sa_family);
321 return -EINVAL;
322 }
323
324 LOG_DBG("Binding to %s", addr_str);
325 }
326
327 ret = bind(sock, &local, addrlen);
328 if (ret < 0) {
329 LOG_ERR("Cannot bind socket (%d)", -errno);
330 return -errno;
331 }
332
333 optval = true;
334 ret = setsockopt(sock, SOL_SOCKET, SO_TXTIME, &optval, sizeof(optval));
335 if (ret < 0) {
336 LOG_ERR("Cannot set SO_TXTIME (%d)", -errno);
337 return -errno;
338 }
339
340 priority = NET_PRIORITY_CA;
341 ret = setsockopt(sock, SOL_SOCKET, SO_PRIORITY, &priority,
342 sizeof(priority));
343 if (ret < 0) {
344 LOG_ERR("Cannot set SO_PRIORITY (%d)", -errno);
345 return -errno;
346 }
347
348 return sock;
349 }
350
get_peer_address(struct net_if ** iface,char * addr_str,int addr_str_len)351 static int get_peer_address(struct net_if **iface, char *addr_str,
352 int addr_str_len)
353 {
354 int ret;
355
356 ret = net_ipaddr_parse(CONFIG_NET_SAMPLE_PEER,
357 strlen(CONFIG_NET_SAMPLE_PEER),
358 &peer_data.peer);
359 if (!ret) {
360 LOG_ERR("Cannot parse '%s'", CONFIG_NET_SAMPLE_PEER);
361 return -EINVAL;
362 }
363
364 if (net_sin(&peer_data.peer)->sin_port == 0) {
365 net_sin(&peer_data.peer)->sin_port = htons(4242);
366 }
367
368 if (IS_ENABLED(CONFIG_NET_IPV6) &&
369 peer_data.peer.sa_family == AF_INET6) {
370 *iface = net_if_ipv6_select_src_iface(
371 &net_sin6(&peer_data.peer)->sin6_addr);
372
373 net_addr_ntop(peer_data.peer.sa_family,
374 &net_sin6(&peer_data.peer)->sin6_addr, addr_str,
375 addr_str_len);
376 peer_data.peer_addr_len = sizeof(struct sockaddr_in6);
377
378 } else if (IS_ENABLED(CONFIG_NET_IPV4) &&
379 peer_data.peer.sa_family == AF_INET) {
380 *iface = net_if_ipv4_select_src_iface(
381 &net_sin(&peer_data.peer)->sin_addr);
382
383 net_addr_ntop(peer_data.peer.sa_family,
384 &net_sin(&peer_data.peer)->sin_addr, addr_str,
385 addr_str_len);
386 peer_data.peer_addr_len = sizeof(struct sockaddr_in);
387 }
388
389 return 0;
390 }
391
enable_txtime_for_queues(struct net_if * iface)392 static void enable_txtime_for_queues(struct net_if *iface)
393 {
394 struct ethernet_req_params params = { 0 };
395 int i;
396
397 params.txtime_param.type = ETHERNET_TXTIME_PARAM_TYPE_ENABLE_QUEUES;
398 params.txtime_param.enable_txtime = true;
399
400 for (i = 0; i < NET_TC_TX_COUNT; i++) {
401 params.txtime_param.queue_id = i;
402
403 (void)net_mgmt(NET_REQUEST_ETHERNET_SET_TXTIME_PARAM,
404 iface, ¶ms, sizeof(params));
405 }
406 }
407
set_qbv_params(struct net_if * iface)408 static void set_qbv_params(struct net_if *iface)
409 {
410 struct ethernet_req_params params = { 0 };
411 int i, ret;
412 int ports_count = 1, row;
413
414 /* Assume only one port atm, the amount of ports could be
415 * queried from controller by ETHERNET_CONFIG_TYPE_PORTS_NUM
416 */
417
418 /* Set some defaults */
419 LOG_DBG("Setting Qbv parameters to %d port%s", ports_count,
420 ports_count > 1 ? "s" : "");
421
422 /* One Qbv setting example:
423 *
424 * Start time: after 20s of current configuring base time
425 * Cycle time: 20ms
426 * Number GCL list: 2
427 * GCL list 0 cycle time: 10ms
428 * GCL list 0 'set' gate open: Txq1 (default queue),
429 * Txq3 (highest priority queue)
430 * GCL list 1 cycle time: 10ms
431 * GCL list 1 'set' gate open: Txq0 (background queue)
432 */
433
434 for (i = 0; i < ports_count; ++i) {
435 /* Turn on the gate control to first two gates (just for demo
436 * purposes)
437 */
438 for (row = 0; row < 2; row++) {
439 memset(¶ms, 0, sizeof(params));
440
441 params.qbv_param.port_id = i;
442 params.qbv_param.type =
443 ETHERNET_QBV_PARAM_TYPE_GATE_CONTROL_LIST;
444 params.qbv_param.gate_control.operation = ETHERNET_SET_GATE_STATE;
445 params.qbv_param.gate_control.time_interval = 10000000UL;
446 params.qbv_param.gate_control.row = row;
447
448 if (row == 0) {
449 params.qbv_param.gate_control.
450 gate_status[net_tx_priority2tc(NET_PRIORITY_CA)] = true;
451 params.qbv_param.gate_control.
452 gate_status[net_tx_priority2tc(NET_PRIORITY_BE)] = true;
453 } else if (row == 1) {
454 params.qbv_param.gate_control.
455 gate_status[net_tx_priority2tc(NET_PRIORITY_BK)] = true;
456 }
457
458 ret = net_mgmt(NET_REQUEST_ETHERNET_SET_QBV_PARAM,
459 iface, ¶ms,
460 sizeof(struct ethernet_req_params));
461 if (ret) {
462 LOG_ERR("Could not set %s%s (%d) to port %d",
463 "gate control list", "", ret, i);
464 }
465 }
466
467 memset(¶ms, 0, sizeof(params));
468
469 params.qbv_param.port_id = i;
470 params.qbv_param.type =
471 ETHERNET_QBV_PARAM_TYPE_GATE_CONTROL_LIST_LEN;
472 params.qbv_param.gate_control_list_len = MIN(NET_TC_TX_COUNT, 2);
473
474 ret = net_mgmt(NET_REQUEST_ETHERNET_SET_QBV_PARAM, iface,
475 ¶ms, sizeof(struct ethernet_req_params));
476 if (ret) {
477 LOG_ERR("Could not set %s%s (%d) to port %d",
478 "gate control list", " len", ret, i);
479 }
480
481 memset(¶ms, 0, sizeof(params));
482
483 params.qbv_param.port_id = i;
484 params.qbv_param.type = ETHERNET_QBV_PARAM_TYPE_TIME;
485 params.qbv_param.base_time.second = 20ULL;
486 params.qbv_param.base_time.fract_nsecond = 0ULL;
487 params.qbv_param.cycle_time.second = 0ULL;
488 params.qbv_param.cycle_time.nanosecond = 20000000UL;
489 params.qbv_param.extension_time = 0UL;
490
491 ret = net_mgmt(NET_REQUEST_ETHERNET_SET_QBV_PARAM, iface,
492 ¶ms, sizeof(struct ethernet_req_params));
493 if (ret) {
494 LOG_ERR("Could not set %s%s (%d) to port %d",
495 "base time", "", ret, i);
496 }
497 }
498 }
499
cmd_sample_quit(const struct shell * sh,size_t argc,char * argv[])500 static int cmd_sample_quit(const struct shell *sh,
501 size_t argc, char *argv[])
502 {
503 want_to_quit = true;
504
505 quit();
506
507 conn_mgr_mon_resend_status();
508
509 return 0;
510 }
511
512 SHELL_STATIC_SUBCMD_SET_CREATE(sample_commands,
513 SHELL_CMD(quit, NULL,
514 "Quit the sample application\n",
515 cmd_sample_quit),
516 SHELL_SUBCMD_SET_END
517 );
518
519 SHELL_CMD_REGISTER(sample, &sample_commands,
520 "Sample application commands", NULL);
521
main(void)522 int main(void)
523 {
524 struct net_if *iface = NULL;
525 char addr_str[INET6_ADDRSTRLEN];
526 enum ethernet_hw_caps caps;
527 int ret, if_index;
528
529 LOG_INF(APP_BANNER);
530
531 k_sem_init(&quit_lock, 0, UINT_MAX);
532
533 if (IS_ENABLED(CONFIG_NET_CONNECTION_MANAGER)) {
534 net_mgmt_init_event_callback(&mgmt_cb,
535 event_handler, EVENT_MASK);
536 net_mgmt_add_event_callback(&mgmt_cb);
537
538 if (IS_ENABLED(CONFIG_NET_DHCPV4)) {
539 net_mgmt_init_event_callback(&dhcpv4_cb,
540 event_handler,
541 DHCPV4_MASK);
542 net_mgmt_add_event_callback(&dhcpv4_cb);
543 }
544
545 conn_mgr_mon_resend_status();
546 }
547
548 /* The VLAN in this example is created for demonstration purposes.
549 */
550 if (IS_ENABLED(CONFIG_NET_VLAN)) {
551 ret = init_vlan();
552 if (ret < 0) {
553 LOG_WRN("Cannot setup VLAN (%d)", ret);
554 }
555 }
556
557 /* Wait for the connection. */
558 k_sem_take(&run_app, K_FOREVER);
559
560 if (IS_ENABLED(CONFIG_NET_SAMPLE_UDP_SOCKET)) {
561 ret = get_peer_address(&iface, addr_str, sizeof(addr_str));
562 if (ret < 0) {
563 return 0;
564 }
565 } else {
566 struct sockaddr_ll *addr = (struct sockaddr_ll *)&peer_data.peer;
567
568 addr->sll_ifindex = net_if_get_by_iface(net_if_get_default());
569 addr->sll_family = AF_PACKET;
570 peer_data.peer_addr_len = sizeof(struct sockaddr_ll);
571 iface = net_if_get_by_index(addr->sll_ifindex);
572 }
573
574 if (!iface) {
575 LOG_ERR("Cannot get local network interface!");
576 return 0;
577 }
578
579 if_index = net_if_get_by_iface(iface);
580
581 caps = net_eth_get_hw_capabilities(iface);
582 if (!(caps & ETHERNET_PTP)) {
583 LOG_ERR("Interface %p does not support %s", iface, "PTP");
584 return 0;
585 }
586
587 if (!(caps & ETHERNET_TXTIME)) {
588 LOG_ERR("Interface %p does not support %s", iface, "TXTIME");
589 return 0;
590 }
591
592 peer_data.clk = net_eth_get_ptp_clock_by_index(if_index);
593 if (!peer_data.clk) {
594 LOG_ERR("Interface %p does not support %s", iface,
595 "PTP clock");
596 return 0;
597 }
598
599 /* Make sure the queues are enabled */
600 if (IS_ENABLED(CONFIG_NET_L2_ETHERNET_MGMT) && (NET_TC_TX_COUNT > 0)) {
601 enable_txtime_for_queues(iface);
602
603 /* Set Qbv options if they are available */
604 if (caps & ETHERNET_QBV) {
605 set_qbv_params(iface);
606 }
607 }
608
609 if (IS_ENABLED(CONFIG_NET_SAMPLE_UDP_SOCKET)) {
610 LOG_INF("Socket SO_TXTIME sample to %s port %d using "
611 "interface %d (%p) and PTP clock %p",
612 addr_str,
613 ntohs(net_sin(&peer_data.peer)->sin_port),
614 if_index, iface, peer_data.clk);
615 }
616
617 if (IS_ENABLED(CONFIG_NET_SAMPLE_PACKET_SOCKET)) {
618 LOG_INF("Socket SO_TXTIME sample using AF_PACKET and "
619 "interface %d (%p) and PTP clock %p",
620 if_index, iface, peer_data.clk);
621 }
622
623 peer_data.sock = create_socket(iface, &peer_data.peer);
624 if (peer_data.sock < 0) {
625 LOG_ERR("Cannot create socket (%d)", peer_data.sock);
626 return 0;
627 }
628
629 tx_tid = k_thread_create(&tx_thread, tx_stack,
630 K_THREAD_STACK_SIZEOF(tx_stack),
631 tx, &peer_data,
632 NULL, NULL, THREAD_PRIORITY, 0,
633 K_FOREVER);
634 if (!tx_tid) {
635 LOG_ERR("Cannot create TX thread!");
636 return 0;
637 }
638
639 k_thread_name_set(tx_tid, "TX");
640
641 rx_tid = k_thread_create(&rx_thread, rx_stack,
642 K_THREAD_STACK_SIZEOF(rx_stack),
643 rx, &peer_data,
644 NULL, NULL, THREAD_PRIORITY, 0,
645 K_FOREVER);
646 if (!rx_tid) {
647 LOG_ERR("Cannot create RX thread!");
648 return 0;
649 }
650
651 k_thread_name_set(rx_tid, "RX");
652
653 k_thread_start(rx_tid);
654 k_thread_start(tx_tid);
655
656 k_sem_take(&quit_lock, K_FOREVER);
657
658 LOG_INF("Stopping...");
659
660 k_thread_abort(tx_tid);
661 k_thread_abort(rx_tid);
662
663 if (peer_data.sock >= 0) {
664 (void)close(peer_data.sock);
665 }
666 return 0;
667 }
668
