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_DECLARE(net_gptp, CONFIG_NET_GPTP_LOG_LEVEL);
9
10 #include <zephyr/drivers/ptp_clock.h>
11
12 #include "gptp_messages.h"
13 #include "gptp_data_set.h"
14 #include "gptp_state.h"
15 #include "gptp_private.h"
16
17 #if CONFIG_NET_GPTP_LOG_LEVEL >= LOG_LEVEL_DBG
state2str(enum gptp_port_state state)18 static const char * const state2str(enum gptp_port_state state)
19 {
20 switch (state) {
21 case GPTP_PORT_INITIALIZING:
22 return "INITIALIZING";
23 case GPTP_PORT_FAULTY:
24 return "FAULTY";
25 case GPTP_PORT_DISABLED:
26 return "DISABLED";
27 case GPTP_PORT_LISTENING:
28 return "LISTENING";
29 case GPTP_PORT_PRE_MASTER:
30 return "PRE_MASTER";
31 case GPTP_PORT_MASTER:
32 return "MASTER";
33 case GPTP_PORT_PASSIVE:
34 return "PASSIVE";
35 case GPTP_PORT_UNCALIBRATED:
36 return "UNCALIBRATED";
37 case GPTP_PORT_SLAVE:
38 return "SLAVE";
39 }
40
41 return "<unknown>";
42 }
43
pa_info_state2str(enum gptp_pa_info_states state)44 static const char * const pa_info_state2str(enum gptp_pa_info_states state)
45 {
46 switch (state) {
47 case GPTP_PA_INFO_DISABLED:
48 return "DISABLED";
49 case GPTP_PA_INFO_POST_DISABLED:
50 return "POST_DISABLED";
51 case GPTP_PA_INFO_AGED:
52 return "AGED";
53 case GPTP_PA_INFO_UPDATE:
54 return "UPDATE";
55 case GPTP_PA_INFO_CURRENT:
56 return "CURRENT";
57 case GPTP_PA_INFO_RECEIVE:
58 return "RECEIVE";
59 case GPTP_PA_INFO_SUPERIOR_MASTER_PORT:
60 return "SUPERIOR_MASTER_PORT";
61 case GPTP_PA_INFO_REPEATED_MASTER_PORT:
62 return "REPEATED_MASTER_PORT";
63 case GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT:
64 return "INFERIOR_MASTER_OR_OTHER_PORT";
65 }
66
67 return "<unknown>";
68 }
69 #endif
70
71 #if CONFIG_NET_GPTP_LOG_LEVEL >= LOG_LEVEL_DBG
gptp_change_port_state_debug(int port,enum gptp_port_state state,const char * caller,int line)72 void gptp_change_port_state_debug(int port, enum gptp_port_state state,
73 const char *caller,
74 int line)
75 #else
76 void gptp_change_port_state(int port, enum gptp_port_state state)
77 #endif
78 {
79 struct gptp_global_ds *global_ds = GPTP_GLOBAL_DS();
80
81 if (global_ds->selected_role[port] == state) {
82 return;
83 }
84
85 #if CONFIG_NET_GPTP_LOG_LEVEL >= LOG_LEVEL_DBG
86 NET_DBG("[%d] state %s -> %s (%s():%d)", port,
87 state2str(global_ds->selected_role[port]),
88 state2str(state), caller, line);
89 #endif
90
91 global_ds->selected_role[port] = state;
92 };
93
94 #if CONFIG_NET_GPTP_LOG_LEVEL >= LOG_LEVEL_DBG
gptp_change_pa_info_state_debug(int port,struct gptp_port_announce_information_state * pa_info_state,enum gptp_pa_info_states state,const char * caller,int line)95 void gptp_change_pa_info_state_debug(
96 int port,
97 struct gptp_port_announce_information_state *pa_info_state,
98 enum gptp_pa_info_states state,
99 const char *caller,
100 int line)
101 #else
102 void gptp_change_pa_info_state(
103 int port,
104 struct gptp_port_announce_information_state *pa_info_state,
105 enum gptp_pa_info_states state)
106 #endif
107 {
108 if (pa_info_state->state == state) {
109 return;
110 }
111
112 #if CONFIG_NET_GPTP_LOG_LEVEL >= LOG_LEVEL_DBG
113 NET_DBG("[%d] PA info state %s -> %s (%s():%d)", port,
114 pa_info_state2str(pa_info_state->state),
115 pa_info_state2str(state), caller, line);
116 #endif
117
118 pa_info_state->state = state;
119 }
120
gptp_mi_half_sync_itv_timeout(struct k_timer * timer)121 static void gptp_mi_half_sync_itv_timeout(struct k_timer *timer)
122 {
123 struct gptp_pss_send_state *state;
124 int port;
125
126 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
127 state = &GPTP_PORT_STATE(port)->pss_send;
128 if (&state->half_sync_itv_timer == timer) {
129 if (!state->half_sync_itv_timer_expired) {
130 state->half_sync_itv_timer_expired = true;
131 } else {
132 /* We do not need the timer anymore. */
133 k_timer_stop(timer);
134
135 state->sync_itv_timer_expired = true;
136 }
137 }
138 }
139 }
140
gptp_mi_rcv_sync_receipt_timeout(struct k_timer * timer)141 static void gptp_mi_rcv_sync_receipt_timeout(struct k_timer *timer)
142 {
143 struct gptp_pss_rcv_state *state;
144 int port;
145
146 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
147 state = &GPTP_PORT_STATE(port)->pss_rcv;
148 if (&state->rcv_sync_receipt_timeout_timer == timer) {
149 state->rcv_sync_receipt_timeout_timer_expired = true;
150 }
151
152 GPTP_STATS_INC(port, sync_receipt_timeout_count);
153 }
154 }
155
gptp_mi_send_sync_receipt_timeout(struct k_timer * timer)156 static void gptp_mi_send_sync_receipt_timeout(struct k_timer *timer)
157 {
158 struct gptp_pss_send_state *state;
159 int port;
160
161 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
162 state = &GPTP_PORT_STATE(port)->pss_send;
163 if (&state->send_sync_receipt_timeout_timer == timer) {
164 state->send_sync_receipt_timeout_timer_expired = true;
165 }
166
167 GPTP_STATS_INC(port, sync_receipt_timeout_count);
168 }
169 }
170
gptp_mi_init_port_sync_sync_rcv_sm(int port)171 static void gptp_mi_init_port_sync_sync_rcv_sm(int port)
172 {
173 struct gptp_pss_rcv_state *pss_rcv;
174
175 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
176 (void)memset(pss_rcv, 0, sizeof(struct gptp_pss_rcv_state));
177
178 k_timer_init(&pss_rcv->rcv_sync_receipt_timeout_timer,
179 gptp_mi_rcv_sync_receipt_timeout, NULL);
180
181 pss_rcv->state = GPTP_PSS_RCV_DISCARD;
182 }
183
gptp_mi_init_port_sync_sync_send_sm(int port)184 static void gptp_mi_init_port_sync_sync_send_sm(int port)
185 {
186 struct gptp_pss_send_state *pss_send;
187
188 pss_send = &GPTP_PORT_STATE(port)->pss_send;
189 (void)memset(pss_send, 0, sizeof(struct gptp_pss_send_state));
190
191 k_timer_init(&pss_send->half_sync_itv_timer,
192 gptp_mi_half_sync_itv_timeout, NULL);
193 k_timer_init(&pss_send->send_sync_receipt_timeout_timer,
194 gptp_mi_send_sync_receipt_timeout, NULL);
195
196 pss_send->state = GPTP_PSS_SEND_TRANSMIT_INIT;
197 }
198
gptp_mi_init_site_sync_sync_sm(void)199 static void gptp_mi_init_site_sync_sync_sm(void)
200 {
201 struct gptp_site_sync_sync_state *site_ss;
202
203 site_ss = &GPTP_STATE()->site_ss;
204 (void)memset(site_ss, 0, sizeof(struct gptp_site_sync_sync_state));
205 site_ss->state = GPTP_SSS_INITIALIZING;
206 }
207
gptp_mi_init_clock_slave_sync_sm(void)208 static void gptp_mi_init_clock_slave_sync_sm(void)
209 {
210 struct gptp_clk_slave_sync_state *clk_ss;
211
212 clk_ss = &GPTP_STATE()->clk_slave_sync;
213 (void)memset(clk_ss, 0, sizeof(struct gptp_clk_slave_sync_state));
214 clk_ss->state = GPTP_CLK_SLAVE_SYNC_INITIALIZING;
215 }
216
gptp_mi_init_port_announce_rcv_sm(int port)217 static void gptp_mi_init_port_announce_rcv_sm(int port)
218 {
219 struct gptp_port_announce_receive_state *pa_rcv;
220
221 pa_rcv = &GPTP_PORT_STATE(port)->pa_rcv;
222 (void)memset(pa_rcv, 0,
223 sizeof(struct gptp_port_announce_receive_state));
224 pa_rcv->state = GPTP_PA_RCV_DISCARD;
225 }
226
gptp_mi_init_clock_master_sync_rcv_sm(void)227 static void gptp_mi_init_clock_master_sync_rcv_sm(void)
228 {
229 struct gptp_clk_master_sync_rcv_state *cms_rcv;
230
231 cms_rcv = &GPTP_STATE()->clk_master_sync_receive;
232 (void)memset(cms_rcv, 0, sizeof(struct gptp_clk_master_sync_rcv_state));
233 cms_rcv->state = GPTP_CMS_RCV_INITIALIZING;
234 }
235
announce_timer_handler(struct k_timer * timer)236 static void announce_timer_handler(struct k_timer *timer)
237 {
238 int port;
239 struct gptp_port_announce_information_state *state;
240
241 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
242 state = &GPTP_PORT_STATE(port)->pa_info;
243 if (&state->ann_rcpt_expiry_timer == timer) {
244 state->ann_expired = true;
245 GPTP_STATS_INC(port, announce_receipt_timeout_count);
246 break;
247 }
248 }
249 }
250
gptp_mi_init_port_announce_info_sm(int port)251 static void gptp_mi_init_port_announce_info_sm(int port)
252 {
253 struct gptp_port_announce_information_state *state;
254
255 state = &GPTP_PORT_STATE(port)->pa_info;
256
257 k_timer_init(&state->ann_rcpt_expiry_timer,
258 announce_timer_handler, NULL);
259
260 state->ann_expired = false;
261 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_DISABLED);
262 }
263
gptp_mi_init_bmca_data(int port)264 static void gptp_mi_init_bmca_data(int port)
265 {
266 struct gptp_port_bmca_data *bmca_data;
267
268 bmca_data = GPTP_PORT_BMCA_DATA(port);
269
270 (void)memset(bmca_data, 0, sizeof(struct gptp_port_bmca_data));
271
272 gptp_set_time_itv(&bmca_data->announce_interval, 1,
273 CONFIG_NET_GPTP_INIT_LOG_ANNOUNCE_ITV);
274
275 (void)memset(&bmca_data->port_priority, 0xFF,
276 sizeof(struct gptp_priority_vector));
277 (void)memset(&bmca_data->master_priority, 0xFF,
278 sizeof(struct gptp_priority_vector));
279 }
280
announce_periodic_timer_handler(struct k_timer * timer)281 static void announce_periodic_timer_handler(struct k_timer *timer)
282 {
283 int port;
284 struct gptp_port_announce_transmit_state *state;
285
286 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
287 state = &GPTP_PORT_STATE(port)->pa_transmit;
288 if (&state->ann_send_periodic_timer == timer) {
289 state->ann_trigger = true;
290 break;
291 }
292 }
293 }
294
gptp_mi_init_port_announce_transmit_sm(int port)295 static void gptp_mi_init_port_announce_transmit_sm(int port)
296 {
297 struct gptp_port_announce_transmit_state *state;
298
299 state = &GPTP_PORT_STATE(port)->pa_transmit;
300
301 k_timer_init(&state->ann_send_periodic_timer,
302 announce_periodic_timer_handler, NULL);
303
304 state->ann_trigger = false;
305 state->state = GPTP_PA_TRANSMIT_INIT;
306 }
307
gptp_mi_init_port_role_selection_sm(void)308 static void gptp_mi_init_port_role_selection_sm(void)
309 {
310 GPTP_STATE()->pr_sel.state = GPTP_PR_SELECTION_INIT_BRIDGE;
311 }
312
gptp_mi_init_state_machine(void)313 void gptp_mi_init_state_machine(void)
314 {
315 int port;
316
317 for (port = GPTP_PORT_START;
318 port < (GPTP_PORT_START + CONFIG_NET_GPTP_NUM_PORTS); port++) {
319 gptp_mi_init_port_sync_sync_rcv_sm(port);
320 gptp_mi_init_port_sync_sync_send_sm(port);
321 gptp_mi_init_port_announce_rcv_sm(port);
322 gptp_mi_init_port_announce_info_sm(port);
323 gptp_mi_init_port_announce_transmit_sm(port);
324 gptp_mi_init_bmca_data(port);
325 }
326
327 gptp_mi_init_site_sync_sync_sm();
328 gptp_mi_init_clock_slave_sync_sm();
329 gptp_mi_init_port_role_selection_sm();
330 gptp_mi_init_clock_master_sync_rcv_sm();
331 }
332
gptp_get_current_time_nanosecond(int port)333 uint64_t gptp_get_current_time_nanosecond(int port)
334 {
335 const struct device *clk;
336
337 clk = net_eth_get_ptp_clock(GPTP_PORT_IFACE(port));
338 if (clk) {
339 struct net_ptp_time tm = {};
340
341 ptp_clock_get(clk, &tm);
342
343 if (tm.second == 0U && tm.nanosecond == 0U) {
344 goto use_uptime;
345 }
346
347 return gptp_timestamp_to_nsec(&tm);
348 }
349
350 use_uptime:
351 /* A workaround if clock cannot be found. Note that accuracy is
352 * only in milliseconds.
353 */
354 return k_uptime_get() * 1000000;
355 }
356
gptp_get_current_master_time_nanosecond(void)357 uint64_t gptp_get_current_master_time_nanosecond(void)
358 {
359 int port;
360 enum gptp_port_state *port_role;
361
362 port_role = GPTP_GLOBAL_DS()->selected_role;
363
364 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
365 if (port_role[port] == GPTP_PORT_MASTER) {
366 return gptp_get_current_time_nanosecond(port);
367 }
368 }
369
370 /* No master */
371 return 0;
372 }
373
gptp_mi_pss_rcv_compute(int port)374 static void gptp_mi_pss_rcv_compute(int port)
375 {
376 struct gptp_pss_rcv_state *state;
377 struct gptp_mi_port_sync_sync *pss;
378 struct gptp_md_sync_info *sync_rcv;
379 struct gptp_port_ds *port_ds;
380
381 state = &GPTP_PORT_STATE(port)->pss_rcv;
382 pss = &state->pss;
383 sync_rcv = &state->sync_rcv;
384 port_ds = GPTP_PORT_DS(port);
385
386 state->rate_ratio = sync_rcv->rate_ratio;
387 state->rate_ratio += (port_ds->neighbor_rate_ratio - 1.0);
388
389 port_ds->sync_receipt_timeout_time_itv = port_ds->sync_receipt_timeout;
390 port_ds->sync_receipt_timeout_time_itv *= NSEC_PER_SEC;
391 port_ds->sync_receipt_timeout_time_itv *=
392 GPTP_POW2(sync_rcv->log_msg_interval);
393
394 pss->local_port_number = port;
395
396 memcpy(&pss->sync_info, sync_rcv, sizeof(struct gptp_md_sync_info));
397
398 pss->sync_receipt_timeout_time = gptp_get_current_time_nanosecond(port);
399 pss->sync_receipt_timeout_time +=
400 port_ds->sync_receipt_timeout_time_itv;
401
402 pss->sync_info.rate_ratio = state->rate_ratio;
403 }
404
start_rcv_sync_timer(struct gptp_port_ds * port_ds,struct gptp_pss_rcv_state * state)405 static void start_rcv_sync_timer(struct gptp_port_ds *port_ds,
406 struct gptp_pss_rcv_state *state)
407 {
408 k_timeout_t duration;
409
410 duration = K_MSEC(port_ds->sync_receipt_timeout_time_itv /
411 (NSEC_PER_USEC * USEC_PER_MSEC));
412
413 k_timer_start(&state->rcv_sync_receipt_timeout_timer, duration,
414 K_NO_WAIT);
415 }
416
gptp_mi_pss_rcv_state_machine(int port)417 static void gptp_mi_pss_rcv_state_machine(int port)
418 {
419 struct gptp_pss_rcv_state *state;
420 struct gptp_site_sync_sync_state *site_ss_state;
421 struct gptp_port_ds *port_ds;
422
423 state = &GPTP_PORT_STATE(port)->pss_rcv;
424 site_ss_state = &GPTP_STATE()->site_ss;
425 port_ds = GPTP_PORT_DS(port);
426
427 if ((!port_ds->ptt_port_enabled) || !port_ds->as_capable) {
428 state->rcvd_md_sync = false;
429 state->state = GPTP_PSS_RCV_DISCARD;
430 return;
431 }
432
433 switch (state->state) {
434 case GPTP_PSS_RCV_DISCARD:
435 k_timer_stop(&state->rcv_sync_receipt_timeout_timer);
436 state->rcv_sync_receipt_timeout_timer_expired = false;
437
438 __fallthrough;
439 case GPTP_PSS_RCV_RECEIVED_SYNC:
440 if (state->rcvd_md_sync) {
441 state->rcvd_md_sync = false;
442 gptp_mi_pss_rcv_compute(port);
443
444 state->state = GPTP_PSS_RCV_RECEIVED_SYNC;
445
446 site_ss_state->pss_rcv_ptr = &state->pss;
447 site_ss_state->rcvd_pss = true;
448
449 k_timer_stop(&state->rcv_sync_receipt_timeout_timer);
450 state->rcv_sync_receipt_timeout_timer_expired = false;
451
452 if (GPTP_GLOBAL_DS()->gm_present) {
453 start_rcv_sync_timer(port_ds, state);
454 }
455 }
456
457 break;
458 }
459 }
460
gptp_mi_pss_store_last_pss(int port)461 static void gptp_mi_pss_store_last_pss(int port)
462 {
463 struct gptp_pss_send_state *state;
464 struct gptp_mi_port_sync_sync *pss_ptr;
465 struct gptp_md_sync_info *sync_info;
466
467 state = &GPTP_PORT_STATE(port)->pss_send;
468 pss_ptr = state->pss_sync_ptr;
469 sync_info = &pss_ptr->sync_info;
470
471 state->last_rcvd_port_num = pss_ptr->local_port_number;
472
473 memcpy(&state->last_precise_orig_ts, &sync_info->precise_orig_ts,
474 sizeof(struct net_ptp_time));
475 memcpy(&state->last_gm_phase_change, &sync_info->last_gm_phase_change,
476 sizeof(struct gptp_scaled_ns));
477
478 state->last_follow_up_correction_field =
479 sync_info->follow_up_correction_field;
480 state->last_rate_ratio = sync_info->rate_ratio;
481 state->last_upstream_tx_time = sync_info->upstream_tx_time;
482 state->last_gm_time_base_indicator = sync_info->gm_time_base_indicator;
483 state->last_gm_freq_change = sync_info->last_gm_freq_change;
484 }
485
gptp_mi_pss_send_md_sync_send(int port)486 static void gptp_mi_pss_send_md_sync_send(int port)
487 {
488 struct gptp_pss_send_state *state;
489 struct gptp_mi_port_sync_sync *pss_ptr;
490 struct gptp_sync_send_state *sync_send;
491
492 state = &GPTP_PORT_STATE(port)->pss_send;
493 pss_ptr = state->pss_sync_ptr;
494 sync_send = &GPTP_PORT_STATE(port)->sync_send;
495
496 memcpy(&state->sync_send, &pss_ptr->sync_info,
497 sizeof(struct gptp_md_sync_info));
498
499 sync_send->sync_send_ptr = &state->sync_send;
500 sync_send->rcvd_md_sync = true;
501 }
502
gptp_mi_pss_send_state_machine(int port)503 static void gptp_mi_pss_send_state_machine(int port)
504 {
505 struct gptp_pss_send_state *state;
506 struct gptp_port_ds *port_ds;
507 struct gptp_global_ds *global_ds;
508 k_timeout_t duration;
509
510 global_ds = GPTP_GLOBAL_DS();
511 state = &GPTP_PORT_STATE(port)->pss_send;
512 port_ds = GPTP_PORT_DS(port);
513
514 /* Reset interval as defined in LinkDelaySyncIntervalSetting state
515 * machine.
516 */
517 if (port_ds->ptt_port_enabled && !port_ds->prev_ptt_port_enabled) {
518 gptp_update_sync_interval(port, GPTP_ITV_SET_TO_INIT);
519 }
520
521 if (state->rcvd_pss_sync && ((!port_ds->ptt_port_enabled) ||
522 !port_ds->as_capable)) {
523 state->rcvd_pss_sync = false;
524 state->state = GPTP_PSS_SEND_TRANSMIT_INIT;
525
526 return;
527 }
528
529 switch (state->state) {
530 case GPTP_PSS_SEND_TRANSMIT_INIT:
531 case GPTP_PSS_SEND_SYNC_RECEIPT_TIMEOUT:
532 if (state->rcvd_pss_sync &&
533 (state->pss_sync_ptr->local_port_number != port) &&
534 (global_ds->selected_role[port] == GPTP_PORT_MASTER)) {
535 state->state = GPTP_PSS_SEND_SEND_MD_SYNC;
536 } else {
537 break;
538 }
539
540 __fallthrough;
541 case GPTP_PSS_SEND_SEND_MD_SYNC:
542 if (state->rcvd_pss_sync) {
543 gptp_mi_pss_store_last_pss(port);
544 state->rcvd_pss_sync = false;
545 }
546
547 /* Make sure no previous timer is still running. */
548 k_timer_stop(&state->half_sync_itv_timer);
549 k_timer_stop(&state->send_sync_receipt_timeout_timer);
550
551 state->half_sync_itv_timer_expired = false;
552 state->sync_itv_timer_expired = false;
553 state->send_sync_receipt_timeout_timer_expired = false;
554
555 /* Convert ns to ms. */
556 duration = K_MSEC(gptp_uscaled_ns_to_timer_ms(
557 &port_ds->half_sync_itv));
558
559 /* Start 0.5 * syncInterval timeout timer. */
560 k_timer_start(&state->half_sync_itv_timer, duration,
561 K_NO_WAIT);
562
563 /* sourcePortIdentity is set to the portIdentity of this
564 * PTP Port (see ch. 10.2.12.2.1 and ch 8.5.2).
565 */
566 memcpy(&state->pss_sync_ptr->sync_info.src_port_id.clk_id,
567 GPTP_DEFAULT_DS()->clk_id,
568 GPTP_CLOCK_ID_LEN);
569 state->pss_sync_ptr->sync_info.src_port_id.port_number = port;
570
571
572 gptp_mi_pss_send_md_sync_send(port);
573
574 __fallthrough;
575 case GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT:
576 /* Test conditions have been slightly rearranged compared to
577 * their definitions in the standard in order not to test
578 * AsCapable and pttPortEnabled when not needed (they are
579 * already tested with rcvdPSSync for the reset of this state
580 * machine).
581 */
582 if ((global_ds->selected_role[port] == GPTP_PORT_MASTER) &&
583 ((state->rcvd_pss_sync &&
584 state->half_sync_itv_timer_expired &&
585 state->pss_sync_ptr->local_port_number != port) ||
586 (state->sync_itv_timer_expired &&
587 (state->last_rcvd_port_num != port) &&
588 port_ds->as_capable && port_ds->ptt_port_enabled))) {
589
590 state->state = GPTP_PSS_SEND_SEND_MD_SYNC;
591
592 } else if ((state->state == GPTP_PSS_SEND_SEND_MD_SYNC) ||
593 (state->rcvd_pss_sync &&
594 !state->sync_itv_timer_expired &&
595 (global_ds->selected_role[port] ==
596 GPTP_PORT_MASTER) &&
597 state->pss_sync_ptr->local_port_number != port)) {
598
599 /* Change state as it may have transitioned from
600 * SEND_MD_SYNC.
601 */
602 state->state = GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT;
603
604 /* Stop and (re)start receipt timeout timer. */
605 k_timer_stop(&state->send_sync_receipt_timeout_timer);
606 state->send_sync_receipt_timeout_timer_expired = false;
607
608 duration =
609 K_MSEC(port_ds->sync_receipt_timeout_time_itv /
610 (NSEC_PER_USEC * USEC_PER_MSEC));
611
612 k_timer_start(&state->send_sync_receipt_timeout_timer,
613 duration, K_NO_WAIT);
614
615 } else if (state->send_sync_receipt_timeout_timer_expired) {
616 state->state = GPTP_PSS_SEND_SYNC_RECEIPT_TIMEOUT;
617 }
618
619 break;
620 }
621 }
622
gptp_mi_site_ss_prepare_pss_send(void)623 static void gptp_mi_site_ss_prepare_pss_send(void)
624 {
625 struct gptp_site_sync_sync_state *state;
626
627 state = &GPTP_STATE()->site_ss;
628
629 memcpy(&state->pss_send, state->pss_rcv_ptr,
630 sizeof(struct gptp_mi_port_sync_sync));
631 }
632
gptp_mi_site_ss_send_to_pss(void)633 static void gptp_mi_site_ss_send_to_pss(void)
634 {
635 struct gptp_site_sync_sync_state *state;
636 struct gptp_pss_send_state *pss_send;
637 int port;
638
639 state = &GPTP_STATE()->site_ss;
640
641 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
642 pss_send = &GPTP_PORT_STATE(port)->pss_send;
643 pss_send->pss_sync_ptr = &state->pss_send;
644 pss_send->rcvd_pss_sync = true;
645 }
646 }
647
gptp_mi_site_sync_sync_state_machine(void)648 static void gptp_mi_site_sync_sync_state_machine(void)
649 {
650 bool gm_present;
651 uint16_t local_port_number;
652 struct gptp_site_sync_sync_state *state;
653 struct gptp_clk_slave_sync_state *clk_ss;
654
655 state = &GPTP_STATE()->site_ss;
656 clk_ss = &GPTP_STATE()->clk_slave_sync;
657 gm_present = GPTP_GLOBAL_DS()->gm_present;
658
659 if (!state->pss_rcv_ptr) {
660 /* We do not have connection to GM yet */
661 return;
662 }
663
664 local_port_number = state->pss_rcv_ptr->local_port_number;
665
666 switch (state->state) {
667 case GPTP_SSS_INITIALIZING:
668 state->rcvd_pss = false;
669 state->state = GPTP_SSS_RECEIVING_SYNC;
670 break;
671
672 case GPTP_SSS_RECEIVING_SYNC:
673 if (state->rcvd_pss) {
674 state->rcvd_pss = false;
675 if (gptp_is_slave_port(local_port_number) &&
676 gm_present) {
677 gptp_mi_site_ss_prepare_pss_send();
678
679 /*
680 * Send Port Sync Sync to all
681 * PortSyncSyncSend State Machines.
682 */
683 gptp_mi_site_ss_send_to_pss();
684
685 /*
686 * Send PortSyncSync to
687 * ClockSlaveSync State Machine.
688 */
689 clk_ss->pss_rcv_ptr = &state->pss_send;
690 clk_ss->rcvd_pss = true;
691 }
692 }
693
694 break;
695 }
696 }
697
gptp_mi_clk_slave_sync_compute(void)698 static void gptp_mi_clk_slave_sync_compute(void)
699 {
700 struct gptp_clk_slave_sync_state *state;
701 struct gptp_clk_master_sync_offset_state *offset_state;
702 struct gptp_global_ds *global_ds;
703 struct gptp_md_sync_info *pss;
704 struct gptp_port_ds *port_ds;
705 uint64_t sync_receipt_time;
706
707 state = &GPTP_STATE()->clk_slave_sync;
708 offset_state = &GPTP_STATE()->clk_master_sync_offset;
709 global_ds = GPTP_GLOBAL_DS();
710 port_ds = GPTP_PORT_DS(state->pss_rcv_ptr->local_port_number);
711
712 pss = &state->pss_rcv_ptr->sync_info;
713
714 sync_receipt_time = port_ds->neighbor_prop_delay;
715 sync_receipt_time *= pss->rate_ratio;
716 sync_receipt_time /= port_ds->neighbor_rate_ratio;
717 sync_receipt_time += pss->follow_up_correction_field;
718 sync_receipt_time += port_ds->delay_asymmetry;
719
720 global_ds->sync_receipt_time.second = sync_receipt_time / NSEC_PER_SEC;
721 global_ds->sync_receipt_time.fract_nsecond =
722 (sync_receipt_time % NSEC_PER_SEC) * GPTP_POW2_16;
723 global_ds->sync_receipt_time.second += pss->precise_orig_ts.second;
724 global_ds->sync_receipt_time.fract_nsecond +=
725 pss->precise_orig_ts.nanosecond * GPTP_POW2_16;
726
727 global_ds->sync_receipt_local_time = port_ds->delay_asymmetry;
728 global_ds->sync_receipt_local_time /= pss->rate_ratio;
729 global_ds->sync_receipt_local_time +=
730 (port_ds->neighbor_prop_delay / port_ds->neighbor_rate_ratio);
731 global_ds->sync_receipt_local_time += pss->upstream_tx_time;
732
733 global_ds->gm_time_base_indicator = pss->gm_time_base_indicator;
734 global_ds->last_gm_phase_change.high = pss->last_gm_phase_change.high;
735 global_ds->last_gm_phase_change.low = pss->last_gm_phase_change.low;
736 global_ds->last_gm_freq_change = pss->last_gm_freq_change;
737
738 offset_state->rcvd_sync_receipt_time = true;
739 }
740
741 #if defined(CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE)
gptp_update_local_port_clock(void)742 static void gptp_update_local_port_clock(void)
743 {
744 struct gptp_clk_slave_sync_state *state;
745 struct gptp_global_ds *global_ds;
746 struct gptp_port_ds *port_ds;
747 int port;
748 int64_t nanosecond_diff;
749 int64_t second_diff;
750 const struct device *clk;
751 struct net_ptp_time tm;
752 unsigned int key;
753
754 state = &GPTP_STATE()->clk_slave_sync;
755 global_ds = GPTP_GLOBAL_DS();
756 port = state->pss_rcv_ptr->local_port_number;
757 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
758
759 port_ds = GPTP_PORT_DS(port);
760
761 /* Check if the last neighbor rate ratio can still be used */
762 if (!port_ds->neighbor_rate_ratio_valid) {
763 return;
764 }
765
766 port_ds->neighbor_rate_ratio_valid = false;
767
768 second_diff = global_ds->sync_receipt_time.second -
769 (global_ds->sync_receipt_local_time / NSEC_PER_SEC);
770 nanosecond_diff =
771 (global_ds->sync_receipt_time.fract_nsecond / GPTP_POW2_16) -
772 (global_ds->sync_receipt_local_time % NSEC_PER_SEC);
773
774 clk = net_eth_get_ptp_clock(GPTP_PORT_IFACE(port));
775 if (!clk) {
776 return;
777 }
778
779 if (second_diff > 0 && nanosecond_diff < 0) {
780 second_diff--;
781 nanosecond_diff = NSEC_PER_SEC + nanosecond_diff;
782 }
783
784 if (second_diff < 0 && nanosecond_diff > 0) {
785 second_diff++;
786 nanosecond_diff = -(int64_t)NSEC_PER_SEC + nanosecond_diff;
787 }
788
789 ptp_clock_rate_adjust(clk, port_ds->neighbor_rate_ratio);
790
791 /* If time difference is too high, set the clock value.
792 * Otherwise, adjust it.
793 */
794 if (second_diff || (second_diff == 0 &&
795 (nanosecond_diff < -5000 ||
796 nanosecond_diff > 5000))) {
797 bool underflow = false;
798
799 key = irq_lock();
800 ptp_clock_get(clk, &tm);
801
802 if (second_diff < 0 && tm.second < -second_diff) {
803 NET_DBG("Do not set local clock because %lu < %ld",
804 (unsigned long int)tm.second,
805 (long int)-second_diff);
806 goto skip_clock_set;
807 }
808
809 tm.second += second_diff;
810
811 if (nanosecond_diff < 0 &&
812 tm.nanosecond < -nanosecond_diff) {
813 underflow = true;
814 }
815
816 tm.nanosecond += nanosecond_diff;
817
818 if (underflow) {
819 tm.second--;
820 tm.nanosecond += NSEC_PER_SEC;
821 } else if (tm.nanosecond >= NSEC_PER_SEC) {
822 tm.second++;
823 tm.nanosecond -= NSEC_PER_SEC;
824 }
825
826 /* This prints too much data normally but can be enabled to see
827 * what time we are setting to the local clock.
828 */
829 if (0) {
830 NET_INFO("Set local clock %lu.%lu",
831 (unsigned long int)tm.second,
832 (unsigned long int)tm.nanosecond);
833 }
834
835 ptp_clock_set(clk, &tm);
836
837 skip_clock_set:
838 irq_unlock(key);
839 } else {
840 if (nanosecond_diff < -200) {
841 nanosecond_diff = -200;
842 } else if (nanosecond_diff > 200) {
843 nanosecond_diff = 200;
844 }
845
846 ptp_clock_adjust(clk, nanosecond_diff);
847 }
848 }
849 #endif /* CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE */
850
gptp_mi_clk_slave_sync_state_machine(void)851 static void gptp_mi_clk_slave_sync_state_machine(void)
852 {
853 struct gptp_clk_slave_sync_state *state;
854
855 state = &GPTP_STATE()->clk_slave_sync;
856
857 switch (state->state) {
858 case GPTP_CLK_SLAVE_SYNC_INITIALIZING:
859 state->rcvd_pss = false;
860 state->state = GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND;
861 break;
862
863 case GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND:
864 if (state->rcvd_pss) {
865 state->rcvd_pss = false;
866 gptp_mi_clk_slave_sync_compute();
867
868 #if defined(CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE)
869 /* Instead of updating SlaveClock, update LocalClock */
870 gptp_update_local_port_clock();
871 #endif
872 gptp_call_phase_dis_cb();
873 }
874
875 break;
876 }
877 }
878
gptp_mi_clk_master_sync_offset_state_machine(void)879 static void gptp_mi_clk_master_sync_offset_state_machine(void)
880 {
881 struct gptp_clk_master_sync_offset_state *state;
882 struct gptp_global_ds *global_ds;
883
884 state = &GPTP_STATE()->clk_master_sync_offset;
885 global_ds = GPTP_GLOBAL_DS();
886
887 switch (state->state) {
888 case GPTP_CMS_OFFSET_INITIALIZING:
889 state->rcvd_sync_receipt_time = false;
890 state->state = GPTP_CMS_OFFSET_INDICATION;
891 break;
892 case GPTP_CMS_OFFSET_INDICATION:
893 if (!state->rcvd_sync_receipt_time) {
894 break;
895 }
896
897 state->rcvd_sync_receipt_time = false;
898
899 if (global_ds->selected_role[0] == GPTP_PORT_PASSIVE) {
900 /* TODO Calculate real values for proper BC support */
901 memset(&global_ds->clk_src_phase_offset, 0x0,
902 sizeof(struct gptp_scaled_ns));
903 global_ds->clk_src_freq_offset = 0;
904 } else if (global_ds->clk_src_time_base_indicator_prev
905 != global_ds->clk_src_time_base_indicator) {
906 memcpy(&global_ds->clk_src_phase_offset,
907 &global_ds->last_gm_phase_change,
908 sizeof(struct gptp_scaled_ns));
909
910 global_ds->clk_src_freq_offset =
911 global_ds->last_gm_freq_change;
912 }
913
914 break;
915 default:
916 NET_ERR("Unrecognised state %d", state->state);
917 break;
918 }
919 }
920
921 #if defined(CONFIG_NET_GPTP_GM_CAPABLE)
gptp_mi_setup_sync_send_time(void)922 static inline void gptp_mi_setup_sync_send_time(void)
923 {
924 struct gptp_clk_master_sync_snd_state *state;
925 struct gptp_global_ds *global_ds;
926 uint64_t time_helper;
927
928 state = &GPTP_STATE()->clk_master_sync_send;
929 global_ds = GPTP_GLOBAL_DS();
930
931 time_helper = state->sync_send_time.low;
932
933 state->sync_send_time.low +=
934 global_ds->clk_master_sync_itv;
935
936 /* Check for overflow */
937 if (state->sync_send_time.low < time_helper) {
938 state->sync_send_time.high += 1U;
939 state->sync_send_time.low =
940 UINT64_MAX - state->sync_send_time.low;
941 }
942 }
943
gptp_mi_set_ps_sync_cmss(void)944 static void gptp_mi_set_ps_sync_cmss(void)
945 {
946 struct gptp_clk_master_sync_snd_state *state;
947 struct gptp_global_ds *global_ds;
948 struct gptp_md_sync_info *sync_info;
949 uint64_t current_time;
950
951 global_ds = GPTP_GLOBAL_DS();
952 state = &GPTP_STATE()->clk_master_sync_send;
953
954 sync_info = &state->pss_snd.sync_info;
955
956 state->pss_snd.local_port_number = 0U;
957
958 current_time = gptp_get_current_master_time_nanosecond();
959
960 sync_info->precise_orig_ts.second = current_time / NSEC_PER_SEC;
961 sync_info->precise_orig_ts.nanosecond = current_time % NSEC_PER_SEC;
962
963 /* TODO calculate rate ratio and correction field properly.
964 * Whenever time aware system is the grand master clock, we currently
965 * make the following shortcuts:
966 * - assuming that clock source is the local clock,
967 * rate_ratio is set to 1.0 instead of being copied from global_ds.
968 * - considering that precise origin timestamp is directly inherited
969 * from sync egress timestamp in gptp_md_follow_up_prepare(),
970 * follow_up_correction_field is set to 0.
971 */
972 sync_info->follow_up_correction_field = 0;
973 sync_info->rate_ratio = 1.0;
974
975 memcpy(&sync_info->src_port_id.clk_id,
976 GPTP_DEFAULT_DS()->clk_id,
977 GPTP_CLOCK_ID_LEN);
978
979 sync_info->src_port_id.port_number = 0U;
980 sync_info->log_msg_interval = CONFIG_NET_GPTP_INIT_LOG_SYNC_ITV;
981 sync_info->upstream_tx_time = global_ds->local_time.low;
982
983 state->pss_snd.sync_receipt_timeout_time = UINT64_MAX;
984
985 sync_info->gm_time_base_indicator =
986 global_ds->clk_src_time_base_indicator;
987
988 memcpy(&sync_info->last_gm_phase_change,
989 &global_ds->clk_src_phase_offset,
990 sizeof(struct gptp_scaled_ns));
991
992 sync_info->last_gm_freq_change = global_ds->clk_src_freq_offset;
993 }
994
gptp_mi_tx_ps_sync_cmss(void)995 static inline void gptp_mi_tx_ps_sync_cmss(void)
996 {
997 struct gptp_clk_master_sync_snd_state *state;
998 struct gptp_pss_send_state *pss_send;
999 int port;
1000
1001 state = &GPTP_STATE()->clk_master_sync_send;
1002
1003 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1004 pss_send = &GPTP_PORT_STATE(port)->pss_send;
1005 pss_send->pss_sync_ptr = &state->pss_snd;
1006
1007 pss_send->rcvd_pss_sync = true;
1008 }
1009 }
1010
gptp_mi_clk_master_sync_snd_state_machine(void)1011 static void gptp_mi_clk_master_sync_snd_state_machine(void)
1012 {
1013 struct gptp_clk_master_sync_snd_state *state;
1014 uint64_t current_time;
1015
1016 state = &GPTP_STATE()->clk_master_sync_send;
1017
1018 switch (state->state) {
1019 case GPTP_CMS_SND_INITIALIZING:
1020 gptp_mi_setup_sync_send_time();
1021
1022 state->state = GPTP_CMS_SND_INDICATION;
1023 break;
1024
1025 case GPTP_CMS_SND_INDICATION:
1026 current_time = gptp_get_current_master_time_nanosecond();
1027
1028 if (current_time >= state->sync_send_time.low) {
1029 gptp_mi_set_ps_sync_cmss();
1030 gptp_mi_tx_ps_sync_cmss();
1031
1032 gptp_mi_setup_sync_send_time();
1033 }
1034
1035 break;
1036
1037 default:
1038 NET_ERR("Unrecognised state %d", state->state);
1039 break;
1040 }
1041 }
1042 #endif
1043
gptp_compute_gm_rate_ratio(void)1044 static void gptp_compute_gm_rate_ratio(void)
1045 {
1046 static struct net_ptp_extended_time src_time_0;
1047 static struct gptp_uscaled_ns local_time_0;
1048 struct net_ptp_extended_time src_time_n;
1049 struct gptp_uscaled_ns local_time_n;
1050 struct net_ptp_extended_time src_time_t;
1051 struct gptp_uscaled_ns local_time_t;
1052 struct gptp_clk_master_sync_rcv_state *state;
1053 struct gptp_global_ds *global_ds;
1054 double new_gm_rate;
1055
1056 state = &GPTP_STATE()->clk_master_sync_receive;
1057 global_ds = GPTP_GLOBAL_DS();
1058
1059 /* Get current local and source time */
1060 memcpy(&src_time_n, &state->rcvd_clk_src_req.src_time,
1061 sizeof(struct net_ptp_extended_time));
1062
1063 memcpy(&local_time_n, &global_ds->local_time,
1064 sizeof(struct gptp_uscaled_ns));
1065
1066 if ((src_time_0.second == 0U && src_time_0.fract_nsecond == 0U)
1067 || (local_time_0.high == 0U && local_time_0.low == 0U)) {
1068 memcpy(&src_time_0, &src_time_n,
1069 sizeof(struct net_ptp_extended_time));
1070
1071 memcpy(&local_time_0, &local_time_n,
1072 sizeof(struct gptp_uscaled_ns));
1073
1074 global_ds->gm_rate_ratio = 1.0;
1075
1076 return;
1077 }
1078
1079 /* Take care of the sign of the result */
1080 new_gm_rate = 1.0;
1081
1082 if ((src_time_n.second < src_time_0.second)
1083 || (src_time_n.second == src_time_0.second
1084 && src_time_n.fract_nsecond < src_time_0.fract_nsecond)) {
1085 /* Change result sign and swap src_time_n and src_time_0 */
1086 memcpy(&src_time_t, &src_time_n,
1087 sizeof(struct net_ptp_extended_time));
1088 memcpy(&src_time_n, &src_time_0,
1089 sizeof(struct net_ptp_extended_time));
1090 memcpy(&src_time_0, &src_time_t,
1091 sizeof(struct net_ptp_extended_time));
1092
1093 new_gm_rate *= -1;
1094 }
1095
1096 if ((local_time_n.high < local_time_0.high)
1097 || (local_time_n.high == local_time_0.high
1098 && local_time_n.low < local_time_0.low)) {
1099 /* Change result sign and swap local_time_n and local_time_0 */
1100 memcpy(&local_time_t, &local_time_n,
1101 sizeof(struct gptp_uscaled_ns));
1102 memcpy(&local_time_n, &local_time_0,
1103 sizeof(struct gptp_uscaled_ns));
1104 memcpy(&local_time_0, &local_time_t,
1105 sizeof(struct gptp_uscaled_ns));
1106
1107 new_gm_rate *= -1;
1108 }
1109
1110 /* At this point src_time_n >= src_time_0 */
1111 src_time_n.second -= src_time_0.second;
1112
1113 if (src_time_n.fract_nsecond >= src_time_0.fract_nsecond) {
1114 src_time_n.fract_nsecond -= src_time_0.fract_nsecond;
1115 } else {
1116 src_time_n.second -= 1U;
1117 src_time_n.fract_nsecond = (NSEC_PER_SEC * GPTP_POW2_16)
1118 - src_time_0.fract_nsecond;
1119 }
1120
1121 /* At this point local_time_n >= local_time_0 */
1122 local_time_n.high -= local_time_0.high;
1123
1124 if (local_time_n.low >= local_time_0.low) {
1125 local_time_n.low -= local_time_0.low;
1126 } else {
1127 local_time_n.high -= 1U;
1128 local_time_n.low = UINT64_MAX - local_time_0.low;
1129 }
1130
1131 /* Calculate it in nanoseconds, new_gm_rate is either 1 or -1 here */
1132 new_gm_rate *= ((src_time_n.second * NSEC_PER_SEC)
1133 + (src_time_n.fract_nsecond / GPTP_POW2_16));
1134
1135 new_gm_rate /= local_time_n.low;
1136
1137 global_ds->gm_rate_ratio = new_gm_rate;
1138 }
1139
gptp_mi_clk_master_sync_rcv_state_machine(void)1140 static void gptp_mi_clk_master_sync_rcv_state_machine(void)
1141 {
1142 struct gptp_clk_master_sync_rcv_state *s;
1143 struct gptp_global_ds *global_ds;
1144
1145 #ifdef CONFIG_NET_GPTP_PROBE_CLOCK_SOURCE_ON_DEMAND
1146 struct gptp_clk_src_time_invoke_params invoke_args = {};
1147 uint64_t cur = gptp_get_current_master_time_nanosecond();
1148
1149 invoke_args.src_time.second = cur / NSEC_PER_SEC;
1150 cur -= (invoke_args.src_time.second * NSEC_PER_SEC);
1151
1152 invoke_args.src_time.fract_nsecond = cur * GPTP_POW2_16;
1153
1154 memset(&invoke_args.last_gm_phase_change, 0x0,
1155 sizeof(struct gptp_scaled_ns));
1156 invoke_args.last_gm_freq_change = 0;
1157
1158 gptp_clk_src_time_invoke(&invoke_args);
1159 #endif
1160
1161 global_ds = GPTP_GLOBAL_DS();
1162
1163 s = &GPTP_STATE()->clk_master_sync_receive;
1164 switch (s->state) {
1165 case GPTP_CMS_RCV_INITIALIZING:
1166 s->state = GPTP_CMS_RCV_WAITING;
1167 break;
1168
1169 case GPTP_CMS_RCV_WAITING:
1170 if (s->rcvd_clock_source_req || s->rcvd_local_clock_tick) {
1171 s->state = GPTP_CMS_RCV_SOURCE_TIME;
1172 }
1173
1174 break;
1175
1176 case GPTP_CMS_RCV_SOURCE_TIME:
1177 global_ds->local_time.high = 0U;
1178 global_ds->local_time.low =
1179 gptp_get_current_master_time_nanosecond();
1180
1181 if (s->rcvd_clock_source_req) {
1182 gptp_compute_gm_rate_ratio();
1183
1184 global_ds->clk_src_time_base_indicator_prev =
1185 global_ds->clk_src_time_base_indicator;
1186
1187 global_ds->clk_src_time_base_indicator =
1188 s->rcvd_clk_src_req.time_base_indicator;
1189
1190 memcpy(&global_ds->clk_src_last_gm_phase_change,
1191 &s->rcvd_clk_src_req.last_gm_phase_change,
1192 sizeof(struct gptp_scaled_ns));
1193
1194 global_ds->clk_src_last_gm_freq_change =
1195 s->rcvd_clk_src_req.last_gm_freq_change;
1196 }
1197
1198 s->rcvd_clock_source_req = false;
1199 s->rcvd_local_clock_tick = false;
1200 s->state = GPTP_CMS_RCV_WAITING;
1201 break;
1202
1203 default:
1204 NET_ERR("Unrecognised state %d", s->state);
1205 break;
1206 }
1207 }
1208
copy_path_trace(struct gptp_announce * announce)1209 static void copy_path_trace(struct gptp_announce *announce)
1210 {
1211 int len = ntohs(announce->tlv.len);
1212 struct gptp_path_trace *sys_path_trace;
1213
1214 if (len > GPTP_MAX_PATHTRACE_SIZE) {
1215 NET_ERR("Too long path trace (%d vs %d)",
1216 GPTP_MAX_PATHTRACE_SIZE, len);
1217 return;
1218 }
1219
1220 sys_path_trace = &GPTP_GLOBAL_DS()->path_trace;
1221
1222 sys_path_trace->len = htons(len + GPTP_CLOCK_ID_LEN);
1223
1224 memcpy(sys_path_trace->path_sequence, announce->tlv.path_sequence,
1225 len);
1226
1227 /* Append local clockIdentity. */
1228 memcpy((uint8_t *)sys_path_trace->path_sequence + len,
1229 GPTP_DEFAULT_DS()->clk_id, GPTP_CLOCK_ID_LEN);
1230 }
1231
gptp_mi_qualify_announce(int port,struct net_pkt * announce_msg)1232 static bool gptp_mi_qualify_announce(int port, struct net_pkt *announce_msg)
1233 {
1234 struct gptp_announce *announce;
1235 struct gptp_hdr *hdr;
1236 int i;
1237 uint16_t len;
1238
1239 hdr = GPTP_HDR(announce_msg);
1240 announce = GPTP_ANNOUNCE(announce_msg);
1241
1242 if (memcmp(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id,
1243 GPTP_CLOCK_ID_LEN) == 0) {
1244 return false;
1245 }
1246
1247 len = ntohs(announce->steps_removed);
1248 if (len >= 255U) {
1249 return false;
1250 }
1251
1252 for (i = 0; i < len + 1; i++) {
1253 if (memcmp(announce->tlv.path_sequence[i],
1254 GPTP_DEFAULT_DS()->clk_id,
1255 GPTP_CLOCK_ID_LEN) == 0) {
1256 return false;
1257 }
1258 }
1259
1260 if (GPTP_GLOBAL_DS()->selected_role[port] == GPTP_PORT_SLAVE) {
1261 copy_path_trace(announce);
1262 }
1263
1264 return true;
1265 }
1266
gptp_mi_port_announce_receive_state_machine(int port)1267 static void gptp_mi_port_announce_receive_state_machine(int port)
1268 {
1269 struct gptp_port_ds *port_ds;
1270 struct gptp_port_announce_receive_state *state;
1271 struct gptp_port_bmca_data *bmca_data;
1272
1273 state = &GPTP_PORT_STATE(port)->pa_rcv;
1274 port_ds = GPTP_PORT_DS(port);
1275 bmca_data = GPTP_PORT_BMCA_DATA(port);
1276
1277 if ((!port_ds->ptt_port_enabled) || (!port_ds->as_capable)) {
1278 state->state = GPTP_PA_RCV_DISCARD;
1279 }
1280
1281 switch (state->state) {
1282 case GPTP_PA_RCV_DISCARD:
1283 state->rcvd_announce = false;
1284 bmca_data->rcvd_msg = false;
1285 if (bmca_data->rcvd_announce_ptr != NULL) {
1286 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1287 bmca_data->rcvd_announce_ptr = NULL;
1288 }
1289
1290 state->state = GPTP_PA_RCV_RECEIVE;
1291 break;
1292
1293 case GPTP_PA_RCV_RECEIVE:
1294 /* "portEnabled" is not checked: the interface is always up. */
1295 if (state->rcvd_announce &&
1296 port_ds->ptt_port_enabled &&
1297 port_ds->as_capable &&
1298 !bmca_data->rcvd_msg) {
1299 state->rcvd_announce = false;
1300
1301 bmca_data->rcvd_msg = gptp_mi_qualify_announce(
1302 port, bmca_data->rcvd_announce_ptr);
1303 if (!bmca_data->rcvd_msg) {
1304 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1305 bmca_data->rcvd_announce_ptr = NULL;
1306 }
1307 }
1308
1309 break;
1310 }
1311 }
1312
1313 /*
1314 * Compare a vector to an announce message vector.
1315 * All must be in big endian (network) order.
1316 */
compare_priority_vectors(struct gptp_priority_vector * vector,struct net_pkt * pkt,int port)1317 static enum gptp_received_info compare_priority_vectors(
1318 struct gptp_priority_vector *vector,
1319 struct net_pkt *pkt, int port)
1320 {
1321 struct gptp_hdr *hdr;
1322 struct gptp_announce *announce;
1323 int rsi_cmp, spi_cmp, port_cmp;
1324
1325 hdr = GPTP_HDR(pkt);
1326 announce = GPTP_ANNOUNCE(pkt);
1327
1328 /* Compare rootSystemIdentity and stepsRemoved. */
1329 rsi_cmp = memcmp(&announce->root_system_id,
1330 &vector->root_system_id,
1331 sizeof(struct gptp_root_system_identity) +
1332 sizeof(uint16_t));
1333 if (rsi_cmp < 0) {
1334 /* Better rootSystemIdentity. */
1335 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1336 }
1337
1338 /* Compare sourcePortIdentity. */
1339 spi_cmp = memcmp(&hdr->port_id, &vector->src_port_id,
1340 sizeof(struct gptp_port_identity));
1341
1342 port_cmp = (int)port - ntohs(vector->port_number);
1343
1344 if (spi_cmp == 0) {
1345 if (rsi_cmp == 0) {
1346 if (port_cmp == 0) {
1347 /* Same priority vector. */
1348 return GPTP_RCVD_INFO_REPEATED_MASTER_INFO;
1349 } else if (port_cmp < 0) {
1350 /* Priority vector with better reception port
1351 * number.
1352 */
1353 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1354 }
1355 } else {
1356 /* Same master port but different Grand Master. */
1357 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1358 }
1359 } else if ((spi_cmp < 0) && (rsi_cmp == 0)) {
1360 /* Same Grand Master but better masterPort. */
1361 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1362 }
1363
1364 return GPTP_RCVD_INFO_INFERIOR_MASTER_INFO;
1365 }
1366
rcv_info(int port)1367 static enum gptp_received_info rcv_info(int port)
1368 {
1369 /* TODO
1370 * How can we define that a message does not convey the port
1371 * role Master port ?
1372 * It is needed to define that to be able to send
1373 * GPTP_RCVD_INFO_OTHER_INFO.
1374 */
1375 struct gptp_port_bmca_data *bmca_data;
1376 struct gptp_announce *announce;
1377
1378 bmca_data = GPTP_PORT_BMCA_DATA(port);
1379 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1380
1381 bmca_data->message_steps_removed = announce->steps_removed;
1382
1383 return compare_priority_vectors(&bmca_data->port_priority,
1384 bmca_data->rcvd_announce_ptr,
1385 port);
1386 }
1387
record_other_announce_info(int port)1388 static void record_other_announce_info(int port)
1389 {
1390 struct gptp_hdr *hdr;
1391 struct gptp_announce *announce;
1392 struct gptp_port_bmca_data *bmca_data;
1393
1394 bmca_data = GPTP_PORT_BMCA_DATA(port);
1395 hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr);
1396 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1397
1398 /* Copy leap61, leap59, current UTC offset valid, time traceable and
1399 * frequency traceable flags.
1400 */
1401 bmca_data->ann_flags.octets[1] = hdr->flags.octets[1];
1402
1403 bmca_data->ann_current_utc_offset = ntohs(announce->cur_utc_offset);
1404 bmca_data->ann_time_source = announce->time_source;
1405 }
1406
copy_priority_vector(struct gptp_priority_vector * vector,struct net_pkt * pkt,int port)1407 static void copy_priority_vector(struct gptp_priority_vector *vector,
1408 struct net_pkt *pkt, int port)
1409 {
1410 struct gptp_hdr *hdr;
1411 struct gptp_announce *announce;
1412
1413 hdr = GPTP_HDR(pkt);
1414 announce = GPTP_ANNOUNCE(pkt);
1415
1416 memcpy(&vector->root_system_id, &announce->root_system_id,
1417 sizeof(struct gptp_root_system_identity) + sizeof(uint16_t));
1418
1419 memcpy(&vector->src_port_id, &hdr->port_id,
1420 sizeof(struct gptp_port_identity));
1421
1422 vector->port_number = htons(port);
1423 }
1424
gptp_mi_port_announce_information_state_machine(int port)1425 static void gptp_mi_port_announce_information_state_machine(int port)
1426 {
1427 struct gptp_port_ds *port_ds;
1428 struct gptp_global_ds *global_ds;
1429 struct gptp_port_announce_information_state *state;
1430 struct gptp_announce *announce;
1431 struct gptp_hdr *hdr;
1432 struct gptp_port_bmca_data *bmca_data;
1433 struct gptp_pss_rcv_state *pss_rcv;
1434
1435 bmca_data = GPTP_PORT_BMCA_DATA(port);
1436 state = &GPTP_PORT_STATE(port)->pa_info;
1437 port_ds = GPTP_PORT_DS(port);
1438 global_ds = GPTP_GLOBAL_DS();
1439
1440 if ((!port_ds->ptt_port_enabled || !port_ds->as_capable) &&
1441 (bmca_data->info_is != GPTP_INFO_IS_DISABLED)) {
1442 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_DISABLED);
1443 }
1444
1445 switch (state->state) {
1446 case GPTP_PA_INFO_DISABLED:
1447 bmca_data->rcvd_msg = false;
1448 bmca_data->info_is = GPTP_INFO_IS_DISABLED;
1449 SET_RESELECT(global_ds, port);
1450 CLEAR_SELECTED(global_ds, port);
1451 gptp_change_pa_info_state(port, state,
1452 GPTP_PA_INFO_POST_DISABLED);
1453 k_timer_stop(&state->ann_rcpt_expiry_timer);
1454 state->ann_expired = true;
1455 __fallthrough;
1456
1457 case GPTP_PA_INFO_POST_DISABLED:
1458 if (port_ds->ptt_port_enabled && port_ds->as_capable) {
1459 gptp_change_pa_info_state(port, state,
1460 GPTP_PA_INFO_AGED);
1461 } else if (bmca_data->rcvd_msg) {
1462 gptp_change_pa_info_state(port, state,
1463 GPTP_PA_INFO_DISABLED);
1464 }
1465
1466 break;
1467
1468 case GPTP_PA_INFO_AGED:
1469 bmca_data->info_is = GPTP_INFO_IS_AGED;
1470 CLEAR_SELECTED(global_ds, port);
1471 SET_RESELECT(global_ds, port);
1472 /* Transition will be actually tested in UPDATE state. */
1473 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_UPDATE);
1474 break;
1475
1476 case GPTP_PA_INFO_UPDATE:
1477 if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) {
1478 memcpy(&bmca_data->port_priority,
1479 &bmca_data->master_priority,
1480 sizeof(struct gptp_priority_vector));
1481
1482 bmca_data->port_steps_removed =
1483 global_ds->master_steps_removed;
1484 bmca_data->updt_info = false;
1485 bmca_data->info_is = GPTP_INFO_IS_MINE;
1486 bmca_data->new_info = true;
1487 gptp_change_pa_info_state(port, state,
1488 GPTP_PA_INFO_CURRENT);
1489 }
1490
1491 break;
1492
1493 case GPTP_PA_INFO_CURRENT:
1494 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
1495 if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) {
1496 gptp_change_pa_info_state(port, state,
1497 GPTP_PA_INFO_UPDATE);
1498 } else if (bmca_data->rcvd_msg && !bmca_data->updt_info) {
1499 gptp_change_pa_info_state(port, state,
1500 GPTP_PA_INFO_RECEIVE);
1501 } else if ((bmca_data->info_is == GPTP_INFO_IS_RECEIVED) &&
1502 !bmca_data->updt_info &&
1503 !bmca_data->rcvd_msg &&
1504 (state->ann_expired ||
1505 (global_ds->gm_present &&
1506 pss_rcv->rcv_sync_receipt_timeout_timer_expired))) {
1507 gptp_change_pa_info_state(port, state,
1508 GPTP_PA_INFO_AGED);
1509 }
1510
1511 break;
1512
1513 case GPTP_PA_INFO_RECEIVE:
1514 switch (rcv_info(port)) {
1515 case GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO:
1516 gptp_change_pa_info_state(port, state,
1517 GPTP_PA_INFO_SUPERIOR_MASTER_PORT);
1518 break;
1519 case GPTP_RCVD_INFO_REPEATED_MASTER_INFO:
1520 gptp_change_pa_info_state(port, state,
1521 GPTP_PA_INFO_REPEATED_MASTER_PORT);
1522 break;
1523 case GPTP_RCVD_INFO_INFERIOR_MASTER_INFO:
1524 __fallthrough;
1525 case GPTP_RCVD_INFO_OTHER_INFO:
1526 gptp_change_pa_info_state(port, state,
1527 GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT);
1528 break;
1529 }
1530
1531 break;
1532
1533 case GPTP_PA_INFO_SUPERIOR_MASTER_PORT:
1534 /* We copy directly the content of the message to the port
1535 * priority vector without using an intermediate
1536 * messagePriority structure.
1537 */
1538
1539 if (!bmca_data->rcvd_announce_ptr) {
1540 /* Shouldn't be reached. Checked for safety reason. */
1541 bmca_data->rcvd_msg = false;
1542 gptp_change_pa_info_state(port, state,
1543 GPTP_PA_INFO_CURRENT);
1544 break;
1545 }
1546
1547 copy_priority_vector(&bmca_data->port_priority,
1548 bmca_data->rcvd_announce_ptr, port);
1549
1550 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1551 bmca_data->port_steps_removed = ntohs(announce->steps_removed);
1552 record_other_announce_info(port);
1553 hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr);
1554 gptp_set_time_itv(&bmca_data->ann_rcpt_timeout_time_interval,
1555 port_ds->announce_receipt_timeout,
1556 hdr->log_msg_interval);
1557 bmca_data->info_is = GPTP_INFO_IS_RECEIVED;
1558 CLEAR_SELECTED(global_ds, port);
1559 SET_RESELECT(global_ds, port);
1560 __fallthrough;
1561
1562 case GPTP_PA_INFO_REPEATED_MASTER_PORT:
1563 k_timer_stop(&state->ann_rcpt_expiry_timer);
1564 state->ann_expired = false;
1565 k_timer_start(&state->ann_rcpt_expiry_timer,
1566 K_MSEC(gptp_uscaled_ns_to_timer_ms(
1567 &bmca_data->ann_rcpt_timeout_time_interval)),
1568 K_NO_WAIT);
1569 __fallthrough;
1570
1571 case GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT:
1572 if (bmca_data->rcvd_announce_ptr != NULL) {
1573 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1574 bmca_data->rcvd_announce_ptr = NULL;
1575 }
1576
1577 bmca_data->rcvd_msg = false;
1578 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_CURRENT);
1579 break;
1580 }
1581 }
1582
gptp_updt_role_disabled_tree(void)1583 static void gptp_updt_role_disabled_tree(void)
1584 {
1585 struct gptp_global_ds *global_ds;
1586 int port;
1587
1588 global_ds = GPTP_GLOBAL_DS();
1589
1590 /* Set all elements of the selectedRole array to DisabledPort. */
1591 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1592 gptp_change_port_state(port, GPTP_PORT_DISABLED);
1593 }
1594
1595 /* Set lastGmPriority to all ones. */
1596 (void)memset(&global_ds->last_gm_priority, 0xFF,
1597 sizeof(struct gptp_priority_vector));
1598
1599 /* Set pathTrace array to contain the single element thisClock. */
1600 global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN);
1601 memcpy(global_ds->path_trace.path_sequence, GPTP_DEFAULT_DS()->clk_id,
1602 GPTP_CLOCK_ID_LEN);
1603 }
1604
gptp_clear_reselect_tree(void)1605 static void gptp_clear_reselect_tree(void)
1606 {
1607 /* Set all the elements of the reselect array to FALSE. */
1608 GPTP_GLOBAL_DS()->reselect_array = 0;
1609 }
1610
compute_best_vector(void)1611 static int compute_best_vector(void)
1612 {
1613 struct gptp_priority_vector *gm_prio;
1614 struct gptp_default_ds *default_ds;
1615 struct gptp_global_ds *global_ds;
1616 struct gptp_priority_vector *best_vector, *challenger;
1617 int best_port, port, tmp;
1618 struct gptp_pss_rcv_state *pss_rcv;
1619 struct gptp_port_announce_information_state *pa_info_state;
1620
1621 default_ds = GPTP_DEFAULT_DS();
1622 global_ds = GPTP_GLOBAL_DS();
1623 best_port = 0;
1624 gm_prio = &global_ds->gm_priority;
1625
1626 /* Write systemPriority into grandmaster. */
1627 (void)memset(gm_prio, 0, sizeof(struct gptp_priority_vector));
1628 gm_prio->root_system_id.grand_master_prio1 = default_ds->priority1;
1629 gm_prio->root_system_id.grand_master_prio2 = default_ds->priority2;
1630 gm_prio->root_system_id.clk_quality.clock_class =
1631 default_ds->clk_quality.clock_class;
1632 gm_prio->root_system_id.clk_quality.clock_accuracy =
1633 default_ds->clk_quality.clock_accuracy;
1634 gm_prio->root_system_id.clk_quality.offset_scaled_log_var =
1635 htons(default_ds->clk_quality.offset_scaled_log_var);
1636
1637 memcpy(gm_prio->src_port_id.clk_id, default_ds->clk_id,
1638 GPTP_CLOCK_ID_LEN);
1639 memcpy(gm_prio->root_system_id.grand_master_id, default_ds->clk_id,
1640 GPTP_CLOCK_ID_LEN);
1641
1642 best_vector = gm_prio;
1643
1644 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1645 challenger = &GPTP_PORT_BMCA_DATA(port)->port_priority;
1646 pa_info_state = &GPTP_PORT_STATE(port)->pa_info;
1647 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
1648
1649 if (pa_info_state->ann_expired ||
1650 (global_ds->gm_present &&
1651 pss_rcv->rcv_sync_receipt_timeout_timer_expired)) {
1652 continue;
1653 }
1654
1655 if (memcmp(challenger->src_port_id.clk_id, default_ds->clk_id,
1656 GPTP_CLOCK_ID_LEN) == 0) {
1657 /* Discard this challenger. */
1658 continue;
1659 }
1660
1661 if (best_port == 0) {
1662 tmp = memcmp(&challenger->root_system_id,
1663 &best_vector->root_system_id,
1664 sizeof(struct gptp_root_system_identity));
1665 if (tmp < 0) {
1666 best_vector = challenger;
1667 best_port = port;
1668 } else if (tmp > 0) {
1669 continue;
1670 }
1671
1672 tmp = (int)challenger->steps_removed -
1673 ((int)ntohs(best_vector->steps_removed) + 1);
1674 if (tmp < 0) {
1675 best_vector = challenger;
1676 best_port = port;
1677 } else if (tmp > 0) {
1678 continue;
1679 }
1680
1681 tmp = memcmp(&challenger->src_port_id,
1682 &best_vector->src_port_id,
1683 sizeof(struct gptp_port_identity));
1684 if (tmp < 0) {
1685 best_vector = challenger;
1686 best_port = port;
1687 } else if (tmp > 0) {
1688 continue;
1689 }
1690
1691 if (ntohs(challenger->port_number) <
1692 ntohs(best_vector->port_number)) {
1693 best_vector = challenger;
1694 best_port = port;
1695 }
1696
1697 } else {
1698 /* We can compare portPriority vectors without
1699 * calculating pathPriority vectors.
1700 */
1701 if (memcmp(challenger, best_vector,
1702 sizeof(struct gptp_priority_vector)) < 0) {
1703 best_vector = challenger;
1704 best_port = port;
1705 }
1706 }
1707 }
1708
1709 if (best_port != 0) {
1710 if (&global_ds->gm_priority.root_system_id !=
1711 &best_vector->root_system_id) {
1712 memcpy(&global_ds->gm_priority.root_system_id,
1713 &best_vector->root_system_id,
1714 sizeof(struct gptp_root_system_identity));
1715 }
1716
1717 global_ds->gm_priority.steps_removed =
1718 ntohs(best_vector->steps_removed) + 1;
1719
1720 if (&global_ds->gm_priority.src_port_id !=
1721 &best_vector->src_port_id) {
1722 memcpy(&global_ds->gm_priority.src_port_id,
1723 &best_vector->src_port_id,
1724 sizeof(struct gptp_port_identity));
1725 }
1726
1727 global_ds->gm_priority.port_number = best_vector->port_number;
1728 }
1729
1730 return best_port;
1731 }
1732
update_bmca(int port,int best_port,struct gptp_global_ds * global_ds,struct gptp_default_ds * default_ds,struct gptp_priority_vector * gm_prio)1733 static void update_bmca(int port,
1734 int best_port,
1735 struct gptp_global_ds *global_ds,
1736 struct gptp_default_ds *default_ds,
1737 struct gptp_priority_vector *gm_prio)
1738 {
1739 struct gptp_port_bmca_data *bmca_data = GPTP_PORT_BMCA_DATA(port);
1740
1741 /* Update masterPriorityVector for the port. */
1742 if (best_port == 0) {
1743 memcpy(&bmca_data->master_priority, gm_prio,
1744 sizeof(struct gptp_priority_vector));
1745
1746 bmca_data->master_priority.port_number = htons(port);
1747 bmca_data->master_priority.src_port_id.port_number =
1748 htons(port);
1749 } else {
1750 memcpy(&bmca_data->master_priority.root_system_id,
1751 &gm_prio->root_system_id,
1752 sizeof(struct gptp_root_system_identity));
1753 memcpy(bmca_data->master_priority.src_port_id.clk_id,
1754 default_ds->clk_id, GPTP_CLOCK_ID_LEN);
1755 bmca_data->master_priority.port_number = htons(port);
1756 bmca_data->master_priority.src_port_id.port_number =
1757 htons(port);
1758 }
1759
1760 switch (bmca_data->info_is) {
1761 case GPTP_INFO_IS_DISABLED:
1762 gptp_change_port_state(port, GPTP_PORT_DISABLED);
1763 break;
1764
1765 case GPTP_INFO_IS_AGED:
1766 bmca_data->updt_info = true;
1767 gptp_change_port_state(port, GPTP_PORT_MASTER);
1768 break;
1769
1770 case GPTP_INFO_IS_MINE:
1771 gptp_change_port_state(port, GPTP_PORT_MASTER);
1772
1773 if ((memcmp(&bmca_data->port_priority,
1774 &bmca_data->master_priority,
1775 sizeof(struct gptp_priority_vector)) != 0) ||
1776 (bmca_data->port_steps_removed !=
1777 global_ds->master_steps_removed)) {
1778 bmca_data->updt_info = true;
1779 }
1780
1781 break;
1782
1783 case GPTP_INFO_IS_RECEIVED:
1784 if (best_port == port) {
1785 /* gmPriorityVector is now derived from
1786 * portPriorityVector.
1787 */
1788 gptp_change_port_state(port, GPTP_PORT_SLAVE);
1789 bmca_data->updt_info = false;
1790 } else if (memcmp(&bmca_data->port_priority,
1791 &bmca_data->master_priority,
1792 sizeof(struct gptp_priority_vector)) <= 0) {
1793 /* The masterPriorityVector is not better than
1794 * the portPriorityVector.
1795 */
1796 gptp_change_port_state(port, GPTP_PORT_PASSIVE);
1797
1798 if (memcmp(bmca_data->port_priority.src_port_id.clk_id,
1799 default_ds->clk_id,
1800 GPTP_CLOCK_ID_LEN)) {
1801 /* The sourcePortIdentity component of
1802 * the portPriorityVector does not
1803 * reflect another port on the
1804 * time-aware system.
1805 */
1806 bmca_data->updt_info = true;
1807 } else {
1808 bmca_data->updt_info = false;
1809 }
1810 } else {
1811 gptp_change_port_state(port, GPTP_PORT_MASTER);
1812 bmca_data->updt_info = true;
1813 }
1814
1815 break;
1816 }
1817 }
1818
gptp_updt_roles_tree(void)1819 static void gptp_updt_roles_tree(void)
1820 {
1821 struct gptp_global_ds *global_ds;
1822 struct gptp_default_ds *default_ds;
1823 struct gptp_priority_vector *gm_prio, *last_gm_prio;
1824 struct gptp_port_bmca_data *bmca_data;
1825 int port, best_port;
1826
1827 global_ds = GPTP_GLOBAL_DS();
1828 default_ds = GPTP_DEFAULT_DS();
1829
1830 gm_prio = &global_ds->gm_priority;
1831 last_gm_prio = &global_ds->last_gm_priority;
1832
1833 /* Save gmPriority. */
1834 memcpy(last_gm_prio, gm_prio, sizeof(struct gptp_priority_vector));
1835
1836 best_port = compute_best_vector();
1837
1838 /* If the best vector was the systemPriorityVector. */
1839 if (best_port == 0) {
1840 /* Copy leap61, leap59, current UTC offset valid,
1841 * time traceable and frequency traceable flags.
1842 */
1843 global_ds->global_flags.octets[1] =
1844 global_ds->sys_flags.octets[1];
1845 global_ds->current_utc_offset =
1846 global_ds->sys_current_utc_offset;
1847 global_ds->time_source = global_ds->sys_time_source;
1848 global_ds->master_steps_removed = 0U;
1849 } else {
1850 bmca_data = GPTP_PORT_BMCA_DATA(best_port);
1851
1852 /* Copy leap61, leap59, current UTC offset valid,
1853 * time traceable and frequency traceable flags.
1854 */
1855 global_ds->global_flags.octets[1] =
1856 bmca_data->ann_flags.octets[1];
1857 global_ds->current_utc_offset =
1858 global_ds->sys_current_utc_offset;
1859 global_ds->time_source = bmca_data->ann_time_source;
1860 global_ds->master_steps_removed =
1861 htons(ntohs(bmca_data->message_steps_removed) + 1);
1862 }
1863
1864 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1865 update_bmca(port, best_port, global_ds, default_ds, gm_prio);
1866 }
1867
1868 /* Update gmPresent. */
1869 global_ds->gm_present =
1870 (gm_prio->root_system_id.grand_master_prio1 == 255U) ?
1871 false : true;
1872
1873 /* Assign the port role for port 0. */
1874 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1875 if (global_ds->selected_role[port] == GPTP_PORT_SLAVE) {
1876 gptp_change_port_state(0, GPTP_PORT_PASSIVE);
1877 break;
1878 }
1879 }
1880
1881 if (port == GPTP_PORT_END) {
1882 gptp_change_port_state(0, GPTP_PORT_SLAVE);
1883 }
1884
1885 /* If current system is the Grand Master, set pathTrace array. */
1886 if (memcmp(default_ds->clk_id, gm_prio->root_system_id.grand_master_id,
1887 GPTP_CLOCK_ID_LEN) == 0) {
1888 global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN);
1889 memcpy(global_ds->path_trace.path_sequence,
1890 default_ds->clk_id, GPTP_CLOCK_ID_LEN);
1891 }
1892 }
1893
gptp_set_selected_tree(void)1894 static void gptp_set_selected_tree(void)
1895 {
1896 /* Set all the elements of the selected array to TRUE. */
1897 GPTP_GLOBAL_DS()->selected_array = ~0;
1898 }
1899
gptp_mi_port_role_selection_state_machine(void)1900 static void gptp_mi_port_role_selection_state_machine(void)
1901 {
1902 struct gptp_port_role_selection_state *state;
1903
1904 state = &GPTP_STATE()->pr_sel;
1905
1906 switch (state->state) {
1907 case GPTP_PR_SELECTION_INIT_BRIDGE:
1908 gptp_updt_role_disabled_tree();
1909 state->state = GPTP_PR_SELECTION_ROLE_SELECTION;
1910
1911 /* Be sure to enter the "if" statement immediately after. */
1912 GPTP_GLOBAL_DS()->reselect_array = ~0;
1913 __fallthrough;
1914
1915 case GPTP_PR_SELECTION_ROLE_SELECTION:
1916 if (GPTP_GLOBAL_DS()->reselect_array != 0) {
1917 gptp_clear_reselect_tree();
1918 gptp_updt_roles_tree();
1919 gptp_set_selected_tree();
1920 }
1921
1922 break;
1923 }
1924 }
1925
tx_announce(int port)1926 static void tx_announce(int port)
1927 {
1928 struct net_pkt *pkt;
1929
1930 pkt = gptp_prepare_announce(port);
1931 if (pkt) {
1932 gptp_send_announce(port, pkt);
1933 }
1934 }
1935
gptp_mi_port_announce_transmit_state_machine(int port)1936 static void gptp_mi_port_announce_transmit_state_machine(int port)
1937 {
1938 struct gptp_port_ds *port_ds;
1939 struct gptp_global_ds *global_ds;
1940 struct gptp_port_announce_transmit_state *state;
1941 struct gptp_port_bmca_data *bmca_data;
1942
1943 port_ds = GPTP_PORT_DS(port);
1944 global_ds = GPTP_GLOBAL_DS();
1945 bmca_data = GPTP_PORT_BMCA_DATA(port);
1946 state = &GPTP_PORT_STATE(port)->pa_transmit;
1947
1948 /* Reset interval as defined in AnnounceIntervalSetting
1949 * state machine.
1950 */
1951 if (port_ds->ptt_port_enabled && !port_ds->prev_ptt_port_enabled) {
1952 gptp_update_announce_interval(port, GPTP_ITV_SET_TO_INIT);
1953 }
1954
1955 switch (state->state) {
1956 case GPTP_PA_TRANSMIT_INIT:
1957 bmca_data->new_info = true;
1958 __fallthrough;
1959
1960 case GPTP_PA_TRANSMIT_IDLE:
1961 k_timer_stop(&state->ann_send_periodic_timer);
1962 state->ann_trigger = false;
1963 k_timer_start(&state->ann_send_periodic_timer,
1964 K_MSEC(gptp_uscaled_ns_to_timer_ms(
1965 &bmca_data->announce_interval)),
1966 K_NO_WAIT);
1967
1968 state->state = GPTP_PA_TRANSMIT_POST_IDLE;
1969 __fallthrough;
1970
1971 case GPTP_PA_TRANSMIT_POST_IDLE:
1972 if (IS_SELECTED(global_ds, port) &&
1973 !bmca_data->updt_info &&
1974 state->ann_trigger) {
1975
1976 state->state = GPTP_PA_TRANSMIT_PERIODIC;
1977
1978 } else if (IS_SELECTED(global_ds, port) &&
1979 !bmca_data->updt_info &&
1980 !state->ann_trigger &&
1981 (global_ds->selected_role[port] ==
1982 GPTP_PORT_MASTER) &&
1983 bmca_data->new_info) {
1984
1985 bmca_data->new_info = false;
1986 tx_announce(port);
1987 state->state = GPTP_PA_TRANSMIT_IDLE;
1988 }
1989
1990 break;
1991
1992 case GPTP_PA_TRANSMIT_PERIODIC:
1993 if (global_ds->selected_role[port] == GPTP_PORT_MASTER) {
1994 bmca_data->new_info = true;
1995 }
1996 state->state = GPTP_PA_TRANSMIT_IDLE;
1997 break;
1998 }
1999 }
2000
2001
gptp_mi_port_sync_state_machines(int port)2002 void gptp_mi_port_sync_state_machines(int port)
2003 {
2004 gptp_mi_pss_rcv_state_machine(port);
2005 gptp_mi_pss_send_state_machine(port);
2006 }
2007
gptp_mi_port_bmca_state_machines(int port)2008 void gptp_mi_port_bmca_state_machines(int port)
2009 {
2010 gptp_mi_port_announce_receive_state_machine(port);
2011 gptp_mi_port_announce_information_state_machine(port);
2012 gptp_mi_port_announce_transmit_state_machine(port);
2013 }
2014
gptp_mi_state_machines(void)2015 void gptp_mi_state_machines(void)
2016 {
2017 gptp_mi_site_sync_sync_state_machine();
2018 gptp_mi_clk_slave_sync_state_machine();
2019 gptp_mi_port_role_selection_state_machine();
2020 gptp_mi_clk_master_sync_offset_state_machine();
2021 #if defined(CONFIG_NET_GPTP_GM_CAPABLE)
2022 /*
2023 * Only call ClockMasterSyncSend state machine in case a Grand Master clock
2024 * is present and is this time aware system.
2025 * This check is not described by IEEE802.1AS. Instead, according to
2026 * 10.2.9.3, the SiteSyncSync state machine shall not take into account
2027 * information from ClockMasterSyncSend in case this time aware system is
2028 * not grand-master capable. Current implementation of ClockMasterSyncSend
2029 * state machine send sync indication to the PortSync entities, instead of
2030 * sending it to the SiteSyncSync entity. And the SiteSyncSync state machine
2031 * does not make sanity check.
2032 */
2033 if (memcmp(GPTP_GLOBAL_DS()->gm_priority.root_system_id.grand_master_id,
2034 GPTP_DEFAULT_DS()->clk_id, GPTP_CLOCK_ID_LEN) == 0 &&
2035 GPTP_GLOBAL_DS()->gm_present) {
2036 gptp_mi_clk_master_sync_snd_state_machine();
2037 }
2038 #endif
2039 gptp_mi_clk_master_sync_rcv_state_machine();
2040 }
2041
