1 /*
2 * Copyright (c) 2024 BayLibre SAS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(ptp, CONFIG_PTP_LOG_LEVEL);
9
10 #include <errno.h>
11 #include <stdbool.h>
12
13 #include <zephyr/kernel.h>
14 #include <zephyr/net/net_if.h>
15 #include <zephyr/net/ptp.h>
16
17 #include "clock.h"
18 #include "port.h"
19 #include "transport.h"
20
21 K_KERNEL_STACK_DEFINE(ptp_stack, CONFIG_PTP_STACK_SIZE);
22
23 static struct k_thread ptp_thread_data;
24
ptp_thread(void * p1,void * p2,void * p3)25 static void ptp_thread(void *p1, void *p2, void *p3)
26 {
27 static const size_t timer_offset[] = {
28 offsetof(struct ptp_port, timers.announce),
29 offsetof(struct ptp_port, timers.delay),
30 offsetof(struct ptp_port, timers.sync),
31 offsetof(struct ptp_port, timers.qualification)
32 };
33 static const int timeout_bit[] = {
34 PTP_PORT_TIMER_ANNOUNCE_TO,
35 PTP_PORT_TIMER_DELAY_TO,
36 PTP_PORT_TIMER_SYNC_TO,
37 PTP_PORT_TIMER_QUALIFICATION_TO,
38 };
39
40 struct k_timer *timer;
41 struct ptp_port *port;
42 struct zsock_pollfd *fd;
43 enum ptp_port_event event;
44
45 ARG_UNUSED(p1);
46 ARG_UNUSED(p2);
47 ARG_UNUSED(p3);
48
49 while (1) {
50 fd = ptp_clock_poll_sockets();
51
52 SYS_SLIST_FOR_EACH_CONTAINER(ptp_clock_ports_list(), port, node) {
53
54 for (int i = 0; i < ARRAY_SIZE(timer_offset); i++) {
55 timer = (struct k_timer *)((uint8_t *)port +
56 timer_offset[i]);
57
58 if (!atomic_test_bit(&port->timeouts, timeout_bit[i])) {
59 continue;
60 }
61
62 event = ptp_port_timer_event_gen(port, timer);
63
64 if (event == PTP_EVT_STATE_DECISION ||
65 event == PTP_EVT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES) {
66 ptp_clock_state_decision_req();
67 }
68
69 ptp_port_event_handle(port, event, false);
70 }
71
72 for (int i = 0; i < PTP_SOCKET_CNT; i++, fd++) {
73 if (!(fd->revents & (ZSOCK_POLLIN | ZSOCK_POLLPRI))) {
74 continue;
75 }
76
77 event = ptp_port_event_gen(port, i);
78
79 if (event == PTP_EVT_STATE_DECISION ||
80 event == PTP_EVT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES) {
81 ptp_clock_state_decision_req();
82 }
83
84 ptp_port_event_handle(port, event, false);
85 }
86 }
87
88 ptp_clock_handle_state_decision_evt();
89 }
90 }
91
ptp_init(void)92 static int ptp_init(void)
93 {
94 k_tid_t tid;
95 const struct ptp_clock *domain = ptp_clock_init();
96 struct ptp_port *port;
97
98 if (!domain) {
99 return -ENODEV;
100 }
101
102 net_if_foreach(ptp_port_init, NULL);
103
104 SYS_SLIST_FOR_EACH_CONTAINER(ptp_clock_ports_list(), port, node) {
105 ptp_port_event_handle(port, PTP_EVT_INITIALIZE, false);
106 }
107
108 tid = k_thread_create(&ptp_thread_data, ptp_stack, K_KERNEL_STACK_SIZEOF(ptp_stack),
109 ptp_thread, NULL, NULL, NULL,
110 K_PRIO_COOP(1), 0, K_NO_WAIT);
111 k_thread_name_set(&ptp_thread_data, "PTP");
112
113 return 0;
114 }
115
116 SYS_INIT(ptp_init, APPLICATION, CONFIG_PTP_INIT_PRIO);
117
