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_port, CONFIG_PTP_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/net/net_if.h>
12 #include <zephyr/net/ptp.h>
13 #include <zephyr/net/ptp_time.h>
14 #include <zephyr/random/random.h>
15
16 #include "btca.h"
17 #include "clock.h"
18 #include "port.h"
19 #include "msg.h"
20 #include "tlv.h"
21 #include "transport.h"
22
23 #define DEFAULT_LOG_MSG_INTERVAL (0x7F)
24
25 #define PORT_DELAY_REQ_CLEARE_TO (3 * NSEC_PER_SEC)
26
27 #define PORT_LINK_UP BIT(0)
28 #define PORT_LINK_DOWN BIT(1)
29 #define PORT_LINK_CHANGED BIT(2)
30 #define PORT_LINK_EVENT_MASK (NET_EVENT_IF_DOWN | NET_EVENT_IF_UP)
31
32 static struct ptp_port ports[CONFIG_PTP_NUM_PORTS];
33 static struct k_mem_slab foreign_tts_slab;
34 #if CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_FEATURE
35 BUILD_ASSERT(CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_RECORD_SIZE >= 5 * CONFIG_PTP_NUM_PORTS,
36 "PTP_FOREIGN_TIME_TRANSMITTER_RECORD_SIZE is smaller than expected!");
37
38 K_MEM_SLAB_DEFINE_STATIC(foreign_tts_slab,
39 sizeof(struct ptp_foreign_tt_clock),
40 CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_RECORD_SIZE,
41 8);
42 #endif
43
44 char str_port_id[] = "FF:FF:FF:FF:FF:FF:FF:FF-FFFF";
45
port_id_str(struct ptp_port_id * port_id)46 const char *port_id_str(struct ptp_port_id *port_id)
47 {
48 uint8_t *pid = port_id->clk_id.id;
49
50 snprintk(str_port_id, sizeof(str_port_id), "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X-%04X",
51 pid[0],
52 pid[1],
53 pid[2],
54 pid[3],
55 pid[4],
56 pid[5],
57 pid[6],
58 pid[7],
59 port_id->port_number);
60
61 return str_port_id;
62 }
63
port_state_str(enum ptp_port_state state)64 static const char *port_state_str(enum ptp_port_state state)
65 {
66 const static char * const states[] = {
67 [PTP_PS_INITIALIZING] = "INITIALIZING",
68 [PTP_PS_FAULTY] = "FAULTY",
69 [PTP_PS_DISABLED] = "DISABLED",
70 [PTP_PS_LISTENING] = "LISTENING",
71 [PTP_PS_PRE_TIME_TRANSMITTER] = "PRE TIME TRANSMITTER",
72 [PTP_PS_TIME_TRANSMITTER] = "TIME TRANSMITTER",
73 [PTP_PS_GRAND_MASTER] = "GRAND MASTER",
74 [PTP_PS_PASSIVE] = "PASSIVE",
75 [PTP_PS_UNCALIBRATED] = "UNCALIBRATED",
76 [PTP_PS_TIME_RECEIVER] = "TIME RECEIVER",
77 };
78
79 return states[state];
80 }
81
port_msg_send(struct ptp_port * port,struct ptp_msg * msg,enum ptp_socket idx)82 static int port_msg_send(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx)
83 {
84 ptp_msg_pre_send(msg);
85
86 return ptp_transport_send(port, msg, idx);
87 }
88
port_timer_set_timeout(struct k_timer * timer,uint8_t factor,int8_t log_seconds)89 static void port_timer_set_timeout(struct k_timer *timer, uint8_t factor, int8_t log_seconds)
90 {
91 uint64_t timeout = log_seconds < 0 ?
92 (uint64_t)((NSEC_PER_SEC * factor) >> (log_seconds * -1)) :
93 (uint64_t)((NSEC_PER_SEC * factor) << log_seconds);
94
95 k_timer_start(timer, K_NSEC(timeout), K_NO_WAIT);
96 }
97
port_timer_set_timeout_random(struct k_timer * timer,int min_factor,int span,int log_seconds)98 static void port_timer_set_timeout_random(struct k_timer *timer,
99 int min_factor,
100 int span,
101 int log_seconds)
102 {
103 uint64_t timeout, random_ns;
104
105 if (log_seconds < 0) {
106 timeout = (uint64_t)((NSEC_PER_SEC * min_factor) >> -log_seconds);
107 random_ns = (uint64_t)(NSEC_PER_SEC >> -log_seconds);
108 } else {
109 timeout = (uint64_t)((NSEC_PER_SEC * min_factor) << log_seconds);
110 random_ns = (uint64_t)((span * NSEC_PER_SEC) << log_seconds);
111 }
112
113 timeout = (uint64_t)(timeout + (random_ns * (sys_rand32_get() % (1 << 15) + 1) >> 15));
114 k_timer_start(timer, K_NSEC(timeout), K_NO_WAIT);
115 }
116
port_synchronize(struct ptp_port * port,struct net_ptp_time ingress_ts,struct net_ptp_time origin_ts,ptp_timeinterval correction1,ptp_timeinterval correction2)117 static void port_synchronize(struct ptp_port *port,
118 struct net_ptp_time ingress_ts,
119 struct net_ptp_time origin_ts,
120 ptp_timeinterval correction1,
121 ptp_timeinterval correction2)
122 {
123 uint64_t t1, t2, t1c;
124
125 t1 = origin_ts.second * NSEC_PER_SEC + origin_ts.nanosecond;
126 t2 = ingress_ts.second * NSEC_PER_SEC + ingress_ts.nanosecond;
127 t1c = t1 + (correction1 >> 16) + (correction2 >> 16);
128
129 ptp_clock_synchronize(t2, t1c);
130
131 port_timer_set_timeout(&port->timers.sync,
132 port->port_ds.announce_receipt_timeout,
133 port->port_ds.log_sync_interval);
134 }
135
port_ds_init(struct ptp_port * port)136 static void port_ds_init(struct ptp_port *port)
137 {
138 struct ptp_port_ds *ds = &port->port_ds;
139 const struct ptp_default_ds *dds = ptp_clock_default_ds();
140
141 memcpy(&ds->id.clk_id, &dds->clk_id, sizeof(ptp_clk_id));
142 ds->id.port_number = dds->n_ports + 1;
143
144 ds->state = PTP_PS_INITIALIZING;
145 ds->log_min_delay_req_interval = CONFIG_PTP_MIN_DELAY_REQ_LOG_INTERVAL;
146 ds->log_announce_interval = CONFIG_PTP_ANNOUNCE_LOG_INTERVAL;
147 ds->announce_receipt_timeout = CONFIG_PTP_ANNOUNCE_RECV_TIMEOUT;
148 ds->log_sync_interval = CONFIG_PTP_SYNC_LOG_INTERVAL;
149 ds->delay_mechanism = PTP_DM_E2E;
150 ds->log_min_pdelay_req_interval = CONFIG_PTP_MIN_PDELAY_REQ_LOG_INTERVAL;
151 ds->version = PTP_VERSION;
152 ds->delay_asymmetry = 0;
153 }
154
port_delay_req_timestamp_cb(struct net_pkt * pkt)155 static void port_delay_req_timestamp_cb(struct net_pkt *pkt)
156 {
157 struct ptp_port *port = ptp_clock_port_from_iface(pkt->iface);
158 struct ptp_msg *req, *msg = ptp_msg_from_pkt(pkt);
159 sys_snode_t *iter, *last = NULL;
160
161 if (!port || !msg) {
162 return;
163 }
164
165 msg->header.src_port_id.port_number = ntohs(msg->header.src_port_id.port_number);
166
167 if (!ptp_port_id_eq(&port->port_ds.id, &msg->header.src_port_id) ||
168 ptp_msg_type(msg) != PTP_MSG_DELAY_REQ) {
169 return;
170 }
171
172 for (iter = sys_slist_peek_head(&port->delay_req_list);
173 iter;
174 iter = sys_slist_peek_next(iter), last = iter) {
175
176 req = CONTAINER_OF(iter, struct ptp_msg, node);
177
178 if (req->header.sequence_id != msg->header.sequence_id) {
179 continue;
180 }
181
182 if (pkt->timestamp.second == UINT64_MAX ||
183 (pkt->timestamp.second == 0 && pkt->timestamp.nanosecond == 0)) {
184 net_if_unregister_timestamp_cb(&port->delay_req_ts_cb);
185 sys_slist_remove(&port->delay_req_list, last, iter);
186 ptp_msg_unref(req);
187 return;
188 }
189
190 req->timestamp.host._sec.high = pkt->timestamp._sec.high;
191 req->timestamp.host._sec.low = pkt->timestamp._sec.low;
192 req->timestamp.host.nanosecond = pkt->timestamp.nanosecond;
193
194 LOG_DBG("Port %d registered timestamp for %d Delay_Req",
195 port->port_ds.id.port_number,
196 ntohs(msg->header.sequence_id));
197
198 if (iter == sys_slist_peek_tail(&port->delay_req_list)) {
199 net_if_unregister_timestamp_cb(&port->delay_req_ts_cb);
200 }
201 }
202 }
203
port_sync_timestamp_cb(struct net_pkt * pkt)204 static void port_sync_timestamp_cb(struct net_pkt *pkt)
205 {
206 struct ptp_port *port = ptp_clock_port_from_iface(pkt->iface);
207 struct ptp_msg *msg = ptp_msg_from_pkt(pkt);
208
209 if (!port || !msg) {
210 return;
211 }
212
213 msg->header.src_port_id.port_number = ntohs(msg->header.src_port_id.port_number);
214
215 if (ptp_port_id_eq(&port->port_ds.id, &msg->header.src_port_id) &&
216 ptp_msg_type(msg) == PTP_MSG_SYNC) {
217
218 const struct ptp_default_ds *dds = ptp_clock_default_ds();
219 const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
220 struct ptp_msg *resp = ptp_msg_alloc();
221
222 if (!resp) {
223 return;
224 }
225
226 resp->header.type_major_sdo_id = PTP_MSG_FOLLOW_UP;
227 resp->header.version = PTP_VERSION;
228 resp->header.msg_length = sizeof(struct ptp_follow_up_msg);
229 resp->header.domain_number = dds->domain;
230 resp->header.flags[1] = tpds->flags;
231 resp->header.src_port_id = port->port_ds.id;
232 resp->header.sequence_id = port->seq_id.sync++;
233 resp->header.log_msg_interval = port->port_ds.log_sync_interval;
234
235 resp->follow_up.precise_origin_timestamp.seconds_high = pkt->timestamp._sec.high;
236 resp->follow_up.precise_origin_timestamp.seconds_low = pkt->timestamp._sec.low;
237 resp->follow_up.precise_origin_timestamp.nanoseconds = pkt->timestamp.nanosecond;
238
239 net_if_unregister_timestamp_cb(&port->sync_ts_cb);
240
241 port_msg_send(port, resp, PTP_SOCKET_GENERAL);
242 ptp_msg_unref(resp);
243
244 LOG_DBG("Port %d sends Follow_Up message", port->port_ds.id.port_number);
245 }
246 }
247
port_announce_msg_transmit(struct ptp_port * port)248 static int port_announce_msg_transmit(struct ptp_port *port)
249 {
250 const struct ptp_parent_ds *pds = ptp_clock_parent_ds();
251 const struct ptp_current_ds *cds = ptp_clock_current_ds();
252 const struct ptp_default_ds *dds = ptp_clock_default_ds();
253 const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
254 struct ptp_msg *msg = ptp_msg_alloc();
255 int ret;
256
257 if (!msg) {
258 return -ENOMEM;
259 }
260
261 msg->header.type_major_sdo_id = PTP_MSG_ANNOUNCE;
262 msg->header.version = PTP_VERSION;
263 msg->header.msg_length = sizeof(struct ptp_announce_msg);
264 msg->header.domain_number = dds->domain;
265 msg->header.flags[1] = tpds->flags;
266 msg->header.src_port_id = port->port_ds.id;
267 msg->header.sequence_id = port->seq_id.announce++;
268 msg->header.log_msg_interval = port->port_ds.log_sync_interval;
269
270 msg->announce.current_utc_offset = tpds->current_utc_offset;
271 msg->announce.gm_priority1 = pds->gm_priority1;
272 msg->announce.gm_clk_quality = pds->gm_clk_quality;
273 msg->announce.gm_priority2 = pds->gm_priority2;
274 msg->announce.gm_id = pds->gm_id;
275 msg->announce.steps_rm = cds->steps_rm;
276 msg->announce.time_src = tpds->time_src;
277
278 ret = port_msg_send(port, msg, PTP_SOCKET_GENERAL);
279 ptp_msg_unref(msg);
280
281 if (ret < 0) {
282 return -EFAULT;
283 }
284
285 LOG_DBG("Port %d sends Announce message", port->port_ds.id.port_number);
286 return 0;
287 }
288
port_delay_req_msg_transmit(struct ptp_port * port)289 static int port_delay_req_msg_transmit(struct ptp_port *port)
290 {
291 const struct ptp_default_ds *dds = ptp_clock_default_ds();
292 struct ptp_msg *msg = ptp_msg_alloc();
293 int ret;
294
295 if (!msg) {
296 return -ENOMEM;
297 }
298
299 msg->header.type_major_sdo_id = PTP_MSG_DELAY_REQ;
300 msg->header.version = PTP_VERSION;
301 msg->header.msg_length = sizeof(struct ptp_delay_req_msg);
302 msg->header.domain_number = dds->domain;
303 msg->header.src_port_id = port->port_ds.id;
304 msg->header.sequence_id = port->seq_id.delay++;
305 msg->header.log_msg_interval = DEFAULT_LOG_MSG_INTERVAL;
306
307 net_if_register_timestamp_cb(&port->delay_req_ts_cb,
308 NULL,
309 port->iface,
310 port_delay_req_timestamp_cb);
311
312 ret = port_msg_send(port, msg, PTP_SOCKET_EVENT);
313 if (ret < 0) {
314 ptp_msg_unref(msg);
315 return -EFAULT;
316 }
317
318 sys_slist_append(&port->delay_req_list, &msg->node);
319
320 LOG_DBG("Port %d sends Delay_Req message", port->port_ds.id.port_number);
321 return 0;
322 }
323
port_sync_msg_transmit(struct ptp_port * port)324 static int port_sync_msg_transmit(struct ptp_port *port)
325 {
326 const struct ptp_default_ds *dds = ptp_clock_default_ds();
327 const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
328 struct ptp_msg *msg = ptp_msg_alloc();
329 int ret;
330
331 if (!msg) {
332 return -ENOMEM;
333 }
334
335 msg->header.type_major_sdo_id = PTP_MSG_SYNC;
336 msg->header.version = PTP_VERSION;
337 msg->header.msg_length = sizeof(struct ptp_sync_msg);
338 msg->header.domain_number = dds->domain;
339 msg->header.flags[0] = PTP_MSG_TWO_STEP_FLAG;
340 msg->header.flags[1] = tpds->flags;
341 msg->header.src_port_id = port->port_ds.id;
342 msg->header.sequence_id = port->seq_id.sync;
343 msg->header.log_msg_interval = port->port_ds.log_sync_interval;
344
345 net_if_register_timestamp_cb(&port->sync_ts_cb,
346 NULL,
347 port->iface,
348 port_sync_timestamp_cb);
349
350 ret = port_msg_send(port, msg, PTP_SOCKET_EVENT);
351 ptp_msg_unref(msg);
352
353 if (ret < 0) {
354 return -EFAULT;
355 }
356 LOG_DBG("Port %d sends Sync message", port->port_ds.id.port_number);
357 return 0;
358 }
359
port_timer_init(struct k_timer * timer,k_timer_expiry_t timeout_fn,void * user_data)360 static void port_timer_init(struct k_timer *timer, k_timer_expiry_t timeout_fn, void *user_data)
361 {
362 k_timer_init(timer, timeout_fn, NULL);
363 k_timer_user_data_set(timer, user_data);
364 }
365
port_timer_to_handler(struct k_timer * timer)366 static void port_timer_to_handler(struct k_timer *timer)
367 {
368 struct ptp_port *port = (struct ptp_port *)k_timer_user_data_get(timer);
369
370 if (timer == &port->timers.announce) {
371 atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO);
372 } else if (timer == &port->timers.sync) {
373 atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO);
374 } else if (timer == &port->timers.delay) {
375 atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_DELAY_TO);
376 } else if (timer == &port->timers.qualification) {
377 atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_QUALIFICATION_TO);
378 }
379
380 ptp_clock_signal_timeout();
381 }
382
foreign_clock_cleanup(struct ptp_foreign_tt_clock * foreign)383 static void foreign_clock_cleanup(struct ptp_foreign_tt_clock *foreign)
384 {
385 struct ptp_msg *msg;
386 int64_t timestamp, timeout, current = k_uptime_get() * NSEC_PER_MSEC;
387
388 while (foreign->messages_count > FOREIGN_TIME_TRANSMITTER_THRESHOLD) {
389 msg = (struct ptp_msg *)k_fifo_get(&foreign->messages, K_NO_WAIT);
390 ptp_msg_unref(msg);
391 foreign->messages_count--;
392 }
393
394 /* Remove messages that don't arrived at
395 * FOREIGN_TIME_TRANSMITTER_TIME_WINDOW (4 * announce interval) - IEEE 1588-2019 9.3.2.4.5
396 */
397 while (!k_fifo_is_empty(&foreign->messages)) {
398 msg = (struct ptp_msg *)k_fifo_peek_head(&foreign->messages);
399
400 if (msg->header.log_msg_interval <= -31) {
401 timeout = 0;
402 } else if (msg->header.log_msg_interval >= 31) {
403 timeout = INT64_MAX;
404 } else if (msg->header.log_msg_interval > 0) {
405 timeout = FOREIGN_TIME_TRANSMITTER_TIME_WINDOW_MUL *
406 (1 << msg->header.log_msg_interval) * NSEC_PER_SEC;
407 } else {
408 timeout = FOREIGN_TIME_TRANSMITTER_TIME_WINDOW_MUL * NSEC_PER_SEC /
409 (1 << (-msg->header.log_msg_interval));
410 }
411
412 timestamp = msg->timestamp.host.second * NSEC_PER_SEC +
413 msg->timestamp.host.nanosecond;
414
415 if (current - timestamp < timeout) {
416 /* Remaining messages are within time window */
417 break;
418 }
419
420 msg = (struct ptp_msg *)k_fifo_get(&foreign->messages, K_NO_WAIT);
421 ptp_msg_unref(msg);
422 foreign->messages_count--;
423 }
424 }
425
port_clear_foreign_clock_records(struct ptp_foreign_tt_clock * foreign)426 static void port_clear_foreign_clock_records(struct ptp_foreign_tt_clock *foreign)
427 {
428 struct ptp_msg *msg;
429
430 while (!k_fifo_is_empty(&foreign->messages)) {
431 msg = (struct ptp_msg *)k_fifo_get(&foreign->messages, K_NO_WAIT);
432 ptp_msg_unref(msg);
433 foreign->messages_count--;
434 }
435 }
436
port_delay_req_cleanup(struct ptp_port * port)437 static void port_delay_req_cleanup(struct ptp_port *port)
438 {
439 sys_snode_t *prev = NULL;
440 struct ptp_msg *msg;
441 int64_t timestamp, current = k_uptime_get() * NSEC_PER_MSEC;
442
443 SYS_SLIST_FOR_EACH_CONTAINER(&port->delay_req_list, msg, node) {
444 timestamp = msg->timestamp.host.second * NSEC_PER_SEC +
445 msg->timestamp.host.nanosecond;
446
447 if (current - timestamp < PORT_DELAY_REQ_CLEARE_TO) {
448 break;
449 }
450
451 ptp_msg_unref(msg);
452 sys_slist_remove(&port->delay_req_list, prev, &msg->node);
453 prev = &msg->node;
454 }
455 }
456
port_clear_delay_req(struct ptp_port * port)457 static void port_clear_delay_req(struct ptp_port *port)
458 {
459 sys_snode_t *prev = NULL;
460 struct ptp_msg *msg;
461
462 SYS_SLIST_FOR_EACH_CONTAINER(&port->delay_req_list, msg, node) {
463 ptp_msg_unref(msg);
464 sys_slist_remove(&port->delay_req_list, prev, &msg->node);
465 prev = &msg->node;
466 }
467 }
468
port_sync_fup_ooo_handle(struct ptp_port * port,struct ptp_msg * msg)469 static void port_sync_fup_ooo_handle(struct ptp_port *port, struct ptp_msg *msg)
470 {
471 struct ptp_msg *last = port->last_sync_fup;
472
473 if (ptp_msg_type(msg) != PTP_MSG_FOLLOW_UP &&
474 ptp_msg_type(msg) != PTP_MSG_SYNC) {
475 return;
476 }
477
478 if (!last) {
479 port->last_sync_fup = msg;
480 ptp_msg_ref(msg);
481 return;
482 }
483
484 if (ptp_msg_type(last) == PTP_MSG_SYNC &&
485 ptp_msg_type(msg) == PTP_MSG_FOLLOW_UP &&
486 msg->header.sequence_id == last->header.sequence_id) {
487
488 port_synchronize(port,
489 last->timestamp.host,
490 msg->timestamp.protocol,
491 last->header.correction,
492 msg->header.correction);
493
494 ptp_msg_unref(port->last_sync_fup);
495 port->last_sync_fup = NULL;
496 } else if (ptp_msg_type(last) == PTP_MSG_FOLLOW_UP &&
497 ptp_msg_type(msg) == PTP_MSG_SYNC &&
498 msg->header.sequence_id == last->header.sequence_id) {
499
500 port_synchronize(port,
501 msg->timestamp.host,
502 last->timestamp.protocol,
503 msg->header.correction,
504 last->header.correction);
505
506 ptp_msg_unref(port->last_sync_fup);
507 port->last_sync_fup = NULL;
508 } else {
509 ptp_msg_unref(port->last_sync_fup);
510 port->last_sync_fup = msg;
511 ptp_msg_ref(msg);
512 }
513 }
514
port_announce_msg_process(struct ptp_port * port,struct ptp_msg * msg)515 static int port_announce_msg_process(struct ptp_port *port, struct ptp_msg *msg)
516 {
517 int ret = 0;
518 const struct ptp_default_ds *dds = ptp_clock_default_ds();
519
520 if (msg->announce.steps_rm >= dds->max_steps_rm) {
521 return ret;
522 }
523
524 switch (ptp_port_state(port)) {
525 case PTP_PS_INITIALIZING:
526 __fallthrough;
527 case PTP_PS_DISABLED:
528 __fallthrough;
529 case PTP_PS_FAULTY:
530 break;
531 case PTP_PS_LISTENING:
532 __fallthrough;
533 case PTP_PS_PRE_TIME_TRANSMITTER:
534 __fallthrough;
535 case PTP_PS_TIME_TRANSMITTER:
536 __fallthrough;
537 case PTP_PS_GRAND_MASTER:
538 #if CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_FEATURE
539 ret = ptp_port_add_foreign_tt(port, msg);
540 break;
541 #else
542 __fallthrough;
543 #endif
544 case PTP_PS_TIME_RECEIVER:
545 __fallthrough;
546 case PTP_PS_UNCALIBRATED:
547 __fallthrough;
548 case PTP_PS_PASSIVE:
549 ret = ptp_port_update_current_time_transmitter(port, msg);
550 break;
551 default:
552 break;
553 }
554
555 return ret;
556 }
557
port_sync_msg_process(struct ptp_port * port,struct ptp_msg * msg)558 static void port_sync_msg_process(struct ptp_port *port, struct ptp_msg *msg)
559 {
560 enum ptp_port_state state = ptp_port_state(port);
561
562 if (state != PTP_PS_TIME_RECEIVER && state != PTP_PS_UNCALIBRATED) {
563 return;
564 }
565
566 if (!ptp_msg_current_parent(msg)) {
567 return;
568 }
569
570 if (port->port_ds.log_sync_interval != msg->header.log_msg_interval) {
571 port->port_ds.log_sync_interval = msg->header.log_msg_interval;
572 }
573
574 msg->header.correction += port->port_ds.delay_asymmetry;
575
576 if (!(msg->header.flags[0] & PTP_MSG_TWO_STEP_FLAG)) {
577 port_synchronize(port,
578 msg->timestamp.host,
579 msg->timestamp.protocol,
580 msg->header.correction,
581 0);
582
583 if (port->last_sync_fup) {
584 ptp_msg_unref(port->last_sync_fup);
585 port->last_sync_fup = NULL;
586 }
587
588 return;
589 }
590
591 port_sync_fup_ooo_handle(port, msg);
592 }
593
port_follow_up_msg_process(struct ptp_port * port,struct ptp_msg * msg)594 static void port_follow_up_msg_process(struct ptp_port *port, struct ptp_msg *msg)
595 {
596 enum ptp_port_state state = ptp_port_state(port);
597
598 if (state != PTP_PS_TIME_RECEIVER && state != PTP_PS_UNCALIBRATED) {
599 return;
600 }
601
602 if (!ptp_msg_current_parent(msg)) {
603 return;
604 }
605
606 port_sync_fup_ooo_handle(port, msg);
607 }
608
port_delay_req_msg_process(struct ptp_port * port,struct ptp_msg * msg)609 static int port_delay_req_msg_process(struct ptp_port *port, struct ptp_msg *msg)
610 {
611 int ret;
612 struct ptp_msg *resp;
613 enum ptp_port_state state = ptp_port_state(port);
614 const struct ptp_default_ds *dds = ptp_clock_default_ds();
615
616 if (state != PTP_PS_TIME_TRANSMITTER && state != PTP_PS_GRAND_MASTER) {
617 return 0;
618 }
619
620 resp = ptp_msg_alloc();
621 if (!resp) {
622 return -ENOMEM;
623 }
624
625 resp->header.type_major_sdo_id = PTP_MSG_DELAY_RESP;
626 resp->header.version = PTP_VERSION;
627 resp->header.msg_length = sizeof(struct ptp_delay_resp_msg);
628 resp->header.domain_number = dds->domain;
629 resp->header.correction = msg->header.correction;
630 resp->header.src_port_id = port->port_ds.id;
631 resp->header.sequence_id = msg->header.sequence_id;
632 resp->header.log_msg_interval = port->port_ds.log_min_delay_req_interval;
633
634 resp->delay_resp.receive_timestamp.seconds_high = msg->timestamp.host._sec.high;
635 resp->delay_resp.receive_timestamp.seconds_low = msg->timestamp.host._sec.low;
636 resp->delay_resp.receive_timestamp.nanoseconds = msg->timestamp.host.nanosecond;
637 resp->delay_resp.req_port_id = msg->header.src_port_id;
638
639 if (msg->header.flags[0] & PTP_MSG_UNICAST_FLAG) {
640 /* TODO handle unicast messages */
641 resp->header.flags[0] |= PTP_MSG_UNICAST_FLAG;
642 }
643
644 ret = port_msg_send(port, resp, PTP_SOCKET_EVENT);
645 ptp_msg_unref(resp);
646
647 if (ret < 0) {
648 return -EFAULT;
649 }
650
651 LOG_DBG("Port %d responds to Delay_Req message", port->port_ds.id.port_number);
652 return 0;
653 }
654
port_delay_resp_msg_process(struct ptp_port * port,struct ptp_msg * msg)655 static void port_delay_resp_msg_process(struct ptp_port *port, struct ptp_msg *msg)
656 {
657 uint64_t t3, t4, t4c;
658 sys_snode_t *prev = NULL;
659 struct ptp_msg *req;
660 enum ptp_port_state state = ptp_port_state(port);
661
662 if (state != PTP_PS_TIME_RECEIVER && state != PTP_PS_UNCALIBRATED) {
663 return;
664 }
665
666 if (!ptp_port_id_eq(&msg->delay_resp.req_port_id, &port->port_ds.id)) {
667 /* Message is not meant for this PTP Port */
668 return;
669 }
670
671 SYS_SLIST_FOR_EACH_CONTAINER(&port->delay_req_list, req, node) {
672 if (msg->header.sequence_id == ntohs(req->header.sequence_id)) {
673 break;
674 }
675 prev = &req->node;
676 }
677
678 if (!req) {
679 return;
680 }
681
682 t3 = req->timestamp.host.second * NSEC_PER_SEC + req->timestamp.host.nanosecond;
683 t4 = msg->timestamp.protocol.second * NSEC_PER_SEC + msg->timestamp.protocol.nanosecond;
684 t4c = t4 - (msg->header.correction >> 16);
685
686 ptp_clock_delay(t3, t4c);
687
688 sys_slist_remove(&port->delay_req_list, prev, &req->node);
689 ptp_msg_unref(req);
690
691 port->port_ds.log_min_delay_req_interval = msg->header.log_msg_interval;
692 }
693
port_management_resp_prepare(struct ptp_port * port,struct ptp_msg * req)694 static struct ptp_msg *port_management_resp_prepare(struct ptp_port *port, struct ptp_msg *req)
695 {
696 struct ptp_msg *resp = ptp_msg_alloc();
697 const struct ptp_default_ds *dds = ptp_clock_default_ds();
698
699 if (!resp) {
700 return NULL;
701 }
702
703 resp->header.type_major_sdo_id = PTP_MSG_MANAGEMENT;
704 resp->header.version = PTP_VERSION;
705 resp->header.msg_length = sizeof(struct ptp_management_msg);
706 resp->header.domain_number = dds->domain;
707 resp->header.src_port_id = port->port_ds.id;
708 resp->header.sequence_id = req->header.sequence_id;
709 resp->header.log_msg_interval = port->port_ds.log_min_delay_req_interval;
710
711 if (req->management.action == PTP_MGMT_GET ||
712 req->management.action == PTP_MGMT_SET) {
713 resp->management.action = PTP_MGMT_RESP;
714 } else if (req->management.action == PTP_MGMT_CMD) {
715 resp->management.action = PTP_MGMT_ACK;
716 }
717
718 memcpy(&resp->management.target_port_id,
719 &req->header.src_port_id,
720 sizeof(struct ptp_port_id));
721
722 resp->management.starting_boundary_hops = req->management.starting_boundary_hops -
723 req->management.boundary_hops;
724 resp->management.boundary_hops = resp->management.starting_boundary_hops;
725
726 return resp;
727 }
728
port_management_resp_tlv_fill(struct ptp_port * port,struct ptp_msg * req,struct ptp_msg * resp,struct ptp_tlv_mgmt * req_mgmt)729 static int port_management_resp_tlv_fill(struct ptp_port *port,
730 struct ptp_msg *req,
731 struct ptp_msg *resp,
732 struct ptp_tlv_mgmt *req_mgmt)
733 {
734 int length = 0;
735 struct ptp_tlv_mgmt *mgmt;
736 struct ptp_tlv_default_ds *tlv_dds;
737 struct ptp_tlv_parent_ds *tlv_pds;
738 const struct ptp_parent_ds *pds = ptp_clock_parent_ds();
739 const struct ptp_current_ds *cds = ptp_clock_current_ds();
740 const struct ptp_default_ds *dds = ptp_clock_default_ds();
741 const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
742 struct ptp_tlv_container *container = ptp_tlv_alloc();
743
744 if (!container) {
745 return -ENOMEM;
746 }
747
748 container->tlv = (struct ptp_tlv *)resp->management.suffix;
749 mgmt = (struct ptp_tlv_mgmt *)container->tlv;
750 mgmt->type = PTP_TLV_TYPE_MANAGEMENT;
751 mgmt->id = req_mgmt->id;
752
753 switch (mgmt->id) {
754 case PTP_MGMT_DEFAULT_DATA_SET:
755 tlv_dds = (struct ptp_tlv_default_ds *)mgmt->data;
756
757 length = sizeof(struct ptp_tlv_default_ds);
758 tlv_dds->flags = 0x1 | (dds->time_receiver_only << 1);
759 tlv_dds->n_ports = dds->n_ports;
760 tlv_dds->priority1 = dds->priority1;
761 tlv_dds->priority2 = dds->priority2;
762 tlv_dds->domain = dds->domain;
763 memcpy(&tlv_dds->clk_id, &dds->clk_id, sizeof(tlv_dds->clk_id));
764 memcpy(&tlv_dds->clk_quality, &dds->clk_quality, sizeof(tlv_dds->clk_quality));
765 break;
766 case PTP_MGMT_CURRENT_DATA_SET:
767 length = sizeof(struct ptp_tlv_current_ds);
768 memcpy(mgmt->data, cds, sizeof(struct ptp_tlv_current_ds));
769 break;
770 case PTP_MGMT_PARENT_DATA_SET:
771 tlv_pds = (struct ptp_tlv_parent_ds *)mgmt->data;
772
773 length = sizeof(struct ptp_tlv_parent_ds);
774
775 tlv_pds->obsreved_parent_offset_scaled_log_variance =
776 pds->obsreved_parent_offset_scaled_log_variance;
777 tlv_pds->obsreved_parent_clk_phase_change_rate =
778 pds->obsreved_parent_clk_phase_change_rate;
779 tlv_pds->gm_priority1 = pds->gm_priority1;
780 tlv_pds->gm_priority2 = pds->gm_priority2;
781 memcpy(&tlv_pds->port_id, &pds->port_id, sizeof(tlv_pds->port_id));
782 memcpy(&tlv_pds->gm_id, &pds->gm_id, sizeof(tlv_pds->gm_id));
783 memcpy(&tlv_pds->gm_clk_quality,
784 &pds->gm_clk_quality,
785 sizeof(tlv_pds->gm_clk_quality));
786
787 break;
788 case PTP_MGMT_TIME_PROPERTIES_DATA_SET:
789 length = sizeof(struct ptp_tlv_time_prop_ds);
790 memcpy(mgmt->data, tpds, sizeof(struct ptp_tlv_time_prop_ds));
791 break;
792 case PTP_MGMT_PORT_DATA_SET:
793 length = sizeof(struct ptp_tlv_port_ds);
794 memcpy(mgmt->data, &port->port_ds, sizeof(struct ptp_tlv_port_ds));
795 break;
796 case PTP_MGMT_PRIORITY1:
797 length = sizeof(dds->priority1);
798 *mgmt->data = dds->priority1;
799 break;
800 case PTP_MGMT_PRIORITY2:
801 length = sizeof(dds->priority2);
802 *mgmt->data = dds->priority2;
803 break;
804 case PTP_MGMT_DOMAIN:
805 length = sizeof(dds->domain);
806 *mgmt->data = dds->domain;
807 break;
808 case PTP_MGMT_TIME_RECEIVER_ONLY:
809 length = sizeof(dds->time_receiver_only);
810 *mgmt->data = dds->time_receiver_only;
811 break;
812 case PTP_MGMT_LOG_ANNOUNCE_INTERVAL:
813 length = sizeof(port->port_ds.log_announce_interval);
814 *mgmt->data = port->port_ds.log_announce_interval;
815 break;
816 case PTP_MGMT_LOG_SYNC_INTERVAL:
817 length = sizeof(port->port_ds.log_sync_interval);
818 *mgmt->data = port->port_ds.log_sync_interval;
819 break;
820 case PTP_MGMT_VERSION_NUMBER:
821 length = sizeof(port->port_ds.version);
822 *mgmt->data = port->port_ds.version;
823 break;
824 case PTP_MGMT_CLOCK_ACCURACY:
825 length = sizeof(dds->clk_quality.accuracy);
826 *mgmt->data = dds->clk_quality.accuracy;
827 break;
828 case PTP_MGMT_DELAY_MECHANISM:
829 length = sizeof(port->port_ds.delay_mechanism);
830 *(uint16_t *)mgmt->data = port->port_ds.delay_mechanism;
831 break;
832 default:
833 ptp_tlv_free(container);
834 return -EINVAL;
835 }
836
837 /* Management TLV length shall be an even number */
838 if (length % 2) {
839 mgmt->data[length] = 0;
840 length++;
841 }
842
843 container->tlv->length = sizeof(mgmt->id) + length;
844 resp->header.msg_length += sizeof(*container->tlv) + container->tlv->length;
845 sys_slist_append(&resp->tlvs, &container->node);
846
847 return 0;
848 }
849
port_management_set(struct ptp_port * port,struct ptp_msg * req,struct ptp_tlv_mgmt * tlv)850 static int port_management_set(struct ptp_port *port,
851 struct ptp_msg *req,
852 struct ptp_tlv_mgmt *tlv)
853 {
854 bool send_resp = false;
855
856 switch (tlv->id) {
857 case PTP_MGMT_LOG_ANNOUNCE_INTERVAL:
858 port->port_ds.log_announce_interval = *tlv->data;
859 send_resp = true;
860 break;
861 case PTP_MGMT_LOG_SYNC_INTERVAL:
862 port->port_ds.log_sync_interval = *tlv->data;
863 send_resp = true;
864 break;
865 case PTP_MGMT_UNICAST_NEGOTIATION_ENABLE:
866 /* TODO unicast */
867 break;
868 default:
869 break;
870 }
871
872 return send_resp ? ptp_port_management_resp(port, req, tlv) : 0;
873 }
874
port_enable(struct ptp_port * port)875 static int port_enable(struct ptp_port *port)
876 {
877 while (!net_if_is_up(port->iface)) {
878 return -1;
879 }
880
881 port->link_status = PORT_LINK_UP;
882
883 if (ptp_transport_open(port)) {
884 LOG_ERR("Couldn't open socket on Port %d.", port->port_ds.id.port_number);
885 return -1;
886 }
887
888 port->port_ds.enable = true;
889
890 ptp_clock_pollfd_invalidate();
891 LOG_DBG("Port %d opened", port->port_ds.id.port_number);
892 return 0;
893 }
894
port_is_enabled(struct ptp_port * port)895 static bool port_is_enabled(struct ptp_port *port)
896 {
897 enum ptp_port_state state = ptp_port_state(port);
898
899 if (state == PTP_PS_FAULTY ||
900 state == PTP_PS_DISABLED ||
901 state == PTP_PS_INITIALIZING) {
902 return false;
903 }
904 return true;
905 }
906
port_disable(struct ptp_port * port)907 static void port_disable(struct ptp_port *port)
908 {
909 k_timer_stop(&port->timers.announce);
910 k_timer_stop(&port->timers.delay);
911 k_timer_stop(&port->timers.sync);
912 k_timer_stop(&port->timers.qualification);
913
914 atomic_clear(&port->timeouts);
915
916 ptp_transport_close(port);
917 ptp_port_free_foreign_tts(port);
918 port->best = NULL;
919
920 net_if_unregister_timestamp_cb(&port->sync_ts_cb);
921 net_if_unregister_timestamp_cb(&port->delay_req_ts_cb);
922
923 ptp_clock_pollfd_invalidate();
924 port->port_ds.enable = false;
925 LOG_DBG("Port %d disabled", port->port_ds.id.port_number);
926 }
927
port_state_update(struct ptp_port * port,enum ptp_port_event event,bool tt_diff)928 int port_state_update(struct ptp_port *port, enum ptp_port_event event, bool tt_diff)
929 {
930 enum ptp_port_state next_state = port->state_machine(ptp_port_state(port),
931 event,
932 tt_diff);
933
934 if (next_state == PTP_PS_FAULTY) {
935 /* clear fault if interface is UP */
936 if (net_if_oper_state(port->iface) == NET_IF_OPER_UP) {
937 next_state = port->state_machine(next_state, PTP_EVT_FAULT_CLEARED, false);
938 }
939 }
940
941 if (next_state == PTP_PS_INITIALIZING) {
942 if (port_is_enabled(port)) {
943 port_disable(port);
944 }
945 if (port_enable(port)) {
946 event = PTP_EVT_FAULT_DETECTED;
947 } else {
948 event = PTP_EVT_INIT_COMPLETE;
949 }
950 next_state = port->state_machine(next_state, event, false);
951 }
952
953 if (next_state != ptp_port_state(port)) {
954 LOG_DBG("Port %d changed state from %s to %s",
955 port->port_ds.id.port_number,
956 port_state_str(ptp_port_state(port)),
957 port_state_str(next_state));
958
959 port->port_ds.state = next_state;
960 return 1;
961 }
962
963 return 0;
964 }
965
port_link_monitor(struct net_mgmt_event_callback * cb,uint32_t mgmt_event,struct net_if * iface)966 static void port_link_monitor(struct net_mgmt_event_callback *cb,
967 uint32_t mgmt_event,
968 struct net_if *iface)
969 {
970 ARG_UNUSED(cb);
971
972 enum ptp_port_event event = PTP_EVT_NONE;
973 struct ptp_port *port = ptp_clock_port_from_iface(iface);
974 uint8_t iface_state = mgmt_event == NET_EVENT_IF_UP ? PORT_LINK_UP : PORT_LINK_DOWN;
975
976 if (!port) {
977 return;
978 }
979
980 if (iface_state & port->link_status) {
981 port->link_status = iface_state;
982 } else {
983 port->link_status = iface_state | PORT_LINK_CHANGED;
984 LOG_DBG("Port %d link %s",
985 port->port_ds.id.port_number,
986 port->link_status & PORT_LINK_UP ? "up" : "down");
987 }
988
989 if (port->link_status & PORT_LINK_CHANGED) {
990 event = iface_state == PORT_LINK_UP ?
991 PTP_EVT_FAULT_CLEARED : PTP_EVT_FAULT_DETECTED;
992 port->link_status ^= PORT_LINK_CHANGED;
993 }
994
995 if (port->link_status & PORT_LINK_DOWN) {
996 ptp_clock_state_decision_req();
997 }
998
999 ptp_port_event_handle(port, event, false);
1000 }
1001
ptp_port_init(struct net_if * iface,void * user_data)1002 void ptp_port_init(struct net_if *iface, void *user_data)
1003 {
1004 struct ptp_port *port;
1005 const struct ptp_default_ds *dds = ptp_clock_default_ds();
1006
1007 ARG_UNUSED(user_data);
1008
1009 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
1010 return;
1011 }
1012
1013 if (dds->n_ports > CONFIG_PTP_NUM_PORTS) {
1014 LOG_WRN("Exceeded number of PTP Ports.");
1015 return;
1016 }
1017
1018 port = &ports[dds->n_ports];
1019
1020 port->iface = iface;
1021 port->best = NULL;
1022 port->socket[PTP_SOCKET_EVENT] = -1;
1023 port->socket[PTP_SOCKET_GENERAL] = -1;
1024
1025 port->state_machine = dds->time_receiver_only ? ptp_tr_state_machine : ptp_state_machine;
1026 port->last_sync_fup = NULL;
1027
1028 port_ds_init(port);
1029 sys_slist_init(&port->foreign_list);
1030 sys_slist_init(&port->delay_req_list);
1031
1032 port_timer_init(&port->timers.delay, port_timer_to_handler, port);
1033 port_timer_init(&port->timers.announce, port_timer_to_handler, port);
1034 port_timer_init(&port->timers.sync, port_timer_to_handler, port);
1035 port_timer_init(&port->timers.qualification, port_timer_to_handler, port);
1036
1037 ptp_clock_pollfd_invalidate();
1038 ptp_clock_port_add(port);
1039
1040 net_mgmt_init_event_callback(&port->link_cb, port_link_monitor, PORT_LINK_EVENT_MASK);
1041 net_mgmt_add_event_callback(&port->link_cb);
1042
1043 LOG_DBG("Port %d initialized", port->port_ds.id.port_number);
1044 }
1045
ptp_port_event_gen(struct ptp_port * port,int idx)1046 enum ptp_port_event ptp_port_event_gen(struct ptp_port *port, int idx)
1047 {
1048 enum ptp_port_event event = PTP_EVT_NONE;
1049 struct ptp_msg *msg;
1050 int ret, cnt;
1051
1052 if (idx < 0) {
1053 return event;
1054 }
1055
1056 msg = ptp_msg_alloc();
1057 if (!msg) {
1058 return PTP_EVT_FAULT_DETECTED;
1059 }
1060
1061 cnt = ptp_transport_recv(port, msg, idx);
1062 if (cnt <= 0) {
1063 LOG_ERR("Error during message reception");
1064 ptp_msg_unref(msg);
1065 return PTP_EVT_FAULT_DETECTED;
1066 }
1067
1068 ret = ptp_msg_post_recv(port, msg, cnt);
1069 if (ret) {
1070 ptp_msg_unref(msg);
1071 return PTP_EVT_FAULT_DETECTED;
1072 }
1073
1074 if (ptp_port_id_eq(&msg->header.src_port_id, &port->port_ds.id)) {
1075 ptp_msg_unref(msg);
1076 return PTP_EVT_NONE;
1077 }
1078
1079 switch (ptp_msg_type(msg)) {
1080 case PTP_MSG_SYNC:
1081 port_sync_msg_process(port, msg);
1082 break;
1083 case PTP_MSG_DELAY_REQ:
1084 if (port_delay_req_msg_process(port, msg)) {
1085 event = PTP_EVT_FAULT_DETECTED;
1086 }
1087 break;
1088 case PTP_MSG_PDELAY_REQ:
1089 __fallthrough;
1090 case PTP_MSG_PDELAY_RESP:
1091 __fallthrough;
1092 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
1093 /* P2P delay mechanism not supported */
1094 break;
1095 case PTP_MSG_FOLLOW_UP:
1096 port_follow_up_msg_process(port, msg);
1097 break;
1098 case PTP_MSG_DELAY_RESP:
1099 port_delay_resp_msg_process(port, msg);
1100 break;
1101 case PTP_MSG_ANNOUNCE:
1102 if (port_announce_msg_process(port, msg)) {
1103 event = PTP_EVT_STATE_DECISION;
1104 }
1105 break;
1106 case PTP_MSG_SIGNALING:
1107 /* Signalling messages not supported */
1108 break;
1109 case PTP_MSG_MANAGEMENT:
1110 if (ptp_clock_management_msg_process(port, msg)) {
1111 event = PTP_EVT_STATE_DECISION;
1112 }
1113 break;
1114 default:
1115 break;
1116 }
1117
1118 ptp_msg_unref(msg);
1119 return event;
1120 }
1121
ptp_port_event_handle(struct ptp_port * port,enum ptp_port_event event,bool tt_diff)1122 void ptp_port_event_handle(struct ptp_port *port, enum ptp_port_event event, bool tt_diff)
1123 {
1124 const struct ptp_current_ds *cds = ptp_clock_current_ds();
1125
1126 if (event == PTP_EVT_NONE) {
1127 return;
1128 }
1129
1130 if (!port_state_update(port, event, tt_diff)) {
1131 /* No PTP Port state change */
1132 return;
1133 }
1134
1135 k_timer_stop(&port->timers.announce);
1136 k_timer_stop(&port->timers.delay);
1137 k_timer_stop(&port->timers.sync);
1138 k_timer_stop(&port->timers.qualification);
1139
1140 switch (port->port_ds.state) {
1141 case PTP_PS_INITIALIZING:
1142 break;
1143 case PTP_PS_FAULTY:
1144 __fallthrough;
1145 case PTP_PS_DISABLED:
1146 port_disable(port);
1147 break;
1148 case PTP_PS_LISTENING:
1149 port_timer_set_timeout_random(&port->timers.announce,
1150 port->port_ds.announce_receipt_timeout,
1151 1,
1152 port->port_ds.log_announce_interval);
1153 break;
1154 case PTP_PS_PRE_TIME_TRANSMITTER:
1155 port_timer_set_timeout(&port->timers.qualification,
1156 1 + cds->steps_rm,
1157 port->port_ds.log_announce_interval);
1158 break;
1159 case PTP_PS_GRAND_MASTER:
1160 __fallthrough;
1161 case PTP_PS_TIME_TRANSMITTER:
1162 port_timer_set_timeout(&port->timers.announce,
1163 1,
1164 port->port_ds.log_announce_interval);
1165 port_timer_set_timeout(&port->timers.sync, 1, port->port_ds.log_sync_interval);
1166 break;
1167 case PTP_PS_PASSIVE:
1168 port_timer_set_timeout_random(&port->timers.announce,
1169 port->port_ds.announce_receipt_timeout,
1170 1,
1171 port->port_ds.log_announce_interval);
1172 break;
1173 case PTP_PS_UNCALIBRATED:
1174 if (port->last_sync_fup) {
1175 ptp_msg_unref(port->last_sync_fup);
1176 port->last_sync_fup = NULL;
1177 }
1178 port_clear_delay_req(port);
1179 __fallthrough;
1180 case PTP_PS_TIME_RECEIVER:
1181 port_timer_set_timeout_random(&port->timers.announce,
1182 port->port_ds.announce_receipt_timeout,
1183 1,
1184 port->port_ds.log_announce_interval);
1185 port_timer_set_timeout_random(&port->timers.delay,
1186 0,
1187 2,
1188 port->port_ds.log_min_delay_req_interval);
1189 break;
1190 };
1191 }
1192
ptp_port_state(struct ptp_port * port)1193 enum ptp_port_state ptp_port_state(struct ptp_port *port)
1194 {
1195 return (enum ptp_port_state)port->port_ds.state;
1196 }
1197
ptp_port_timer_event_gen(struct ptp_port * port,struct k_timer * timer)1198 enum ptp_port_event ptp_port_timer_event_gen(struct ptp_port *port, struct k_timer *timer)
1199 {
1200 enum ptp_port_event event = PTP_EVT_NONE;
1201 enum ptp_port_state state = ptp_port_state(port);
1202
1203 switch (state) {
1204 case PTP_PS_PRE_TIME_TRANSMITTER:
1205 if (timer == &port->timers.qualification &&
1206 atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_QUALIFICATION_TO)) {
1207 LOG_DBG("Port %d Qualification timeout", port->port_ds.id.port_number);
1208 atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_QUALIFICATION_TO);
1209
1210 return PTP_EVT_QUALIFICATION_TIMEOUT_EXPIRES;
1211 }
1212 break;
1213 case PTP_PS_GRAND_MASTER:
1214 __fallthrough;
1215 case PTP_PS_TIME_TRANSMITTER:
1216 if (timer == &port->timers.sync &&
1217 atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO)) {
1218 LOG_DBG("Port %d TX Sync timeout", port->port_ds.id.port_number);
1219 port_timer_set_timeout(&port->timers.sync,
1220 1,
1221 port->port_ds.log_sync_interval);
1222 atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO);
1223
1224 return port_sync_msg_transmit(port) == 0 ? PTP_EVT_NONE :
1225 PTP_EVT_FAULT_DETECTED;
1226 }
1227
1228 if (timer == &port->timers.announce &&
1229 atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO)) {
1230 LOG_DBG("Port %d TimeTransmitter Announce timeout",
1231 port->port_ds.id.port_number);
1232 port_timer_set_timeout(&port->timers.announce,
1233 1,
1234 port->port_ds.log_announce_interval);
1235 atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO);
1236
1237 return port_announce_msg_transmit(port) == 0 ? PTP_EVT_NONE :
1238 PTP_EVT_FAULT_DETECTED;
1239 }
1240 break;
1241 case PTP_PS_TIME_RECEIVER:
1242 if (timer == &port->timers.delay &&
1243 atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_DELAY_TO)) {
1244
1245 atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_DELAY_TO);
1246 port_delay_req_cleanup(port);
1247 port_timer_set_timeout(&port->timers.delay,
1248 1,
1249 port->port_ds.log_announce_interval);
1250
1251 if (port_delay_req_msg_transmit(port) < 0) {
1252 return PTP_EVT_FAULT_DETECTED;
1253 }
1254 }
1255 __fallthrough;
1256 case PTP_PS_PASSIVE:
1257 __fallthrough;
1258 case PTP_PS_UNCALIBRATED:
1259 __fallthrough;
1260 case PTP_PS_LISTENING:
1261 if ((timer == &port->timers.announce || timer == &port->timers.sync) &&
1262 (atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO) ||
1263 atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO))) {
1264
1265 LOG_DBG("Port %d %s timeout",
1266 port->port_ds.id.port_number,
1267 timer == &port->timers.announce ? "Announce" : "RX Sync");
1268
1269 atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO);
1270 atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO);
1271
1272 if (port->best) {
1273 port_clear_foreign_clock_records(port->best);
1274 }
1275
1276 port_delay_req_cleanup(port);
1277 port_timer_set_timeout_random(&port->timers.announce,
1278 port->port_ds.announce_receipt_timeout,
1279 1,
1280 port->port_ds.log_announce_interval);
1281
1282 return PTP_EVT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES;
1283 }
1284 break;
1285 default:
1286 break;
1287 }
1288
1289 return event;
1290 }
1291
ptp_port_id_eq(const struct ptp_port_id * p1,const struct ptp_port_id * p2)1292 bool ptp_port_id_eq(const struct ptp_port_id *p1, const struct ptp_port_id *p2)
1293 {
1294 return memcmp(p1, p2, sizeof(struct ptp_port_id)) == 0;
1295 }
1296
ptp_port_best_foreign_ds(struct ptp_port * port)1297 struct ptp_dataset *ptp_port_best_foreign_ds(struct ptp_port *port)
1298 {
1299 return port->best ? &port->best->dataset : NULL;
1300 }
1301
ptp_port_best_foreign(struct ptp_port * port)1302 struct ptp_foreign_tt_clock *ptp_port_best_foreign(struct ptp_port *port)
1303 {
1304 struct ptp_foreign_tt_clock *foreign;
1305 struct ptp_announce_msg *last;
1306
1307 port->best = NULL;
1308
1309 if (port->port_ds.time_transmitter_only) {
1310 return NULL;
1311 }
1312
1313 SYS_SLIST_FOR_EACH_CONTAINER(&port->foreign_list, foreign, node) {
1314 if (!foreign->messages_count) {
1315 continue;
1316 }
1317
1318 foreign_clock_cleanup(foreign);
1319
1320 if (foreign->messages_count < FOREIGN_TIME_TRANSMITTER_THRESHOLD) {
1321 continue;
1322 }
1323
1324 last = (struct ptp_announce_msg *)k_fifo_peek_head(&foreign->messages);
1325
1326 foreign->dataset.priority1 = last->gm_priority1;
1327 foreign->dataset.priority2 = last->gm_priority2;
1328 foreign->dataset.steps_rm = last->steps_rm;
1329
1330 memcpy(&foreign->dataset.clk_quality,
1331 &last->gm_clk_quality,
1332 sizeof(last->gm_clk_quality));
1333 memcpy(&foreign->dataset.clk_id, &last->gm_id, sizeof(last->gm_id));
1334 memcpy(&foreign->dataset.receiver, &port->port_ds.id, sizeof(port->port_ds.id));
1335
1336 if (!port->best) {
1337 port->best = foreign;
1338 } else if (ptp_btca_ds_cmp(&foreign->dataset, &port->best->dataset)) {
1339 port->best = foreign;
1340 } else {
1341 port_clear_foreign_clock_records(foreign);
1342 }
1343 }
1344 return port->best;
1345 }
1346
ptp_port_add_foreign_tt(struct ptp_port * port,struct ptp_msg * msg)1347 int ptp_port_add_foreign_tt(struct ptp_port *port, struct ptp_msg *msg)
1348 {
1349 struct ptp_foreign_tt_clock *foreign;
1350 struct ptp_msg *last;
1351 int diff = 0;
1352
1353 SYS_SLIST_FOR_EACH_CONTAINER(&port->foreign_list, foreign, node) {
1354 if (ptp_port_id_eq(&msg->header.src_port_id, &foreign->dataset.sender)) {
1355 break;
1356 }
1357 }
1358
1359 if (!foreign) {
1360 LOG_DBG("Port %d has a new foreign timeTransmitter %s",
1361 port->port_ds.id.port_number,
1362 port_id_str(&msg->header.src_port_id));
1363
1364 int ret = k_mem_slab_alloc(&foreign_tts_slab, (void **)&foreign, K_NO_WAIT);
1365
1366 if (ret) {
1367 LOG_ERR("Couldn't allocate memory for new foreign timeTransmitter");
1368 return 0;
1369 }
1370
1371 memset(foreign, 0, sizeof(*foreign));
1372 memcpy(&foreign->dataset.sender,
1373 &msg->header.src_port_id,
1374 sizeof(foreign->dataset.sender));
1375 k_fifo_init(&foreign->messages);
1376 foreign->port = port;
1377
1378 sys_slist_append(&port->foreign_list, &foreign->node);
1379
1380 /* First message is not added to records. */
1381 return 0;
1382 }
1383
1384 foreign_clock_cleanup(foreign);
1385 ptp_msg_ref(msg);
1386
1387 foreign->messages_count++;
1388 k_fifo_put(&foreign->messages, (void *)msg);
1389
1390 if (foreign->messages_count > 1) {
1391 last = (struct ptp_msg *)k_fifo_peek_tail(&foreign->messages);
1392 diff = ptp_msg_announce_cmp(&msg->announce, &last->announce);
1393 }
1394
1395 return (foreign->messages_count == FOREIGN_TIME_TRANSMITTER_THRESHOLD ? 1 : 0) || diff;
1396 }
1397
ptp_port_free_foreign_tts(struct ptp_port * port)1398 void ptp_port_free_foreign_tts(struct ptp_port *port)
1399 {
1400 sys_snode_t *iter;
1401 struct ptp_foreign_tt_clock *foreign;
1402
1403 while (!sys_slist_is_empty(&port->foreign_list)) {
1404 iter = sys_slist_get(&port->foreign_list);
1405 foreign = CONTAINER_OF(iter, struct ptp_foreign_tt_clock, node);
1406
1407 while (foreign->messages_count > FOREIGN_TIME_TRANSMITTER_THRESHOLD) {
1408 struct ptp_msg *msg = (struct ptp_msg *)k_fifo_get(&foreign->messages,
1409 K_NO_WAIT);
1410 foreign->messages_count--;
1411 ptp_msg_unref(msg);
1412 }
1413
1414 k_mem_slab_free(&foreign_tts_slab, (void *)foreign);
1415 }
1416 }
1417
ptp_port_update_current_time_transmitter(struct ptp_port * port,struct ptp_msg * msg)1418 int ptp_port_update_current_time_transmitter(struct ptp_port *port, struct ptp_msg *msg)
1419 {
1420 struct ptp_foreign_tt_clock *foreign = port->best;
1421
1422 if (!foreign ||
1423 !ptp_port_id_eq(&msg->header.src_port_id, &foreign->dataset.sender)) {
1424 return ptp_port_add_foreign_tt(port, msg);
1425 }
1426
1427 foreign_clock_cleanup(foreign);
1428 ptp_msg_ref(msg);
1429
1430 k_fifo_put(&foreign->messages, (void *)msg);
1431 foreign->messages_count++;
1432
1433 port_timer_set_timeout_random(&port->timers.announce,
1434 port->port_ds.announce_receipt_timeout,
1435 1,
1436 port->port_ds.log_announce_interval);
1437
1438 if (foreign->messages_count > 1) {
1439 struct ptp_msg *last = (struct ptp_msg *)k_fifo_peek_tail(&foreign->messages);
1440
1441 return ptp_msg_announce_cmp(&msg->announce, &last->announce);
1442 }
1443
1444 return 0;
1445 }
1446
ptp_port_management_msg_process(struct ptp_port * port,struct ptp_port * ingress,struct ptp_msg * msg,struct ptp_tlv_mgmt * tlv)1447 int ptp_port_management_msg_process(struct ptp_port *port,
1448 struct ptp_port *ingress,
1449 struct ptp_msg *msg,
1450 struct ptp_tlv_mgmt *tlv)
1451 {
1452 int ret = 0;
1453 uint16_t target_port = msg->management.target_port_id.port_number;
1454
1455 if (target_port != port->port_ds.id.port_number && target_port != 0xFFFF) {
1456 return ret;
1457 }
1458
1459 if (ptp_mgmt_action(msg) == PTP_MGMT_SET) {
1460 ret = port_management_set(port, msg, tlv);
1461 } else {
1462 ret = ptp_port_management_resp(port, msg, tlv);
1463 }
1464
1465 return ret;
1466 }
1467
ptp_port_management_error(struct ptp_port * port,struct ptp_msg * msg,enum ptp_mgmt_err err)1468 int ptp_port_management_error(struct ptp_port *port, struct ptp_msg *msg, enum ptp_mgmt_err err)
1469 {
1470 int ret;
1471 struct ptp_tlv *tlv;
1472 struct ptp_tlv_mgmt_err *mgmt_err;
1473 struct ptp_tlv_mgmt *mgmt = (struct ptp_tlv_mgmt *)msg->management.suffix;
1474
1475 struct ptp_msg *resp = port_management_resp_prepare(port, msg);
1476
1477 if (!resp) {
1478 return -ENOMEM;
1479 }
1480
1481 tlv = ptp_msg_add_tlv(resp, sizeof(struct ptp_tlv_mgmt_err));
1482 if (!tlv) {
1483 ptp_msg_unref(resp);
1484 return -ENOMEM;
1485 }
1486
1487 mgmt_err = (struct ptp_tlv_mgmt_err *)tlv;
1488
1489 mgmt_err->type = PTP_TLV_TYPE_MANAGEMENT_ERROR_STATUS;
1490 mgmt_err->length = 8;
1491 mgmt_err->err_id = err;
1492 mgmt_err->id = mgmt->id;
1493
1494 ret = port_msg_send(port, resp, PTP_SOCKET_GENERAL);
1495 ptp_msg_unref(resp);
1496
1497 if (ret) {
1498 return -EFAULT;
1499 }
1500
1501 LOG_DBG("Port %d sends Menagement Error message", port->port_ds.id.port_number);
1502 return 0;
1503 }
1504
ptp_port_management_resp(struct ptp_port * port,struct ptp_msg * req,struct ptp_tlv_mgmt * tlv)1505 int ptp_port_management_resp(struct ptp_port *port, struct ptp_msg *req, struct ptp_tlv_mgmt *tlv)
1506 {
1507 int ret;
1508 struct ptp_msg *resp = port_management_resp_prepare(port, req);
1509
1510 if (!resp) {
1511 return -ENOMEM;
1512 }
1513
1514 ret = port_management_resp_tlv_fill(port, req, resp, tlv);
1515 if (ret) {
1516 return ret;
1517 }
1518
1519 ret = port_msg_send(port, resp, PTP_SOCKET_GENERAL);
1520 ptp_msg_unref(resp);
1521
1522 if (ret < 0) {
1523 return -EFAULT;
1524 }
1525
1526 LOG_DBG("Port %d sends Menagement message response", port->port_ds.id.port_number);
1527 return 0;
1528 }
1529
