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 gptp_mi_pss_send_md_sync_send(port);
564
565 __fallthrough;
566 case GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT:
567 /* Test conditions have been slightly rearranged compared to
568 * their definitions in the standard in order not to test
569 * AsCapable and pttPortEnabled when not needed (they are
570 * already tested with rcvdPSSync for the reset of this state
571 * machine).
572 */
573 if ((global_ds->selected_role[port] == GPTP_PORT_MASTER) &&
574 ((state->rcvd_pss_sync &&
575 state->half_sync_itv_timer_expired &&
576 state->pss_sync_ptr->local_port_number != port) ||
577 (state->sync_itv_timer_expired &&
578 (state->last_rcvd_port_num != port) &&
579 port_ds->as_capable && port_ds->ptt_port_enabled))) {
580
581 state->state = GPTP_PSS_SEND_SEND_MD_SYNC;
582
583 } else if ((state->state == GPTP_PSS_SEND_SEND_MD_SYNC) ||
584 (state->rcvd_pss_sync &&
585 !state->sync_itv_timer_expired &&
586 (global_ds->selected_role[port] ==
587 GPTP_PORT_MASTER) &&
588 state->pss_sync_ptr->local_port_number != port)) {
589
590 /* Change state as it may have transitioned from
591 * SEND_MD_SYNC.
592 */
593 state->state = GPTP_PSS_SEND_SET_SYNC_RECEIPT_TIMEOUT;
594
595 /* Stop and (re)start receipt timeout timer. */
596 k_timer_stop(&state->send_sync_receipt_timeout_timer);
597 state->send_sync_receipt_timeout_timer_expired = false;
598
599 duration =
600 K_MSEC(port_ds->sync_receipt_timeout_time_itv /
601 (NSEC_PER_USEC * USEC_PER_MSEC));
602
603 k_timer_start(&state->send_sync_receipt_timeout_timer,
604 duration, K_NO_WAIT);
605
606 } else if (state->send_sync_receipt_timeout_timer_expired) {
607 state->state = GPTP_PSS_SEND_SYNC_RECEIPT_TIMEOUT;
608 }
609
610 break;
611 }
612 }
613
gptp_mi_site_ss_prepare_pss_send(void)614 static void gptp_mi_site_ss_prepare_pss_send(void)
615 {
616 struct gptp_site_sync_sync_state *state;
617
618 state = &GPTP_STATE()->site_ss;
619
620 memcpy(&state->pss_send, state->pss_rcv_ptr,
621 sizeof(struct gptp_mi_port_sync_sync));
622 }
623
gptp_mi_site_ss_send_to_pss(void)624 static void gptp_mi_site_ss_send_to_pss(void)
625 {
626 struct gptp_site_sync_sync_state *state;
627 struct gptp_pss_send_state *pss_send;
628 int port;
629
630 state = &GPTP_STATE()->site_ss;
631
632 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
633 pss_send = &GPTP_PORT_STATE(port)->pss_send;
634 pss_send->pss_sync_ptr = &state->pss_send;
635 pss_send->rcvd_pss_sync = true;
636 }
637 }
638
gptp_mi_site_sync_sync_state_machine(void)639 static void gptp_mi_site_sync_sync_state_machine(void)
640 {
641 bool gm_present;
642 uint16_t local_port_number;
643 struct gptp_site_sync_sync_state *state;
644 struct gptp_clk_slave_sync_state *clk_ss;
645
646 state = &GPTP_STATE()->site_ss;
647 clk_ss = &GPTP_STATE()->clk_slave_sync;
648 gm_present = GPTP_GLOBAL_DS()->gm_present;
649
650 if (!state->pss_rcv_ptr) {
651 /* We do not have connection to GM yet */
652 return;
653 }
654
655 local_port_number = state->pss_rcv_ptr->local_port_number;
656
657 switch (state->state) {
658 case GPTP_SSS_INITIALIZING:
659 state->rcvd_pss = false;
660 state->state = GPTP_SSS_RECEIVING_SYNC;
661 break;
662
663 case GPTP_SSS_RECEIVING_SYNC:
664 if (state->rcvd_pss) {
665 state->rcvd_pss = false;
666 if (gptp_is_slave_port(local_port_number) &&
667 gm_present) {
668 gptp_mi_site_ss_prepare_pss_send();
669
670 /*
671 * Send Port Sync Sync to all
672 * PortSyncSyncSend State Machines.
673 */
674 gptp_mi_site_ss_send_to_pss();
675
676 /*
677 * Send PortSyncSync to
678 * ClockSlaveSync State Machine.
679 */
680 clk_ss->pss_rcv_ptr = &state->pss_send;
681 clk_ss->rcvd_pss = true;
682 }
683 }
684
685 break;
686 }
687 }
688
gptp_mi_clk_slave_sync_compute(void)689 static void gptp_mi_clk_slave_sync_compute(void)
690 {
691 struct gptp_clk_slave_sync_state *state;
692 struct gptp_clk_master_sync_offset_state *offset_state;
693 struct gptp_global_ds *global_ds;
694 struct gptp_md_sync_info *pss;
695 struct gptp_port_ds *port_ds;
696 uint64_t sync_receipt_time;
697
698 state = &GPTP_STATE()->clk_slave_sync;
699 offset_state = &GPTP_STATE()->clk_master_sync_offset;
700 global_ds = GPTP_GLOBAL_DS();
701 port_ds = GPTP_PORT_DS(state->pss_rcv_ptr->local_port_number);
702
703 pss = &state->pss_rcv_ptr->sync_info;
704
705 sync_receipt_time = port_ds->neighbor_prop_delay;
706 sync_receipt_time *= pss->rate_ratio;
707 sync_receipt_time /= port_ds->neighbor_rate_ratio;
708 sync_receipt_time += pss->follow_up_correction_field;
709 sync_receipt_time += port_ds->delay_asymmetry;
710
711 global_ds->sync_receipt_time.second = sync_receipt_time / NSEC_PER_SEC;
712 global_ds->sync_receipt_time.fract_nsecond =
713 (sync_receipt_time % NSEC_PER_SEC) * GPTP_POW2_16;
714 global_ds->sync_receipt_time.second += pss->precise_orig_ts.second;
715 global_ds->sync_receipt_time.fract_nsecond +=
716 pss->precise_orig_ts.nanosecond * GPTP_POW2_16;
717
718 global_ds->sync_receipt_local_time = port_ds->delay_asymmetry;
719 global_ds->sync_receipt_local_time /= pss->rate_ratio;
720 global_ds->sync_receipt_local_time +=
721 (port_ds->neighbor_prop_delay / port_ds->neighbor_rate_ratio);
722 global_ds->sync_receipt_local_time += pss->upstream_tx_time;
723
724 global_ds->gm_time_base_indicator = pss->gm_time_base_indicator;
725 global_ds->last_gm_phase_change.high = pss->last_gm_phase_change.high;
726 global_ds->last_gm_phase_change.low = pss->last_gm_phase_change.low;
727 global_ds->last_gm_freq_change = pss->last_gm_freq_change;
728
729 offset_state->rcvd_sync_receipt_time = true;
730 }
731
732 #if defined(CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE)
gptp_update_local_port_clock(void)733 static void gptp_update_local_port_clock(void)
734 {
735 struct gptp_clk_slave_sync_state *state;
736 struct gptp_global_ds *global_ds;
737 struct gptp_port_ds *port_ds;
738 int port;
739 int64_t nanosecond_diff;
740 int64_t second_diff;
741 const struct device *clk;
742 struct net_ptp_time tm;
743 unsigned int key;
744
745 state = &GPTP_STATE()->clk_slave_sync;
746 global_ds = GPTP_GLOBAL_DS();
747 port = state->pss_rcv_ptr->local_port_number;
748 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
749
750 port_ds = GPTP_PORT_DS(port);
751
752 /* Check if the last neighbor rate ratio can still be used */
753 if (!port_ds->neighbor_rate_ratio_valid) {
754 return;
755 }
756
757 port_ds->neighbor_rate_ratio_valid = false;
758
759 second_diff = global_ds->sync_receipt_time.second -
760 (global_ds->sync_receipt_local_time / NSEC_PER_SEC);
761 nanosecond_diff =
762 (global_ds->sync_receipt_time.fract_nsecond / GPTP_POW2_16) -
763 (global_ds->sync_receipt_local_time % NSEC_PER_SEC);
764
765 clk = net_eth_get_ptp_clock(GPTP_PORT_IFACE(port));
766 if (!clk) {
767 return;
768 }
769
770 if (second_diff > 0 && nanosecond_diff < 0) {
771 second_diff--;
772 nanosecond_diff = NSEC_PER_SEC + nanosecond_diff;
773 }
774
775 if (second_diff < 0 && nanosecond_diff > 0) {
776 second_diff++;
777 nanosecond_diff = -(int64_t)NSEC_PER_SEC + nanosecond_diff;
778 }
779
780 ptp_clock_rate_adjust(clk, port_ds->neighbor_rate_ratio);
781
782 /* If time difference is too high, set the clock value.
783 * Otherwise, adjust it.
784 */
785 if (second_diff || (second_diff == 0 &&
786 (nanosecond_diff < -5000 ||
787 nanosecond_diff > 5000))) {
788 bool underflow = false;
789
790 key = irq_lock();
791 ptp_clock_get(clk, &tm);
792
793 if (second_diff < 0 && tm.second < -second_diff) {
794 NET_DBG("Do not set local clock because %lu < %ld",
795 (unsigned long int)tm.second,
796 (long int)-second_diff);
797 goto skip_clock_set;
798 }
799
800 tm.second += second_diff;
801
802 if (nanosecond_diff < 0 &&
803 tm.nanosecond < -nanosecond_diff) {
804 underflow = true;
805 }
806
807 tm.nanosecond += nanosecond_diff;
808
809 if (underflow) {
810 tm.second--;
811 tm.nanosecond += NSEC_PER_SEC;
812 } else if (tm.nanosecond >= NSEC_PER_SEC) {
813 tm.second++;
814 tm.nanosecond -= NSEC_PER_SEC;
815 }
816
817 /* This prints too much data normally but can be enabled to see
818 * what time we are setting to the local clock.
819 */
820 if (0) {
821 NET_INFO("Set local clock %lu.%lu",
822 (unsigned long int)tm.second,
823 (unsigned long int)tm.nanosecond);
824 }
825
826 ptp_clock_set(clk, &tm);
827
828 skip_clock_set:
829 irq_unlock(key);
830 } else {
831 if (nanosecond_diff < -200) {
832 nanosecond_diff = -200;
833 } else if (nanosecond_diff > 200) {
834 nanosecond_diff = 200;
835 }
836
837 ptp_clock_adjust(clk, nanosecond_diff);
838 }
839 }
840 #endif /* CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE */
841
gptp_mi_clk_slave_sync_state_machine(void)842 static void gptp_mi_clk_slave_sync_state_machine(void)
843 {
844 struct gptp_clk_slave_sync_state *state;
845
846 state = &GPTP_STATE()->clk_slave_sync;
847
848 switch (state->state) {
849 case GPTP_CLK_SLAVE_SYNC_INITIALIZING:
850 state->rcvd_pss = false;
851 state->state = GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND;
852 break;
853
854 case GPTP_CLK_SLAVE_SYNC_SEND_SYNC_IND:
855 if (state->rcvd_pss) {
856 state->rcvd_pss = false;
857 gptp_mi_clk_slave_sync_compute();
858
859 #if defined(CONFIG_NET_GPTP_USE_DEFAULT_CLOCK_UPDATE)
860 /* Instead of updating SlaveClock, update LocalClock */
861 gptp_update_local_port_clock();
862 #endif
863 gptp_call_phase_dis_cb();
864 }
865
866 break;
867 }
868 }
869
gptp_mi_clk_master_sync_offset_state_machine(void)870 static void gptp_mi_clk_master_sync_offset_state_machine(void)
871 {
872 struct gptp_clk_master_sync_offset_state *state;
873 struct gptp_global_ds *global_ds;
874
875 state = &GPTP_STATE()->clk_master_sync_offset;
876 global_ds = GPTP_GLOBAL_DS();
877
878 switch (state->state) {
879 case GPTP_CMS_OFFSET_INITIALIZING:
880 state->rcvd_sync_receipt_time = false;
881 state->state = GPTP_CMS_OFFSET_INDICATION;
882 break;
883 case GPTP_CMS_OFFSET_INDICATION:
884 if (!state->rcvd_sync_receipt_time) {
885 break;
886 }
887
888 state->rcvd_sync_receipt_time = false;
889
890 if (global_ds->selected_role[0] == GPTP_PORT_PASSIVE) {
891 /* TODO Calculate real values for proper BC support */
892 memset(&global_ds->clk_src_phase_offset, 0x0,
893 sizeof(struct gptp_scaled_ns));
894 global_ds->clk_src_freq_offset = 0;
895 } else if (global_ds->clk_src_time_base_indicator_prev
896 != global_ds->clk_src_time_base_indicator) {
897 memcpy(&global_ds->clk_src_phase_offset,
898 &global_ds->last_gm_phase_change,
899 sizeof(struct gptp_scaled_ns));
900
901 global_ds->clk_src_freq_offset =
902 global_ds->last_gm_freq_change;
903 }
904
905 break;
906 default:
907 NET_ERR("Unrecognised state %d", state->state);
908 break;
909 }
910 }
911
912 #if defined(CONFIG_NET_GPTP_GM_CAPABLE)
gptp_mi_setup_sync_send_time(void)913 static inline void gptp_mi_setup_sync_send_time(void)
914 {
915 struct gptp_clk_master_sync_snd_state *state;
916 struct gptp_global_ds *global_ds;
917 uint64_t time_helper;
918
919 state = &GPTP_STATE()->clk_master_sync_send;
920 global_ds = GPTP_GLOBAL_DS();
921
922 time_helper = state->sync_send_time.low;
923
924 state->sync_send_time.low +=
925 global_ds->clk_master_sync_itv;
926
927 /* Check for overflow */
928 if (state->sync_send_time.low < time_helper) {
929 state->sync_send_time.high += 1U;
930 state->sync_send_time.low =
931 UINT64_MAX - state->sync_send_time.low;
932 }
933 }
934
gptp_mi_set_ps_sync_cmss(void)935 static void gptp_mi_set_ps_sync_cmss(void)
936 {
937 struct gptp_clk_master_sync_snd_state *state;
938 struct gptp_global_ds *global_ds;
939 struct gptp_md_sync_info *sync_info;
940 uint64_t current_time;
941
942 global_ds = GPTP_GLOBAL_DS();
943 state = &GPTP_STATE()->clk_master_sync_send;
944
945 sync_info = &state->pss_snd.sync_info;
946
947 state->pss_snd.local_port_number = 0U;
948
949 current_time = gptp_get_current_master_time_nanosecond();
950
951 sync_info->precise_orig_ts.second = current_time / NSEC_PER_SEC;
952 sync_info->precise_orig_ts.nanosecond = current_time % NSEC_PER_SEC;
953
954 /* TODO calculate correction field properly, rate_ratio is also set to
955 * zero instead of being copied from global_ds as it affects the final
956 * value of FUP correction field.
957 */
958 sync_info->follow_up_correction_field = 0;
959 sync_info->rate_ratio = 0;
960
961 memcpy(&sync_info->src_port_id.clk_id,
962 GPTP_DEFAULT_DS()->clk_id,
963 GPTP_CLOCK_ID_LEN);
964
965 sync_info->src_port_id.port_number = 0U;
966 sync_info->log_msg_interval = CONFIG_NET_GPTP_INIT_LOG_SYNC_ITV;
967 sync_info->upstream_tx_time = global_ds->local_time.low;
968
969 state->pss_snd.sync_receipt_timeout_time = UINT64_MAX;
970
971 sync_info->gm_time_base_indicator =
972 global_ds->clk_src_time_base_indicator;
973
974 memcpy(&sync_info->last_gm_phase_change,
975 &global_ds->clk_src_phase_offset,
976 sizeof(struct gptp_scaled_ns));
977
978 sync_info->last_gm_freq_change = global_ds->clk_src_freq_offset;
979 }
980
gptp_mi_tx_ps_sync_cmss(void)981 static inline void gptp_mi_tx_ps_sync_cmss(void)
982 {
983 struct gptp_clk_master_sync_snd_state *state;
984 struct gptp_pss_send_state *pss_send;
985 int port;
986
987 state = &GPTP_STATE()->clk_master_sync_send;
988
989 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
990 pss_send = &GPTP_PORT_STATE(port)->pss_send;
991 pss_send->pss_sync_ptr = &state->pss_snd;
992
993 pss_send->rcvd_pss_sync = true;
994 }
995 }
996
gptp_mi_clk_master_sync_snd_state_machine(void)997 static void gptp_mi_clk_master_sync_snd_state_machine(void)
998 {
999 struct gptp_clk_master_sync_snd_state *state;
1000 uint64_t current_time;
1001
1002 state = &GPTP_STATE()->clk_master_sync_send;
1003
1004 switch (state->state) {
1005 case GPTP_CMS_SND_INITIALIZING:
1006 gptp_mi_setup_sync_send_time();
1007
1008 state->state = GPTP_CMS_SND_INDICATION;
1009 break;
1010
1011 case GPTP_CMS_SND_INDICATION:
1012 current_time = gptp_get_current_master_time_nanosecond();
1013
1014 if (current_time >= state->sync_send_time.low) {
1015 gptp_mi_set_ps_sync_cmss();
1016 gptp_mi_tx_ps_sync_cmss();
1017
1018 gptp_mi_setup_sync_send_time();
1019 }
1020
1021 break;
1022
1023 default:
1024 NET_ERR("Unrecognised state %d", state->state);
1025 break;
1026 }
1027 }
1028 #endif
1029
gptp_compute_gm_rate_ratio(void)1030 static void gptp_compute_gm_rate_ratio(void)
1031 {
1032 static struct net_ptp_extended_time src_time_0;
1033 static struct gptp_uscaled_ns local_time_0;
1034 struct net_ptp_extended_time src_time_n;
1035 struct gptp_uscaled_ns local_time_n;
1036 struct net_ptp_extended_time src_time_t;
1037 struct gptp_uscaled_ns local_time_t;
1038 struct gptp_clk_master_sync_rcv_state *state;
1039 struct gptp_global_ds *global_ds;
1040 double new_gm_rate;
1041
1042 state = &GPTP_STATE()->clk_master_sync_receive;
1043 global_ds = GPTP_GLOBAL_DS();
1044
1045 /* Get current local and source time */
1046 memcpy(&src_time_n, &state->rcvd_clk_src_req.src_time,
1047 sizeof(struct net_ptp_extended_time));
1048
1049 memcpy(&local_time_n, &global_ds->local_time,
1050 sizeof(struct gptp_uscaled_ns));
1051
1052 if ((src_time_0.second == 0U && src_time_0.fract_nsecond == 0U)
1053 || (local_time_0.high == 0U && local_time_0.low == 0U)) {
1054 memcpy(&src_time_0, &src_time_n,
1055 sizeof(struct net_ptp_extended_time));
1056
1057 memcpy(&local_time_0, &local_time_n,
1058 sizeof(struct gptp_uscaled_ns));
1059
1060 global_ds->gm_rate_ratio = 1.0;
1061
1062 return;
1063 }
1064
1065 /* Take care of the sign of the result */
1066 new_gm_rate = 1.0;
1067
1068 if ((src_time_n.second < src_time_0.second)
1069 || (src_time_n.second == src_time_0.second
1070 && src_time_n.fract_nsecond < src_time_0.fract_nsecond)) {
1071 /* Change result sign and swap src_time_n and src_time_0 */
1072 memcpy(&src_time_t, &src_time_n,
1073 sizeof(struct net_ptp_extended_time));
1074 memcpy(&src_time_n, &src_time_0,
1075 sizeof(struct net_ptp_extended_time));
1076 memcpy(&src_time_0, &src_time_t,
1077 sizeof(struct net_ptp_extended_time));
1078
1079 new_gm_rate *= -1;
1080 }
1081
1082 if ((local_time_n.high < local_time_0.high)
1083 || (local_time_n.high == local_time_0.high
1084 && local_time_n.low < local_time_0.low)) {
1085 /* Change result sign and swap local_time_n and local_time_0 */
1086 memcpy(&local_time_t, &local_time_n,
1087 sizeof(struct gptp_uscaled_ns));
1088 memcpy(&local_time_n, &local_time_0,
1089 sizeof(struct gptp_uscaled_ns));
1090 memcpy(&local_time_0, &local_time_t,
1091 sizeof(struct gptp_uscaled_ns));
1092
1093 new_gm_rate *= -1;
1094 }
1095
1096 /* At this point src_time_n >= src_time_0 */
1097 src_time_n.second -= src_time_0.second;
1098
1099 if (src_time_n.fract_nsecond >= src_time_0.fract_nsecond) {
1100 src_time_n.fract_nsecond -= src_time_0.fract_nsecond;
1101 } else {
1102 src_time_n.second -= 1U;
1103 src_time_n.fract_nsecond = (NSEC_PER_SEC * GPTP_POW2_16)
1104 - src_time_0.fract_nsecond;
1105 }
1106
1107 /* At this point local_time_n >= local_time_0 */
1108 local_time_n.high -= local_time_0.high;
1109
1110 if (local_time_n.low >= local_time_0.low) {
1111 local_time_n.low -= local_time_0.low;
1112 } else {
1113 local_time_n.high -= 1U;
1114 local_time_n.low = UINT64_MAX - local_time_0.low;
1115 }
1116
1117 /* Calculate it in nanoseconds, new_gm_rate is either 1 or -1 here */
1118 new_gm_rate *= ((src_time_n.second * NSEC_PER_SEC)
1119 + (src_time_n.fract_nsecond / GPTP_POW2_16));
1120
1121 new_gm_rate /= local_time_n.low;
1122
1123 global_ds->gm_rate_ratio = new_gm_rate;
1124 }
1125
gptp_mi_clk_master_sync_rcv_state_machine(void)1126 static void gptp_mi_clk_master_sync_rcv_state_machine(void)
1127 {
1128 struct gptp_clk_master_sync_rcv_state *s;
1129 struct gptp_global_ds *global_ds;
1130
1131 #ifdef CONFIG_NET_GPTP_PROBE_CLOCK_SOURCE_ON_DEMAND
1132 struct gptp_clk_src_time_invoke_params invoke_args = {};
1133 uint64_t cur = gptp_get_current_master_time_nanosecond();
1134
1135 invoke_args.src_time.second = cur / NSEC_PER_SEC;
1136 cur -= (invoke_args.src_time.second * NSEC_PER_SEC);
1137
1138 invoke_args.src_time.fract_nsecond = cur * GPTP_POW2_16;
1139
1140 memset(&invoke_args.last_gm_phase_change, 0x0,
1141 sizeof(struct gptp_scaled_ns));
1142 invoke_args.last_gm_freq_change = 0;
1143
1144 gptp_clk_src_time_invoke(&invoke_args);
1145 #endif
1146
1147 global_ds = GPTP_GLOBAL_DS();
1148
1149 s = &GPTP_STATE()->clk_master_sync_receive;
1150 switch (s->state) {
1151 case GPTP_CMS_RCV_INITIALIZING:
1152 s->state = GPTP_CMS_RCV_WAITING;
1153 break;
1154
1155 case GPTP_CMS_RCV_WAITING:
1156 if (s->rcvd_clock_source_req || s->rcvd_local_clock_tick) {
1157 s->state = GPTP_CMS_RCV_SOURCE_TIME;
1158 }
1159
1160 break;
1161
1162 case GPTP_CMS_RCV_SOURCE_TIME:
1163 global_ds->local_time.high = 0U;
1164 global_ds->local_time.low =
1165 gptp_get_current_master_time_nanosecond();
1166
1167 if (s->rcvd_clock_source_req) {
1168 gptp_compute_gm_rate_ratio();
1169
1170 global_ds->clk_src_time_base_indicator_prev =
1171 global_ds->clk_src_time_base_indicator;
1172
1173 global_ds->clk_src_time_base_indicator =
1174 s->rcvd_clk_src_req.time_base_indicator;
1175
1176 memcpy(&global_ds->clk_src_last_gm_phase_change,
1177 &s->rcvd_clk_src_req.last_gm_phase_change,
1178 sizeof(struct gptp_scaled_ns));
1179
1180 global_ds->clk_src_last_gm_freq_change =
1181 s->rcvd_clk_src_req.last_gm_freq_change;
1182 }
1183
1184 s->rcvd_clock_source_req = false;
1185 s->rcvd_local_clock_tick = false;
1186 s->state = GPTP_CMS_RCV_WAITING;
1187 break;
1188
1189 default:
1190 NET_ERR("Unrecognised state %d", s->state);
1191 break;
1192 }
1193 }
1194
copy_path_trace(struct gptp_announce * announce)1195 static void copy_path_trace(struct gptp_announce *announce)
1196 {
1197 int len = ntohs(announce->tlv.len);
1198 struct gptp_path_trace *sys_path_trace;
1199
1200 if (len > GPTP_MAX_PATHTRACE_SIZE) {
1201 NET_ERR("Too long path trace (%d vs %d)",
1202 GPTP_MAX_PATHTRACE_SIZE, len);
1203 return;
1204 }
1205
1206 sys_path_trace = &GPTP_GLOBAL_DS()->path_trace;
1207
1208 sys_path_trace->len = htons(len + GPTP_CLOCK_ID_LEN);
1209
1210 memcpy(sys_path_trace->path_sequence, announce->tlv.path_sequence,
1211 len);
1212
1213 /* Append local clockIdentity. */
1214 memcpy((uint8_t *)sys_path_trace->path_sequence + len,
1215 GPTP_DEFAULT_DS()->clk_id, GPTP_CLOCK_ID_LEN);
1216 }
1217
gptp_mi_qualify_announce(int port,struct net_pkt * announce_msg)1218 static bool gptp_mi_qualify_announce(int port, struct net_pkt *announce_msg)
1219 {
1220 struct gptp_announce *announce;
1221 struct gptp_hdr *hdr;
1222 int i;
1223 uint16_t len;
1224
1225 hdr = GPTP_HDR(announce_msg);
1226 announce = GPTP_ANNOUNCE(announce_msg);
1227
1228 if (memcmp(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id,
1229 GPTP_CLOCK_ID_LEN) == 0) {
1230 return false;
1231 }
1232
1233 len = ntohs(announce->steps_removed);
1234 if (len >= 255U) {
1235 return false;
1236 }
1237
1238 for (i = 0; i < len + 1; i++) {
1239 if (memcmp(announce->tlv.path_sequence[i],
1240 GPTP_DEFAULT_DS()->clk_id,
1241 GPTP_CLOCK_ID_LEN) == 0) {
1242 return false;
1243 }
1244 }
1245
1246 if (GPTP_GLOBAL_DS()->selected_role[port] == GPTP_PORT_SLAVE) {
1247 copy_path_trace(announce);
1248 }
1249
1250 return true;
1251 }
1252
gptp_mi_port_announce_receive_state_machine(int port)1253 static void gptp_mi_port_announce_receive_state_machine(int port)
1254 {
1255 struct gptp_port_ds *port_ds;
1256 struct gptp_port_announce_receive_state *state;
1257 struct gptp_port_bmca_data *bmca_data;
1258
1259 state = &GPTP_PORT_STATE(port)->pa_rcv;
1260 port_ds = GPTP_PORT_DS(port);
1261 bmca_data = GPTP_PORT_BMCA_DATA(port);
1262
1263 if ((!port_ds->ptt_port_enabled) || (!port_ds->as_capable)) {
1264 state->state = GPTP_PA_RCV_DISCARD;
1265 }
1266
1267 switch (state->state) {
1268 case GPTP_PA_RCV_DISCARD:
1269 state->rcvd_announce = false;
1270 bmca_data->rcvd_msg = false;
1271 if (bmca_data->rcvd_announce_ptr != NULL) {
1272 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1273 bmca_data->rcvd_announce_ptr = NULL;
1274 }
1275
1276 state->state = GPTP_PA_RCV_RECEIVE;
1277 break;
1278
1279 case GPTP_PA_RCV_RECEIVE:
1280 /* "portEnabled" is not checked: the interface is always up. */
1281 if (state->rcvd_announce &&
1282 port_ds->ptt_port_enabled &&
1283 port_ds->as_capable &&
1284 !bmca_data->rcvd_msg) {
1285 state->rcvd_announce = false;
1286
1287 bmca_data->rcvd_msg = gptp_mi_qualify_announce(
1288 port, bmca_data->rcvd_announce_ptr);
1289 if (!bmca_data->rcvd_msg) {
1290 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1291 bmca_data->rcvd_announce_ptr = NULL;
1292 }
1293 }
1294
1295 break;
1296 }
1297 }
1298
1299 /*
1300 * Compare a vector to an announce message vector.
1301 * All must be in big endian (network) order.
1302 */
compare_priority_vectors(struct gptp_priority_vector * vector,struct net_pkt * pkt,int port)1303 static enum gptp_received_info compare_priority_vectors(
1304 struct gptp_priority_vector *vector,
1305 struct net_pkt *pkt, int port)
1306 {
1307 struct gptp_hdr *hdr;
1308 struct gptp_announce *announce;
1309 int rsi_cmp, spi_cmp, port_cmp;
1310
1311 hdr = GPTP_HDR(pkt);
1312 announce = GPTP_ANNOUNCE(pkt);
1313
1314 /* Compare rootSystemIdentity and stepsRemoved. */
1315 rsi_cmp = memcmp(&announce->root_system_id,
1316 &vector->root_system_id,
1317 sizeof(struct gptp_root_system_identity) +
1318 sizeof(uint16_t));
1319 if (rsi_cmp < 0) {
1320 /* Better rootSystemIdentity. */
1321 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1322 }
1323
1324 /* Compare sourcePortIdentity. */
1325 spi_cmp = memcmp(&hdr->port_id, &vector->src_port_id,
1326 sizeof(struct gptp_port_identity));
1327
1328 port_cmp = (int)port - ntohs(vector->port_number);
1329
1330 if (spi_cmp == 0) {
1331 if (rsi_cmp == 0) {
1332 if (port_cmp == 0) {
1333 /* Same priority vector. */
1334 return GPTP_RCVD_INFO_REPEATED_MASTER_INFO;
1335 } else if (port_cmp < 0) {
1336 /* Priority vector with better reception port
1337 * number.
1338 */
1339 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1340 }
1341 } else {
1342 /* Same master port but different Grand Master. */
1343 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1344 }
1345 } else if ((spi_cmp < 0) && (rsi_cmp == 0)) {
1346 /* Same Grand Master but better masterPort. */
1347 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1348 }
1349
1350 return GPTP_RCVD_INFO_INFERIOR_MASTER_INFO;
1351 }
1352
rcv_info(int port)1353 static enum gptp_received_info rcv_info(int port)
1354 {
1355 /* TODO
1356 * How can we define that a message does not convey the port
1357 * role Master port ?
1358 * It is needed to define that to be able to send
1359 * GPTP_RCVD_INFO_OTHER_INFO.
1360 */
1361 struct gptp_port_bmca_data *bmca_data;
1362 struct gptp_announce *announce;
1363
1364 bmca_data = GPTP_PORT_BMCA_DATA(port);
1365 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1366
1367 bmca_data->message_steps_removed = announce->steps_removed;
1368
1369 return compare_priority_vectors(&bmca_data->port_priority,
1370 bmca_data->rcvd_announce_ptr,
1371 port);
1372 }
1373
record_other_announce_info(int port)1374 static void record_other_announce_info(int port)
1375 {
1376 struct gptp_hdr *hdr;
1377 struct gptp_announce *announce;
1378 struct gptp_port_bmca_data *bmca_data;
1379
1380 bmca_data = GPTP_PORT_BMCA_DATA(port);
1381 hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr);
1382 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1383
1384 /* Copy leap61, leap59, current UTC offset valid, time traceable and
1385 * frequency traceable flags.
1386 */
1387 bmca_data->ann_flags.octets[1] = hdr->flags.octets[1];
1388
1389 bmca_data->ann_current_utc_offset = ntohs(announce->cur_utc_offset);
1390 bmca_data->ann_time_source = announce->time_source;
1391 }
1392
copy_priority_vector(struct gptp_priority_vector * vector,struct net_pkt * pkt,int port)1393 static void copy_priority_vector(struct gptp_priority_vector *vector,
1394 struct net_pkt *pkt, int port)
1395 {
1396 struct gptp_hdr *hdr;
1397 struct gptp_announce *announce;
1398
1399 hdr = GPTP_HDR(pkt);
1400 announce = GPTP_ANNOUNCE(pkt);
1401
1402 memcpy(&vector->root_system_id, &announce->root_system_id,
1403 sizeof(struct gptp_root_system_identity) + sizeof(uint16_t));
1404
1405 memcpy(&vector->src_port_id, &hdr->port_id,
1406 sizeof(struct gptp_port_identity));
1407
1408 vector->port_number = htons(port);
1409 }
1410
gptp_mi_port_announce_information_state_machine(int port)1411 static void gptp_mi_port_announce_information_state_machine(int port)
1412 {
1413 struct gptp_port_ds *port_ds;
1414 struct gptp_global_ds *global_ds;
1415 struct gptp_port_announce_information_state *state;
1416 struct gptp_announce *announce;
1417 struct gptp_hdr *hdr;
1418 struct gptp_port_bmca_data *bmca_data;
1419 struct gptp_pss_rcv_state *pss_rcv;
1420
1421 bmca_data = GPTP_PORT_BMCA_DATA(port);
1422 state = &GPTP_PORT_STATE(port)->pa_info;
1423 port_ds = GPTP_PORT_DS(port);
1424 global_ds = GPTP_GLOBAL_DS();
1425
1426 if ((!port_ds->ptt_port_enabled || !port_ds->as_capable) &&
1427 (bmca_data->info_is != GPTP_INFO_IS_DISABLED)) {
1428 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_DISABLED);
1429 }
1430
1431 switch (state->state) {
1432 case GPTP_PA_INFO_DISABLED:
1433 bmca_data->rcvd_msg = false;
1434 bmca_data->info_is = GPTP_INFO_IS_DISABLED;
1435 SET_RESELECT(global_ds, port);
1436 CLEAR_SELECTED(global_ds, port);
1437 gptp_change_pa_info_state(port, state,
1438 GPTP_PA_INFO_POST_DISABLED);
1439 k_timer_stop(&state->ann_rcpt_expiry_timer);
1440 state->ann_expired = true;
1441 __fallthrough;
1442
1443 case GPTP_PA_INFO_POST_DISABLED:
1444 if (port_ds->ptt_port_enabled && port_ds->as_capable) {
1445 gptp_change_pa_info_state(port, state,
1446 GPTP_PA_INFO_AGED);
1447 } else if (bmca_data->rcvd_msg) {
1448 gptp_change_pa_info_state(port, state,
1449 GPTP_PA_INFO_DISABLED);
1450 }
1451
1452 break;
1453
1454 case GPTP_PA_INFO_AGED:
1455 bmca_data->info_is = GPTP_INFO_IS_AGED;
1456 CLEAR_SELECTED(global_ds, port);
1457 SET_RESELECT(global_ds, port);
1458 /* Transition will be actually tested in UPDATE state. */
1459 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_UPDATE);
1460 break;
1461
1462 case GPTP_PA_INFO_UPDATE:
1463 if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) {
1464 memcpy(&bmca_data->port_priority,
1465 &bmca_data->master_priority,
1466 sizeof(struct gptp_priority_vector));
1467
1468 bmca_data->port_steps_removed =
1469 global_ds->master_steps_removed;
1470 bmca_data->updt_info = false;
1471 bmca_data->info_is = GPTP_INFO_IS_MINE;
1472 bmca_data->new_info = true;
1473 gptp_change_pa_info_state(port, state,
1474 GPTP_PA_INFO_CURRENT);
1475 }
1476
1477 break;
1478
1479 case GPTP_PA_INFO_CURRENT:
1480 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
1481 if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) {
1482 gptp_change_pa_info_state(port, state,
1483 GPTP_PA_INFO_UPDATE);
1484 } else if (bmca_data->rcvd_msg && !bmca_data->updt_info) {
1485 gptp_change_pa_info_state(port, state,
1486 GPTP_PA_INFO_RECEIVE);
1487 } else if ((bmca_data->info_is == GPTP_INFO_IS_RECEIVED) &&
1488 !bmca_data->updt_info &&
1489 !bmca_data->rcvd_msg &&
1490 (state->ann_expired ||
1491 (global_ds->gm_present &&
1492 pss_rcv->rcv_sync_receipt_timeout_timer_expired))) {
1493 gptp_change_pa_info_state(port, state,
1494 GPTP_PA_INFO_AGED);
1495 }
1496
1497 break;
1498
1499 case GPTP_PA_INFO_RECEIVE:
1500 switch (rcv_info(port)) {
1501 case GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO:
1502 gptp_change_pa_info_state(port, state,
1503 GPTP_PA_INFO_SUPERIOR_MASTER_PORT);
1504 break;
1505 case GPTP_RCVD_INFO_REPEATED_MASTER_INFO:
1506 gptp_change_pa_info_state(port, state,
1507 GPTP_PA_INFO_REPEATED_MASTER_PORT);
1508 break;
1509 case GPTP_RCVD_INFO_INFERIOR_MASTER_INFO:
1510 __fallthrough;
1511 case GPTP_RCVD_INFO_OTHER_INFO:
1512 gptp_change_pa_info_state(port, state,
1513 GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT);
1514 break;
1515 }
1516
1517 break;
1518
1519 case GPTP_PA_INFO_SUPERIOR_MASTER_PORT:
1520 /* We copy directly the content of the message to the port
1521 * priority vector without using an intermediate
1522 * messagePriority structure.
1523 */
1524
1525 if (!bmca_data->rcvd_announce_ptr) {
1526 /* Shouldn't be reached. Checked for safety reason. */
1527 bmca_data->rcvd_msg = false;
1528 gptp_change_pa_info_state(port, state,
1529 GPTP_PA_INFO_CURRENT);
1530 break;
1531 }
1532
1533 copy_priority_vector(&bmca_data->port_priority,
1534 bmca_data->rcvd_announce_ptr, port);
1535
1536 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1537 bmca_data->port_steps_removed = ntohs(announce->steps_removed);
1538 record_other_announce_info(port);
1539 hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr);
1540 gptp_set_time_itv(&bmca_data->ann_rcpt_timeout_time_interval,
1541 port_ds->announce_receipt_timeout,
1542 hdr->log_msg_interval);
1543 bmca_data->info_is = GPTP_INFO_IS_RECEIVED;
1544 CLEAR_SELECTED(global_ds, port);
1545 SET_RESELECT(global_ds, port);
1546 __fallthrough;
1547
1548 case GPTP_PA_INFO_REPEATED_MASTER_PORT:
1549 k_timer_stop(&state->ann_rcpt_expiry_timer);
1550 state->ann_expired = false;
1551 k_timer_start(&state->ann_rcpt_expiry_timer,
1552 K_MSEC(gptp_uscaled_ns_to_timer_ms(
1553 &bmca_data->ann_rcpt_timeout_time_interval)),
1554 K_NO_WAIT);
1555 __fallthrough;
1556
1557 case GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT:
1558 if (bmca_data->rcvd_announce_ptr != NULL) {
1559 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1560 bmca_data->rcvd_announce_ptr = NULL;
1561 }
1562
1563 bmca_data->rcvd_msg = false;
1564 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_CURRENT);
1565 break;
1566 }
1567 }
1568
gptp_updt_role_disabled_tree(void)1569 static void gptp_updt_role_disabled_tree(void)
1570 {
1571 struct gptp_global_ds *global_ds;
1572 int port;
1573
1574 global_ds = GPTP_GLOBAL_DS();
1575
1576 /* Set all elements of the selectedRole array to DisabledPort. */
1577 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1578 gptp_change_port_state(port, GPTP_PORT_DISABLED);
1579 }
1580
1581 /* Set lastGmPriority to all ones. */
1582 (void)memset(&global_ds->last_gm_priority, 0xFF,
1583 sizeof(struct gptp_priority_vector));
1584
1585 /* Set pathTrace array to contain the single element thisClock. */
1586 global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN);
1587 memcpy(global_ds->path_trace.path_sequence, GPTP_DEFAULT_DS()->clk_id,
1588 GPTP_CLOCK_ID_LEN);
1589 }
1590
gptp_clear_reselect_tree(void)1591 static void gptp_clear_reselect_tree(void)
1592 {
1593 /* Set all the elements of the reselect array to FALSE. */
1594 GPTP_GLOBAL_DS()->reselect_array = 0;
1595 }
1596
compute_best_vector(void)1597 static int compute_best_vector(void)
1598 {
1599 struct gptp_priority_vector *gm_prio;
1600 struct gptp_default_ds *default_ds;
1601 struct gptp_global_ds *global_ds;
1602 struct gptp_priority_vector *best_vector, *challenger;
1603 int best_port, port, tmp;
1604 struct gptp_pss_rcv_state *pss_rcv;
1605 struct gptp_port_announce_information_state *pa_info_state;
1606
1607 default_ds = GPTP_DEFAULT_DS();
1608 global_ds = GPTP_GLOBAL_DS();
1609 best_port = 0;
1610 gm_prio = &global_ds->gm_priority;
1611
1612 /* Write systemPriority into grandmaster. */
1613 (void)memset(gm_prio, 0, sizeof(struct gptp_priority_vector));
1614 gm_prio->root_system_id.grand_master_prio1 = default_ds->priority1;
1615 gm_prio->root_system_id.grand_master_prio2 = default_ds->priority2;
1616 gm_prio->root_system_id.clk_quality.clock_class =
1617 default_ds->clk_quality.clock_class;
1618 gm_prio->root_system_id.clk_quality.clock_accuracy =
1619 default_ds->clk_quality.clock_accuracy;
1620 gm_prio->root_system_id.clk_quality.offset_scaled_log_var =
1621 htons(default_ds->clk_quality.offset_scaled_log_var);
1622
1623 memcpy(gm_prio->src_port_id.clk_id, default_ds->clk_id,
1624 GPTP_CLOCK_ID_LEN);
1625 memcpy(gm_prio->root_system_id.grand_master_id, default_ds->clk_id,
1626 GPTP_CLOCK_ID_LEN);
1627
1628 best_vector = gm_prio;
1629
1630 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1631 challenger = &GPTP_PORT_BMCA_DATA(port)->port_priority;
1632 pa_info_state = &GPTP_PORT_STATE(port)->pa_info;
1633 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
1634
1635 if (pa_info_state->ann_expired ||
1636 (global_ds->gm_present &&
1637 pss_rcv->rcv_sync_receipt_timeout_timer_expired)) {
1638 continue;
1639 }
1640
1641 if (memcmp(challenger->src_port_id.clk_id, default_ds->clk_id,
1642 GPTP_CLOCK_ID_LEN) == 0) {
1643 /* Discard this challenger. */
1644 continue;
1645 }
1646
1647 if (best_port == 0) {
1648 tmp = memcmp(&challenger->root_system_id,
1649 &best_vector->root_system_id,
1650 sizeof(struct gptp_root_system_identity));
1651 if (tmp < 0) {
1652 best_vector = challenger;
1653 best_port = port;
1654 } else if (tmp > 0) {
1655 continue;
1656 }
1657
1658 tmp = (int)challenger->steps_removed -
1659 ((int)ntohs(best_vector->steps_removed) + 1);
1660 if (tmp < 0) {
1661 best_vector = challenger;
1662 best_port = port;
1663 } else if (tmp > 0) {
1664 continue;
1665 }
1666
1667 tmp = memcmp(&challenger->src_port_id,
1668 &best_vector->src_port_id,
1669 sizeof(struct gptp_port_identity));
1670 if (tmp < 0) {
1671 best_vector = challenger;
1672 best_port = port;
1673 } else if (tmp > 0) {
1674 continue;
1675 }
1676
1677 if (ntohs(challenger->port_number) <
1678 ntohs(best_vector->port_number)) {
1679 best_vector = challenger;
1680 best_port = port;
1681 }
1682
1683 } else {
1684 /* We can compare portPriority vectors without
1685 * calculating pathPriority vectors.
1686 */
1687 if (memcmp(challenger, best_vector,
1688 sizeof(struct gptp_priority_vector)) < 0) {
1689 best_vector = challenger;
1690 best_port = port;
1691 }
1692 }
1693 }
1694
1695 if (best_port != 0) {
1696 if (&global_ds->gm_priority.root_system_id !=
1697 &best_vector->root_system_id) {
1698 memcpy(&global_ds->gm_priority.root_system_id,
1699 &best_vector->root_system_id,
1700 sizeof(struct gptp_root_system_identity));
1701 }
1702
1703 global_ds->gm_priority.steps_removed =
1704 htons(ntohs(best_vector->steps_removed) + 1);
1705
1706 if (&global_ds->gm_priority.src_port_id !=
1707 &best_vector->src_port_id) {
1708 memcpy(&global_ds->gm_priority.src_port_id,
1709 &best_vector->src_port_id,
1710 sizeof(struct gptp_port_identity));
1711 }
1712
1713 global_ds->gm_priority.port_number = best_vector->port_number;
1714 }
1715
1716 return best_port;
1717 }
1718
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)1719 static void update_bmca(int port,
1720 int best_port,
1721 struct gptp_global_ds *global_ds,
1722 struct gptp_default_ds *default_ds,
1723 struct gptp_priority_vector *gm_prio)
1724 {
1725 struct gptp_port_bmca_data *bmca_data = GPTP_PORT_BMCA_DATA(port);
1726
1727 /* Update masterPriorityVector for the port. */
1728 if (best_port == 0) {
1729 memcpy(&bmca_data->master_priority, gm_prio,
1730 sizeof(struct gptp_priority_vector));
1731
1732 bmca_data->master_priority.port_number = htons(port);
1733 bmca_data->master_priority.src_port_id.port_number =
1734 htons(port);
1735 } else {
1736 memcpy(&bmca_data->master_priority.root_system_id,
1737 &gm_prio->root_system_id,
1738 sizeof(struct gptp_root_system_identity));
1739 memcpy(bmca_data->master_priority.src_port_id.clk_id,
1740 default_ds->clk_id, GPTP_CLOCK_ID_LEN);
1741 bmca_data->master_priority.port_number = htons(port);
1742 bmca_data->master_priority.src_port_id.port_number =
1743 htons(port);
1744 }
1745
1746 switch (bmca_data->info_is) {
1747 case GPTP_INFO_IS_DISABLED:
1748 gptp_change_port_state(port, GPTP_PORT_DISABLED);
1749 break;
1750
1751 case GPTP_INFO_IS_AGED:
1752 bmca_data->updt_info = true;
1753 gptp_change_port_state(port, GPTP_PORT_MASTER);
1754 break;
1755
1756 case GPTP_INFO_IS_MINE:
1757 gptp_change_port_state(port, GPTP_PORT_MASTER);
1758
1759 if ((memcmp(&bmca_data->port_priority,
1760 &bmca_data->master_priority,
1761 sizeof(struct gptp_priority_vector)) != 0) ||
1762 (bmca_data->port_steps_removed !=
1763 global_ds->master_steps_removed)) {
1764 bmca_data->updt_info = true;
1765 }
1766
1767 break;
1768
1769 case GPTP_INFO_IS_RECEIVED:
1770 if (best_port == port) {
1771 /* gmPriorityVector is now derived from
1772 * portPriorityVector.
1773 */
1774 gptp_change_port_state(port, GPTP_PORT_SLAVE);
1775 bmca_data->updt_info = false;
1776 } else if (memcmp(&bmca_data->port_priority,
1777 &bmca_data->master_priority,
1778 sizeof(struct gptp_priority_vector)) <= 0) {
1779 /* The masterPriorityVector is not better than
1780 * the portPriorityVector.
1781 */
1782 gptp_change_port_state(port, GPTP_PORT_PASSIVE);
1783
1784 if (memcmp(bmca_data->port_priority.src_port_id.clk_id,
1785 default_ds->clk_id,
1786 GPTP_CLOCK_ID_LEN)) {
1787 /* The sourcePortIdentity component of
1788 * the portPriorityVector does not
1789 * reflect another port on the
1790 * time-aware system.
1791 */
1792 bmca_data->updt_info = true;
1793 } else {
1794 bmca_data->updt_info = false;
1795 }
1796 } else {
1797 gptp_change_port_state(port, GPTP_PORT_MASTER);
1798 bmca_data->updt_info = true;
1799 }
1800
1801 break;
1802 }
1803 }
1804
gptp_updt_roles_tree(void)1805 static void gptp_updt_roles_tree(void)
1806 {
1807 struct gptp_global_ds *global_ds;
1808 struct gptp_default_ds *default_ds;
1809 struct gptp_priority_vector *gm_prio, *last_gm_prio;
1810 struct gptp_port_bmca_data *bmca_data;
1811 int port, best_port;
1812
1813 global_ds = GPTP_GLOBAL_DS();
1814 default_ds = GPTP_DEFAULT_DS();
1815
1816 gm_prio = &global_ds->gm_priority;
1817 last_gm_prio = &global_ds->last_gm_priority;
1818
1819 /* Save gmPriority. */
1820 memcpy(last_gm_prio, gm_prio, sizeof(struct gptp_priority_vector));
1821
1822 best_port = compute_best_vector();
1823
1824 /* If the best vector was the systemPriorityVector. */
1825 if (best_port == 0) {
1826 /* Copy leap61, leap59, current UTC offset valid,
1827 * time traceable and frequency traceable flags.
1828 */
1829 global_ds->global_flags.octets[1] =
1830 global_ds->sys_flags.octets[1];
1831 global_ds->current_utc_offset =
1832 global_ds->sys_current_utc_offset;
1833 global_ds->time_source = global_ds->sys_time_source;
1834 global_ds->master_steps_removed = 0U;
1835 } else {
1836 bmca_data = GPTP_PORT_BMCA_DATA(best_port);
1837
1838 /* Copy leap61, leap59, current UTC offset valid,
1839 * time traceable and frequency traceable flags.
1840 */
1841 global_ds->global_flags.octets[1] =
1842 bmca_data->ann_flags.octets[1];
1843 global_ds->current_utc_offset =
1844 global_ds->sys_current_utc_offset;
1845 global_ds->time_source = bmca_data->ann_time_source;
1846 global_ds->master_steps_removed =
1847 htons(ntohs(bmca_data->message_steps_removed) + 1);
1848 }
1849
1850 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1851 update_bmca(port, best_port, global_ds, default_ds, gm_prio);
1852 }
1853
1854 /* Update gmPresent. */
1855 global_ds->gm_present =
1856 (gm_prio->root_system_id.grand_master_prio1 == 255U) ?
1857 false : true;
1858
1859 /* Assign the port role for port 0. */
1860 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1861 if (global_ds->selected_role[port] == GPTP_PORT_SLAVE) {
1862 gptp_change_port_state(0, GPTP_PORT_PASSIVE);
1863 break;
1864 }
1865 }
1866
1867 if (port == GPTP_PORT_END) {
1868 gptp_change_port_state(0, GPTP_PORT_SLAVE);
1869 }
1870
1871 /* If current system is the Grand Master, set pathTrace array. */
1872 if (memcmp(default_ds->clk_id, gm_prio->root_system_id.grand_master_id,
1873 GPTP_CLOCK_ID_LEN) == 0) {
1874 global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN);
1875 memcpy(global_ds->path_trace.path_sequence,
1876 default_ds->clk_id, GPTP_CLOCK_ID_LEN);
1877 }
1878 }
1879
gptp_set_selected_tree(void)1880 static void gptp_set_selected_tree(void)
1881 {
1882 /* Set all the elements of the selected array to TRUE. */
1883 GPTP_GLOBAL_DS()->selected_array = ~0;
1884 }
1885
gptp_mi_port_role_selection_state_machine(void)1886 static void gptp_mi_port_role_selection_state_machine(void)
1887 {
1888 struct gptp_port_role_selection_state *state;
1889
1890 state = &GPTP_STATE()->pr_sel;
1891
1892 switch (state->state) {
1893 case GPTP_PR_SELECTION_INIT_BRIDGE:
1894 gptp_updt_role_disabled_tree();
1895 state->state = GPTP_PR_SELECTION_ROLE_SELECTION;
1896
1897 /* Be sure to enter the "if" statement immediately after. */
1898 GPTP_GLOBAL_DS()->reselect_array = ~0;
1899 __fallthrough;
1900
1901 case GPTP_PR_SELECTION_ROLE_SELECTION:
1902 if (GPTP_GLOBAL_DS()->reselect_array != 0) {
1903 gptp_clear_reselect_tree();
1904 gptp_updt_roles_tree();
1905 gptp_set_selected_tree();
1906 }
1907
1908 break;
1909 }
1910 }
1911
tx_announce(int port)1912 static void tx_announce(int port)
1913 {
1914 struct net_pkt *pkt;
1915
1916 pkt = gptp_prepare_announce(port);
1917 if (pkt) {
1918 gptp_send_announce(port, pkt);
1919 }
1920 }
1921
gptp_mi_port_announce_transmit_state_machine(int port)1922 static void gptp_mi_port_announce_transmit_state_machine(int port)
1923 {
1924 struct gptp_port_ds *port_ds;
1925 struct gptp_global_ds *global_ds;
1926 struct gptp_port_announce_transmit_state *state;
1927 struct gptp_port_bmca_data *bmca_data;
1928
1929 port_ds = GPTP_PORT_DS(port);
1930 global_ds = GPTP_GLOBAL_DS();
1931 bmca_data = GPTP_PORT_BMCA_DATA(port);
1932 state = &GPTP_PORT_STATE(port)->pa_transmit;
1933
1934 /* Reset interval as defined in AnnounceIntervalSetting
1935 * state machine.
1936 */
1937 if (port_ds->ptt_port_enabled && !port_ds->prev_ptt_port_enabled) {
1938 gptp_update_announce_interval(port, GPTP_ITV_SET_TO_INIT);
1939 }
1940
1941 switch (state->state) {
1942 case GPTP_PA_TRANSMIT_INIT:
1943 bmca_data->new_info = true;
1944 __fallthrough;
1945
1946 case GPTP_PA_TRANSMIT_IDLE:
1947 k_timer_stop(&state->ann_send_periodic_timer);
1948 state->ann_trigger = false;
1949 k_timer_start(&state->ann_send_periodic_timer,
1950 K_MSEC(gptp_uscaled_ns_to_timer_ms(
1951 &bmca_data->announce_interval)),
1952 K_NO_WAIT);
1953
1954 state->state = GPTP_PA_TRANSMIT_POST_IDLE;
1955 __fallthrough;
1956
1957 case GPTP_PA_TRANSMIT_POST_IDLE:
1958 if (IS_SELECTED(global_ds, port) &&
1959 !bmca_data->updt_info &&
1960 state->ann_trigger) {
1961
1962 state->state = GPTP_PA_TRANSMIT_PERIODIC;
1963
1964 } else if (IS_SELECTED(global_ds, port) &&
1965 !bmca_data->updt_info &&
1966 !state->ann_trigger &&
1967 (global_ds->selected_role[port] ==
1968 GPTP_PORT_MASTER) &&
1969 bmca_data->new_info) {
1970
1971 bmca_data->new_info = false;
1972 tx_announce(port);
1973 state->state = GPTP_PA_TRANSMIT_IDLE;
1974 }
1975
1976 break;
1977
1978 case GPTP_PA_TRANSMIT_PERIODIC:
1979 if (global_ds->selected_role[port] == GPTP_PORT_MASTER) {
1980 bmca_data->new_info = true;
1981 }
1982 state->state = GPTP_PA_TRANSMIT_IDLE;
1983 break;
1984 }
1985 }
1986
1987
gptp_mi_port_sync_state_machines(int port)1988 void gptp_mi_port_sync_state_machines(int port)
1989 {
1990 gptp_mi_pss_rcv_state_machine(port);
1991 gptp_mi_pss_send_state_machine(port);
1992 }
1993
gptp_mi_port_bmca_state_machines(int port)1994 void gptp_mi_port_bmca_state_machines(int port)
1995 {
1996 gptp_mi_port_announce_receive_state_machine(port);
1997 gptp_mi_port_announce_information_state_machine(port);
1998 gptp_mi_port_announce_transmit_state_machine(port);
1999 }
2000
gptp_mi_state_machines(void)2001 void gptp_mi_state_machines(void)
2002 {
2003 gptp_mi_site_sync_sync_state_machine();
2004 gptp_mi_clk_slave_sync_state_machine();
2005 gptp_mi_port_role_selection_state_machine();
2006 gptp_mi_clk_master_sync_offset_state_machine();
2007 #if defined(CONFIG_NET_GPTP_GM_CAPABLE)
2008 gptp_mi_clk_master_sync_snd_state_machine();
2009 #endif
2010 gptp_mi_clk_master_sync_rcv_state_machine();
2011 }
2012
