1 /*
2 * Copyright (c) 2017 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_gptp, CONFIG_NET_GPTP_LOG_LEVEL);
9
10 #include <zephyr/net/net_pkt.h>
11 #include <zephyr/drivers/ptp_clock.h>
12 #include <zephyr/net/ethernet_mgmt.h>
13 #include <zephyr/random/random.h>
14
15 #include <zephyr/net/gptp.h>
16
17 #include "gptp_messages.h"
18 #include "gptp_mi.h"
19 #include "gptp_data_set.h"
20
21 #include "gptp_private.h"
22
23 #if CONFIG_NET_GPTP_NUM_PORTS > 32
24 /*
25 * Boolean arrays sizes have been hardcoded.
26 * It has been arbitrary chosen that a system can not
27 * have more than 32 ports.
28 */
29 #error Maximum number of ports exceeded. (Max is 32).
30 #endif
31
32 K_KERNEL_STACK_DEFINE(gptp_stack, CONFIG_NET_GPTP_STACK_SIZE);
33 K_FIFO_DEFINE(gptp_rx_queue);
34
35 static k_tid_t tid;
36 static struct k_thread gptp_thread_data;
37 struct gptp_domain gptp_domain;
38 struct gptp_clock_data gptp_clock;
39
gptp_get_port_number(struct net_if * iface)40 int gptp_get_port_number(struct net_if *iface)
41 {
42 int port = net_eth_get_ptp_port(iface) + 1;
43
44 if (port >= GPTP_PORT_START && port < GPTP_PORT_END) {
45 return port;
46 }
47
48 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
49 if (GPTP_PORT_IFACE(port) == iface) {
50 return port;
51 }
52 }
53
54 return -ENODEV;
55 }
56
gptp_is_slave_port(int port)57 bool gptp_is_slave_port(int port)
58 {
59 return (GPTP_GLOBAL_DS()->selected_role[port] == GPTP_PORT_SLAVE);
60 }
61
62 /*
63 * Use the given port to generate the clock identity
64 * for the device.
65 * The clock identity is unique for one time-aware system.
66 */
gptp_compute_clock_identity(int port)67 static void gptp_compute_clock_identity(int port)
68 {
69 struct net_if *iface = GPTP_PORT_IFACE(port);
70 struct gptp_default_ds *default_ds;
71
72 default_ds = GPTP_DEFAULT_DS();
73
74 if (iface) {
75 default_ds->clk_id[0] = net_if_get_link_addr(iface)->addr[0];
76 default_ds->clk_id[1] = net_if_get_link_addr(iface)->addr[1];
77 default_ds->clk_id[2] = net_if_get_link_addr(iface)->addr[2];
78 default_ds->clk_id[3] = 0xFF;
79 default_ds->clk_id[4] = 0xFE;
80 default_ds->clk_id[5] = net_if_get_link_addr(iface)->addr[3];
81 default_ds->clk_id[6] = net_if_get_link_addr(iface)->addr[4];
82 default_ds->clk_id[7] = net_if_get_link_addr(iface)->addr[5];
83 }
84 }
85
86 #define PRINT_INFO(msg, hdr, pkt) \
87 NET_DBG("Received %s seq %d pkt %p", (const char *)msg, \
88 ntohs(hdr->sequence_id), pkt) \
89
90
gptp_handle_critical_msg(struct net_if * iface,struct net_pkt * pkt)91 static bool gptp_handle_critical_msg(struct net_if *iface, struct net_pkt *pkt)
92 {
93 struct gptp_hdr *hdr = GPTP_HDR(pkt);
94 bool handled = false;
95 int port;
96
97 switch (hdr->message_type) {
98 case GPTP_PATH_DELAY_REQ_MESSAGE:
99 if (GPTP_CHECK_LEN(pkt, GPTP_PDELAY_REQ_LEN)) {
100 NET_WARN("Invalid length for %s packet "
101 "should have %zd bytes but has %zd bytes",
102 "PDELAY_REQ",
103 GPTP_PDELAY_REQ_LEN,
104 GPTP_PACKET_LEN(pkt));
105 break;
106 }
107
108 PRINT_INFO("PDELAY_REQ", hdr, pkt);
109
110 port = gptp_get_port_number(iface);
111 if (port == -ENODEV) {
112 NET_DBG("No port found for gPTP buffer");
113 return handled;
114 }
115
116 if (GPTP_PORT_STATE(port)->pdelay_resp.state !=
117 GPTP_PDELAY_RESP_NOT_ENABLED) {
118 gptp_handle_pdelay_req(port, pkt);
119 }
120
121 handled = true;
122 break;
123 default:
124 /* Not a critical message, this will be handled later. */
125 break;
126 }
127
128 return handled;
129 }
130
gptp_handle_msg(struct net_pkt * pkt)131 static void gptp_handle_msg(struct net_pkt *pkt)
132 {
133 struct gptp_hdr *hdr = GPTP_HDR(pkt);
134 struct gptp_pdelay_req_state *pdelay_req_state;
135 struct gptp_sync_rcv_state *sync_rcv_state;
136 struct gptp_port_announce_receive_state *pa_rcv_state;
137 struct gptp_port_bmca_data *bmca_data;
138 int port;
139
140 port = gptp_get_port_number(net_pkt_iface(pkt));
141 if (port == -ENODEV) {
142 NET_DBG("No port found for ptp buffer");
143 return;
144 }
145
146 pdelay_req_state = &GPTP_PORT_STATE(port)->pdelay_req;
147 sync_rcv_state = &GPTP_PORT_STATE(port)->sync_rcv;
148
149 switch (hdr->message_type) {
150 case GPTP_SYNC_MESSAGE:
151 if (GPTP_CHECK_LEN(pkt, GPTP_SYNC_LEN)) {
152 NET_WARN("Invalid length for %s packet "
153 "should have %zd bytes but has %zd bytes",
154 "SYNC",
155 GPTP_SYNC_LEN,
156 GPTP_PACKET_LEN(pkt));
157 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
158 break;
159 }
160
161 PRINT_INFO("SYNC", hdr, pkt);
162
163 sync_rcv_state->rcvd_sync = true;
164
165 /* If we already have one, drop the previous one. */
166 if (sync_rcv_state->rcvd_sync_ptr) {
167 net_pkt_unref(sync_rcv_state->rcvd_sync_ptr);
168 }
169
170 /* Keep the buffer alive until follow_up is received. */
171 net_pkt_ref(pkt);
172 sync_rcv_state->rcvd_sync_ptr = pkt;
173
174 GPTP_STATS_INC(port, rx_sync_count);
175 break;
176
177 case GPTP_DELAY_REQ_MESSAGE:
178 NET_DBG("Delay Request not handled.");
179 break;
180
181 case GPTP_PATH_DELAY_REQ_MESSAGE:
182 /*
183 * Path Delay Responses to Path Delay Requests need
184 * very low latency. These need to handled in priority
185 * when received as they cannot afford to be delayed
186 * by context switches.
187 */
188 NET_WARN("Path Delay Request received as normal messages!");
189 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
190 break;
191
192 case GPTP_PATH_DELAY_RESP_MESSAGE:
193 if (GPTP_CHECK_LEN(pkt, GPTP_PDELAY_RESP_LEN)) {
194 NET_WARN("Invalid length for %s packet "
195 "should have %zd bytes but has %zd bytes",
196 "PDELAY_RESP",
197 GPTP_PDELAY_RESP_LEN,
198 GPTP_PACKET_LEN(pkt));
199 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
200 break;
201 }
202
203 PRINT_INFO("PDELAY_RESP", hdr, pkt);
204
205 pdelay_req_state->rcvd_pdelay_resp++;
206
207 /* If we already have one, drop the received one. */
208 if (pdelay_req_state->rcvd_pdelay_resp_ptr) {
209 break;
210 }
211
212 /* Keep the buffer alive until pdelay_rate_ratio is computed. */
213 net_pkt_ref(pkt);
214 pdelay_req_state->rcvd_pdelay_resp_ptr = pkt;
215 break;
216
217 case GPTP_FOLLOWUP_MESSAGE:
218 if (GPTP_CHECK_LEN(pkt, GPTP_FOLLOW_UP_LEN)) {
219 NET_WARN("Invalid length for %s packet "
220 "should have %zd bytes but has %zd bytes",
221 "FOLLOWUP",
222 GPTP_FOLLOW_UP_LEN,
223 GPTP_PACKET_LEN(pkt));
224 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
225 break;
226 }
227
228 PRINT_INFO("FOLLOWUP", hdr, pkt);
229
230 sync_rcv_state->rcvd_follow_up = true;
231
232 /* If we already have one, drop the previous one. */
233 if (sync_rcv_state->rcvd_follow_up_ptr) {
234 net_pkt_unref(sync_rcv_state->rcvd_follow_up_ptr);
235 }
236
237 /* Keep the pkt alive until info is extracted. */
238 sync_rcv_state->rcvd_follow_up_ptr = net_pkt_ref(pkt);
239 NET_DBG("Keeping %s seq %d pkt %p", "FOLLOWUP",
240 ntohs(hdr->sequence_id), pkt);
241 break;
242
243 case GPTP_PATH_DELAY_FOLLOWUP_MESSAGE:
244 if (GPTP_CHECK_LEN(pkt, GPTP_PDELAY_RESP_FUP_LEN)) {
245 NET_WARN("Invalid length for %s packet "
246 "should have %zd bytes but has %zd bytes",
247 "PDELAY_FOLLOWUP",
248 GPTP_PDELAY_RESP_FUP_LEN,
249 GPTP_PACKET_LEN(pkt));
250 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
251 break;
252 }
253
254 PRINT_INFO("PDELAY_FOLLOWUP", hdr, pkt);
255
256 pdelay_req_state->rcvd_pdelay_follow_up++;
257
258 /* If we already have one, drop the received one. */
259 if (pdelay_req_state->rcvd_pdelay_follow_up_ptr) {
260 break;
261 }
262
263 /* Keep the buffer alive until pdelay_rate_ratio is computed. */
264 net_pkt_ref(pkt);
265 pdelay_req_state->rcvd_pdelay_follow_up_ptr = pkt;
266
267 GPTP_STATS_INC(port, rx_pdelay_resp_fup_count);
268 break;
269
270 case GPTP_ANNOUNCE_MESSAGE:
271 if (GPTP_ANNOUNCE_CHECK_LEN(pkt)) {
272 NET_WARN("Invalid length for %s packet "
273 "should have %zd bytes but has %zd bytes",
274 "ANNOUNCE",
275 GPTP_ANNOUNCE_LEN(pkt),
276 GPTP_PACKET_LEN(pkt));
277 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
278 break;
279 }
280
281 PRINT_INFO("ANNOUNCE", hdr, pkt);
282
283 pa_rcv_state = &GPTP_PORT_STATE(port)->pa_rcv;
284 bmca_data = GPTP_PORT_BMCA_DATA(port);
285
286 if (pa_rcv_state->rcvd_announce == false &&
287 bmca_data->rcvd_announce_ptr == NULL) {
288 pa_rcv_state->rcvd_announce = true;
289 bmca_data->rcvd_announce_ptr = pkt;
290 net_pkt_ref(pkt);
291 }
292
293 GPTP_STATS_INC(port, rx_announce_count);
294 break;
295
296 case GPTP_SIGNALING_MESSAGE:
297 if (GPTP_CHECK_LEN(pkt, GPTP_SIGNALING_LEN)) {
298 NET_WARN("Invalid length for %s packet "
299 "should have %zd bytes but has %zd bytes",
300 "SIGNALING",
301 GPTP_SIGNALING_LEN,
302 GPTP_PACKET_LEN(pkt));
303 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
304 break;
305 }
306
307 PRINT_INFO("SIGNALING", hdr, pkt);
308
309 gptp_handle_signaling(port, pkt);
310 break;
311
312 case GPTP_MANAGEMENT_MESSAGE:
313 PRINT_INFO("MANAGEMENT", hdr, pkt);
314 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
315 break;
316
317 default:
318 NET_DBG("Received unknown message %x", hdr->message_type);
319 GPTP_STATS_INC(port, rx_ptp_packet_discard_count);
320 break;
321 }
322 }
323
net_gptp_recv(struct net_if * iface,uint16_t ptype,struct net_pkt * pkt)324 static enum net_verdict net_gptp_recv(struct net_if *iface, uint16_t ptype,
325 struct net_pkt *pkt)
326 {
327 struct gptp_hdr *hdr = GPTP_HDR(pkt);
328
329 ARG_UNUSED(ptype);
330
331 if (!(net_eth_is_addr_ptp_multicast(
332 (struct net_eth_addr *)net_pkt_lladdr_dst(pkt)->addr) ||
333 net_eth_is_addr_lldp_multicast(
334 (struct net_eth_addr *)net_pkt_lladdr_dst(pkt)->addr))) {
335 return NET_DROP;
336 }
337
338 if ((hdr->ptp_version != GPTP_VERSION) ||
339 (hdr->transport_specific != GPTP_TRANSPORT_802_1_AS)) {
340 /* The stack only supports PTP V2 and transportSpecific set
341 * to 1 with IEEE802.1AS-2011.
342 */
343 return NET_DROP;
344 }
345
346 /* Handle critical messages. */
347 if (!gptp_handle_critical_msg(iface, pkt)) {
348 k_fifo_put(&gptp_rx_queue, pkt);
349
350 /* Returning OK here makes sure the network statistics are
351 * properly updated.
352 */
353 return NET_OK;
354 }
355
356 /* Message not propagated up in the stack. */
357 return NET_DROP;
358 }
359
360 ETH_NET_L3_REGISTER(gPTP, NET_ETH_PTYPE_PTP, net_gptp_recv);
361
gptp_init_clock_ds(void)362 static void gptp_init_clock_ds(void)
363 {
364 struct gptp_global_ds *global_ds;
365 struct gptp_default_ds *default_ds;
366 struct gptp_current_ds *current_ds;
367 struct gptp_parent_ds *parent_ds;
368 struct gptp_time_prop_ds *prop_ds;
369
370 global_ds = GPTP_GLOBAL_DS();
371 default_ds = GPTP_DEFAULT_DS();
372 current_ds = GPTP_CURRENT_DS();
373 parent_ds = GPTP_PARENT_DS();
374 prop_ds = GPTP_PROPERTIES_DS();
375
376 /* Initialize global data set. */
377 (void)memset(global_ds, 0, sizeof(struct gptp_global_ds));
378
379 /* Initialize default data set. */
380
381 /* Compute the clock identity from the first port MAC address. */
382 gptp_compute_clock_identity(GPTP_PORT_START);
383
384 default_ds->gm_capable = IS_ENABLED(CONFIG_NET_GPTP_GM_CAPABLE);
385 default_ds->clk_quality.clock_class = GPTP_CLASS_OTHER;
386 default_ds->clk_quality.clock_accuracy =
387 CONFIG_NET_GPTP_CLOCK_ACCURACY;
388 default_ds->clk_quality.offset_scaled_log_var =
389 GPTP_OFFSET_SCALED_LOG_VAR_UNKNOWN;
390
391 if (default_ds->gm_capable) {
392 /* The priority1 value cannot be 255 for GM capable
393 * system.
394 */
395 if (CONFIG_NET_GPTP_BMCA_PRIORITY1 ==
396 GPTP_PRIORITY1_NON_GM_CAPABLE) {
397 default_ds->priority1 = GPTP_PRIORITY1_GM_CAPABLE;
398 } else {
399 default_ds->priority1 = CONFIG_NET_GPTP_BMCA_PRIORITY1;
400 }
401 } else {
402 default_ds->priority1 = GPTP_PRIORITY1_NON_GM_CAPABLE;
403 }
404
405 default_ds->priority2 = GPTP_PRIORITY2_DEFAULT;
406
407 default_ds->cur_utc_offset = 37U; /* Current leap seconds TAI - UTC */
408 default_ds->flags.all = 0U;
409 default_ds->flags.octets[1] = GPTP_FLAG_TIME_TRACEABLE;
410 default_ds->time_source = GPTP_TS_INTERNAL_OSCILLATOR;
411
412 /* Initialize current data set. */
413 (void)memset(current_ds, 0, sizeof(struct gptp_current_ds));
414
415 /* Initialize parent data set. */
416
417 /* parent clock id is initialized to default_ds clock id. */
418 memcpy(parent_ds->port_id.clk_id, default_ds->clk_id,
419 GPTP_CLOCK_ID_LEN);
420 memcpy(parent_ds->gm_id, default_ds->clk_id, GPTP_CLOCK_ID_LEN);
421 parent_ds->port_id.port_number = 0U;
422
423 /* TODO: Check correct value for below field. */
424 parent_ds->cumulative_rate_ratio = 0;
425
426 parent_ds->gm_clk_quality.clock_class =
427 default_ds->clk_quality.clock_class;
428 parent_ds->gm_clk_quality.clock_accuracy =
429 default_ds->clk_quality.clock_accuracy;
430 parent_ds->gm_clk_quality.offset_scaled_log_var =
431 default_ds->clk_quality.offset_scaled_log_var;
432 parent_ds->gm_priority1 = default_ds->priority1;
433 parent_ds->gm_priority2 = default_ds->priority2;
434
435 /* Initialize properties data set. */
436
437 /* TODO: Get accurate values for below. From the GM. */
438 prop_ds->cur_utc_offset = 37U; /* Current leap seconds TAI - UTC */
439 prop_ds->cur_utc_offset_valid = false;
440 prop_ds->leap59 = false;
441 prop_ds->leap61 = false;
442 prop_ds->time_traceable = false;
443 prop_ds->freq_traceable = false;
444 prop_ds->time_source = GPTP_TS_INTERNAL_OSCILLATOR;
445
446 /* Set system values. */
447 global_ds->sys_flags.all = default_ds->flags.all;
448 global_ds->sys_current_utc_offset = default_ds->cur_utc_offset;
449 global_ds->sys_time_source = default_ds->time_source;
450 global_ds->clk_master_sync_itv =
451 NSEC_PER_SEC * GPTP_POW2(CONFIG_NET_GPTP_INIT_LOG_SYNC_ITV);
452 }
453
gptp_init_port_ds(int port)454 static void gptp_init_port_ds(int port)
455 {
456 struct gptp_default_ds *default_ds;
457 struct gptp_port_ds *port_ds;
458
459 #if defined(CONFIG_NET_GPTP_STATISTICS)
460 struct gptp_port_param_ds *port_param_ds;
461
462 port_param_ds = GPTP_PORT_PARAM_DS(port);
463 #endif
464
465 default_ds = GPTP_DEFAULT_DS();
466 port_ds = GPTP_PORT_DS(port);
467
468 /* Initialize port data set. */
469 memcpy(port_ds->port_id.clk_id, default_ds->clk_id, GPTP_CLOCK_ID_LEN);
470 port_ds->port_id.port_number = port;
471
472 port_ds->ptt_port_enabled = true;
473 port_ds->prev_ptt_port_enabled = true;
474
475 port_ds->neighbor_prop_delay = 0;
476 port_ds->neighbor_prop_delay_thresh = GPTP_NEIGHBOR_PROP_DELAY_THR;
477 port_ds->delay_asymmetry = 0;
478
479 port_ds->ini_log_announce_itv = CONFIG_NET_GPTP_INIT_LOG_ANNOUNCE_ITV;
480 port_ds->cur_log_announce_itv = port_ds->ini_log_announce_itv;
481 port_ds->announce_receipt_timeout =
482 CONFIG_NET_GPTP_ANNOUNCE_RECEIPT_TIMEOUT;
483
484 /* Subtract 1 to divide by 2 the sync interval. */
485 port_ds->ini_log_half_sync_itv = CONFIG_NET_GPTP_INIT_LOG_SYNC_ITV - 1;
486 port_ds->cur_log_half_sync_itv = port_ds->ini_log_half_sync_itv;
487 port_ds->sync_receipt_timeout = CONFIG_NET_GPTP_SYNC_RECEIPT_TIMEOUT;
488 port_ds->sync_receipt_timeout_time_itv = 10000000U; /* 10ms */
489
490 port_ds->ini_log_pdelay_req_itv =
491 CONFIG_NET_GPTP_INIT_LOG_PDELAY_REQ_ITV;
492 port_ds->cur_log_pdelay_req_itv = port_ds->ini_log_pdelay_req_itv;
493 port_ds->allowed_lost_responses = GPTP_ALLOWED_LOST_RESP;
494 port_ds->version = GPTP_VERSION;
495
496 gptp_set_time_itv(&port_ds->pdelay_req_itv, 1,
497 port_ds->cur_log_pdelay_req_itv);
498
499 gptp_set_time_itv(&port_ds->half_sync_itv, 1,
500 port_ds->cur_log_half_sync_itv);
501
502 port_ds->compute_neighbor_rate_ratio = true;
503 port_ds->compute_neighbor_prop_delay = true;
504
505 /* Random Sequence Numbers. */
506 port_ds->sync_seq_id = sys_rand16_get();
507 port_ds->pdelay_req_seq_id = sys_rand16_get();
508 port_ds->announce_seq_id = sys_rand16_get();
509 port_ds->signaling_seq_id = sys_rand16_get();
510
511 #if defined(CONFIG_NET_GPTP_STATISTICS)
512 /* Initialize stats data set. */
513 (void)memset(port_param_ds, 0, sizeof(struct gptp_port_param_ds));
514 #endif
515 }
516
gptp_init_state_machine(void)517 static void gptp_init_state_machine(void)
518 {
519 gptp_md_init_state_machine();
520 gptp_mi_init_state_machine();
521 }
522
gptp_state_machine(void)523 static void gptp_state_machine(void)
524 {
525 int port;
526
527 /* Manage port states. */
528 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
529 struct gptp_port_ds *port_ds = GPTP_PORT_DS(port);
530
531 /* If interface is down, don't move forward */
532 if (net_if_flag_is_set(GPTP_PORT_IFACE(port), NET_IF_UP)) {
533 switch (GPTP_GLOBAL_DS()->selected_role[port]) {
534 case GPTP_PORT_DISABLED:
535 case GPTP_PORT_MASTER:
536 case GPTP_PORT_PASSIVE:
537 case GPTP_PORT_SLAVE:
538 gptp_md_state_machines(port);
539 gptp_mi_port_sync_state_machines(port);
540 gptp_mi_port_bmca_state_machines(port);
541 break;
542 default:
543 NET_DBG("%s: Unknown port state", __func__);
544 break;
545 }
546 } else {
547 GPTP_GLOBAL_DS()->selected_role[port] = GPTP_PORT_DISABLED;
548 }
549
550 port_ds->prev_ptt_port_enabled = port_ds->ptt_port_enabled;
551 }
552
553 gptp_mi_state_machines();
554 }
555
gptp_thread(void * p1,void * p2,void * p3)556 static void gptp_thread(void *p1, void *p2, void *p3)
557 {
558 ARG_UNUSED(p1);
559 ARG_UNUSED(p2);
560 ARG_UNUSED(p3);
561
562 int port;
563
564 NET_DBG("Starting PTP thread");
565
566 gptp_init_clock_ds();
567
568 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
569 gptp_init_port_ds(port);
570 gptp_change_port_state(port, GPTP_PORT_DISABLED);
571 }
572
573 while (1) {
574 struct net_pkt *pkt;
575
576 pkt = k_fifo_get(&gptp_rx_queue,
577 K_MSEC(GPTP_THREAD_WAIT_TIMEOUT_MS));
578 if (pkt) {
579 gptp_handle_msg(pkt);
580 net_pkt_unref(pkt);
581 }
582
583 gptp_state_machine();
584 }
585 }
586
587
gptp_add_port(struct net_if * iface,void * user_data)588 static void gptp_add_port(struct net_if *iface, void *user_data)
589 {
590 uint8_t *num_ports = user_data;
591 const struct device *clk;
592
593 if (*num_ports >= CONFIG_NET_GPTP_NUM_PORTS) {
594 return;
595 }
596
597 /* Check if interface has a PTP clock. */
598 clk = net_eth_get_ptp_clock(iface);
599 if (clk) {
600 gptp_domain.iface[*num_ports] = iface;
601 net_eth_set_ptp_port(iface, *num_ports);
602 (*num_ports)++;
603 }
604 }
605
gptp_set_time_itv(struct gptp_uscaled_ns * interval,uint16_t seconds,int8_t log_msg_interval)606 void gptp_set_time_itv(struct gptp_uscaled_ns *interval,
607 uint16_t seconds,
608 int8_t log_msg_interval)
609 {
610 int i;
611
612 if (seconds == 0U) {
613 interval->low = 0U;
614 interval->high = 0U;
615 return;
616 } else if (log_msg_interval >= 96) {
617 /* Overflow, set maximum. */
618 interval->low = UINT64_MAX;
619 interval->high = UINT32_MAX;
620
621 return;
622 } else if (log_msg_interval <= -64) {
623 /* Underflow, set to 0. */
624 interval->low = 0U;
625 interval->high = 0U;
626 return;
627 }
628
629
630 /* NSEC_PER_SEC is between 2^30 and 2^31, seconds is less than 2^16,
631 * thus the computation will be less than 2^63.
632 */
633 interval->low = (seconds * (uint64_t)NSEC_PER_SEC) << 16;
634
635 if (log_msg_interval <= 0) {
636 interval->low >>= -log_msg_interval;
637 interval->high = 0U;
638 } else {
639 /* Find highest bit set. */
640 for (i = 63; i >= 0; i--) {
641 if (interval->low >> i) {
642 break;
643 }
644 }
645
646 if ((i + log_msg_interval) >= 96 || log_msg_interval > 64) {
647 /* Overflow, set maximum. */
648 interval->low = UINT64_MAX;
649 interval->high = UINT32_MAX;
650 } else {
651 interval->high =
652 interval->low >> (64 - log_msg_interval);
653
654 /* << operator is undefined if the shift value is equal
655 * to the number of bits in the left expression’s type
656 */
657 if (log_msg_interval == 64) {
658 interval->low = 0U;
659 } else {
660 interval->low <<= log_msg_interval;
661 }
662 }
663 }
664 }
665
gptp_uscaled_ns_to_timer_ms(struct gptp_uscaled_ns * usns)666 int32_t gptp_uscaled_ns_to_timer_ms(struct gptp_uscaled_ns *usns)
667 {
668 uint64_t tmp;
669
670 if (usns->high) {
671 /* Do not calculate, it reaches max value. */
672 return INT32_MAX;
673 }
674
675 tmp = (usns->low >> 16) / USEC_PER_SEC;
676 if (tmp == 0U) {
677 /* Timer must be started with a minimum value of 1. */
678 return 1;
679 }
680
681 if (tmp > INT32_MAX) {
682 return INT32_MAX;
683 }
684
685 return (tmp & INT32_MAX);
686
687 }
688
timer_get_remaining_and_stop(struct k_timer * timer)689 static int32_t timer_get_remaining_and_stop(struct k_timer *timer)
690 {
691 unsigned int key;
692 int32_t timer_value;
693
694 key = irq_lock();
695 timer_value = k_timer_remaining_get(timer);
696
697 /* Stop timer as the period is about to be modified. */
698 k_timer_stop(timer);
699 irq_unlock(key);
700
701 return timer_value;
702 }
703
update_itv(struct gptp_uscaled_ns * itv,int8_t * cur_log_itv,int8_t * ini_log_itv,int8_t new_log_itv,int8_t correction_log_itv)704 static int32_t update_itv(struct gptp_uscaled_ns *itv,
705 int8_t *cur_log_itv,
706 int8_t *ini_log_itv,
707 int8_t new_log_itv,
708 int8_t correction_log_itv)
709 {
710 switch (new_log_itv) {
711 case GPTP_ITV_KEEP:
712 break;
713 case GPTP_ITV_SET_TO_INIT:
714 *cur_log_itv = *ini_log_itv;
715 gptp_set_time_itv(itv, 1, *ini_log_itv);
716 break;
717 case GPTP_ITV_STOP:
718 default:
719 *cur_log_itv = new_log_itv + correction_log_itv;
720 gptp_set_time_itv(itv, 1, *cur_log_itv);
721 break;
722 }
723
724 return gptp_uscaled_ns_to_timer_ms(itv);
725 }
726
gptp_update_pdelay_req_interval(int port,int8_t log_val)727 void gptp_update_pdelay_req_interval(int port, int8_t log_val)
728 {
729 int32_t remaining;
730 int32_t new_itv, old_itv;
731 struct gptp_pdelay_req_state *state_pdelay;
732 struct gptp_port_ds *port_ds;
733
734 port_ds = GPTP_PORT_DS(port);
735 state_pdelay = &GPTP_PORT_STATE(port)->pdelay_req;
736 remaining = timer_get_remaining_and_stop(&state_pdelay->pdelay_timer);
737
738 old_itv = gptp_uscaled_ns_to_timer_ms(&port_ds->pdelay_req_itv);
739 new_itv = update_itv(&port_ds->pdelay_req_itv,
740 &port_ds->cur_log_pdelay_req_itv,
741 &port_ds->ini_log_pdelay_req_itv,
742 log_val,
743 0);
744
745 new_itv -= (old_itv-remaining);
746 if (new_itv <= 0) {
747 new_itv = 1;
748 }
749
750 k_timer_start(&state_pdelay->pdelay_timer, K_MSEC(new_itv), K_NO_WAIT);
751 }
752
gptp_update_sync_interval(int port,int8_t log_val)753 void gptp_update_sync_interval(int port, int8_t log_val)
754 {
755 struct gptp_pss_send_state *state_pss_send;
756 struct gptp_port_ds *port_ds;
757 int32_t new_itv, old_itv, period;
758 int32_t remaining;
759 uint32_t time_spent;
760
761 port_ds = GPTP_PORT_DS(port);
762 state_pss_send = &GPTP_PORT_STATE(port)->pss_send;
763 remaining =
764 timer_get_remaining_and_stop(
765 &state_pss_send->half_sync_itv_timer);
766 old_itv = gptp_uscaled_ns_to_timer_ms(&port_ds->half_sync_itv);
767 new_itv = update_itv(&port_ds->half_sync_itv,
768 &port_ds->cur_log_half_sync_itv,
769 &port_ds->ini_log_half_sync_itv,
770 log_val,
771 -1);
772 period = new_itv;
773
774 /* Get the time spent from the start of the timer. */
775 time_spent = old_itv;
776 if (state_pss_send->half_sync_itv_timer_expired) {
777 time_spent *= 2U;
778 }
779 time_spent -= remaining;
780
781 /* Calculate remaining time and if half timer has expired. */
782 if ((time_spent / 2U) > new_itv) {
783 state_pss_send->sync_itv_timer_expired = true;
784 state_pss_send->half_sync_itv_timer_expired = true;
785 new_itv = 1;
786 } else if (time_spent > new_itv) {
787 state_pss_send->sync_itv_timer_expired = false;
788 state_pss_send->half_sync_itv_timer_expired = true;
789 new_itv -= (time_spent - new_itv);
790 } else {
791 state_pss_send->sync_itv_timer_expired = false;
792 state_pss_send->half_sync_itv_timer_expired = false;
793 new_itv -= time_spent;
794 }
795
796 if (new_itv <= 0) {
797 new_itv = 1;
798 }
799
800 k_timer_start(&state_pss_send->half_sync_itv_timer, K_MSEC(new_itv),
801 K_MSEC(period));
802 }
803
gptp_update_announce_interval(int port,int8_t log_val)804 void gptp_update_announce_interval(int port, int8_t log_val)
805 {
806 int32_t remaining;
807 int32_t new_itv, old_itv;
808 struct gptp_port_announce_transmit_state *state_ann;
809 struct gptp_port_bmca_data *bmca_data;
810 struct gptp_port_ds *port_ds;
811
812 port_ds = GPTP_PORT_DS(port);
813 state_ann = &GPTP_PORT_STATE(port)->pa_transmit;
814 bmca_data = GPTP_PORT_BMCA_DATA(port);
815 remaining = timer_get_remaining_and_stop(
816 &state_ann->ann_send_periodic_timer);
817
818 old_itv = gptp_uscaled_ns_to_timer_ms(&bmca_data->announce_interval);
819 new_itv = update_itv(&bmca_data->announce_interval,
820 &port_ds->cur_log_announce_itv,
821 &port_ds->ini_log_announce_itv,
822 log_val,
823 0);
824
825 new_itv -= (old_itv-remaining);
826 if (new_itv <= 0) {
827 new_itv = 1;
828 }
829
830 k_timer_start(&state_ann->ann_send_periodic_timer, K_MSEC(new_itv),
831 K_NO_WAIT);
832 }
833
834 struct port_user_data {
835 gptp_port_cb_t cb;
836 void *user_data;
837 };
838
gptp_get_port(struct net_if * iface,void * user_data)839 static void gptp_get_port(struct net_if *iface, void *user_data)
840 {
841 struct port_user_data *ud = user_data;
842 const struct device *clk;
843
844 /* Check if interface has a PTP clock. */
845 clk = net_eth_get_ptp_clock(iface);
846 if (clk) {
847 int port = gptp_get_port_number(iface);
848
849 if (port < 0) {
850 return;
851 }
852
853 ud->cb(port, iface, ud->user_data);
854 }
855 }
856
gptp_foreach_port(gptp_port_cb_t cb,void * user_data)857 void gptp_foreach_port(gptp_port_cb_t cb, void *user_data)
858 {
859 struct port_user_data ud = {
860 .cb = cb,
861 .user_data = user_data
862 };
863
864 net_if_foreach(gptp_get_port, &ud);
865 }
866
gptp_get_domain(void)867 struct gptp_domain *gptp_get_domain(void)
868 {
869 return &gptp_domain;
870 }
871
gptp_get_port_data(struct gptp_domain * domain,int port,struct gptp_port_ds ** port_ds,struct gptp_port_param_ds ** port_param_ds,struct gptp_port_states ** port_state,struct gptp_port_bmca_data ** port_bmca_data,struct net_if ** iface)872 int gptp_get_port_data(struct gptp_domain *domain,
873 int port,
874 struct gptp_port_ds **port_ds,
875 struct gptp_port_param_ds **port_param_ds,
876 struct gptp_port_states **port_state,
877 struct gptp_port_bmca_data **port_bmca_data,
878 struct net_if **iface)
879 {
880 if (domain != &gptp_domain) {
881 return -ENOENT;
882 }
883
884 if (port < GPTP_PORT_START || port >= GPTP_PORT_END) {
885 return -EINVAL;
886 }
887
888 if (port_ds) {
889 *port_ds = GPTP_PORT_DS(port);
890 }
891
892 if (port_param_ds) {
893 #if defined(CONFIG_NET_GPTP_STATISTICS)
894 *port_param_ds = GPTP_PORT_PARAM_DS(port);
895 #else
896 *port_param_ds = NULL;
897 #endif
898 }
899
900 if (port_state) {
901 *port_state = GPTP_PORT_STATE(port);
902 }
903
904 if (port_bmca_data) {
905 *port_bmca_data = GPTP_PORT_BMCA_DATA(port);
906 }
907
908 if (iface) {
909 *iface = GPTP_PORT_IFACE(port);
910 }
911
912 return 0;
913 }
914
gptp_servo_pi(int64_t nanosecond_diff)915 double gptp_servo_pi(int64_t nanosecond_diff)
916 {
917 double kp = 0.7;
918 double ki = 0.3;
919 double ppb;
920
921 gptp_clock.pi_drift += ki * nanosecond_diff;
922 ppb = kp * nanosecond_diff + gptp_clock.pi_drift;
923
924 return ppb;
925 }
926
init_ports(void)927 static void init_ports(void)
928 {
929 net_if_foreach(gptp_add_port, &gptp_domain.default_ds.nb_ports);
930
931 /* Only initialize the state machine once the ports are known. */
932 gptp_init_state_machine();
933
934 tid = k_thread_create(&gptp_thread_data, gptp_stack,
935 K_KERNEL_STACK_SIZEOF(gptp_stack),
936 gptp_thread,
937 NULL, NULL, NULL, K_PRIO_COOP(5), 0, K_NO_WAIT);
938 k_thread_name_set(&gptp_thread_data, "gptp");
939 }
940
net_gptp_init(void)941 void net_gptp_init(void)
942 {
943 gptp_domain.default_ds.nb_ports = 0U;
944
945 gptp_clock.domain = &gptp_domain;
946 gptp_clock.pi_drift = 0.0;
947
948 init_ports();
949 }
950