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 correction field properly, rate_ratio is also set to
964 * zero instead of being copied from global_ds as it affects the final
965 * value of FUP correction field.
966 */
967 sync_info->follow_up_correction_field = 0;
968 sync_info->rate_ratio = 0;
969
970 memcpy(&sync_info->src_port_id.clk_id,
971 GPTP_DEFAULT_DS()->clk_id,
972 GPTP_CLOCK_ID_LEN);
973
974 sync_info->src_port_id.port_number = 0U;
975 sync_info->log_msg_interval = CONFIG_NET_GPTP_INIT_LOG_SYNC_ITV;
976 sync_info->upstream_tx_time = global_ds->local_time.low;
977
978 state->pss_snd.sync_receipt_timeout_time = UINT64_MAX;
979
980 sync_info->gm_time_base_indicator =
981 global_ds->clk_src_time_base_indicator;
982
983 memcpy(&sync_info->last_gm_phase_change,
984 &global_ds->clk_src_phase_offset,
985 sizeof(struct gptp_scaled_ns));
986
987 sync_info->last_gm_freq_change = global_ds->clk_src_freq_offset;
988 }
989
gptp_mi_tx_ps_sync_cmss(void)990 static inline void gptp_mi_tx_ps_sync_cmss(void)
991 {
992 struct gptp_clk_master_sync_snd_state *state;
993 struct gptp_pss_send_state *pss_send;
994 int port;
995
996 state = &GPTP_STATE()->clk_master_sync_send;
997
998 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
999 pss_send = &GPTP_PORT_STATE(port)->pss_send;
1000 pss_send->pss_sync_ptr = &state->pss_snd;
1001
1002 pss_send->rcvd_pss_sync = true;
1003 }
1004 }
1005
gptp_mi_clk_master_sync_snd_state_machine(void)1006 static void gptp_mi_clk_master_sync_snd_state_machine(void)
1007 {
1008 struct gptp_clk_master_sync_snd_state *state;
1009 uint64_t current_time;
1010
1011 state = &GPTP_STATE()->clk_master_sync_send;
1012
1013 switch (state->state) {
1014 case GPTP_CMS_SND_INITIALIZING:
1015 gptp_mi_setup_sync_send_time();
1016
1017 state->state = GPTP_CMS_SND_INDICATION;
1018 break;
1019
1020 case GPTP_CMS_SND_INDICATION:
1021 current_time = gptp_get_current_master_time_nanosecond();
1022
1023 if (current_time >= state->sync_send_time.low) {
1024 gptp_mi_set_ps_sync_cmss();
1025 gptp_mi_tx_ps_sync_cmss();
1026
1027 gptp_mi_setup_sync_send_time();
1028 }
1029
1030 break;
1031
1032 default:
1033 NET_ERR("Unrecognised state %d", state->state);
1034 break;
1035 }
1036 }
1037 #endif
1038
gptp_compute_gm_rate_ratio(void)1039 static void gptp_compute_gm_rate_ratio(void)
1040 {
1041 static struct net_ptp_extended_time src_time_0;
1042 static struct gptp_uscaled_ns local_time_0;
1043 struct net_ptp_extended_time src_time_n;
1044 struct gptp_uscaled_ns local_time_n;
1045 struct net_ptp_extended_time src_time_t;
1046 struct gptp_uscaled_ns local_time_t;
1047 struct gptp_clk_master_sync_rcv_state *state;
1048 struct gptp_global_ds *global_ds;
1049 double new_gm_rate;
1050
1051 state = &GPTP_STATE()->clk_master_sync_receive;
1052 global_ds = GPTP_GLOBAL_DS();
1053
1054 /* Get current local and source time */
1055 memcpy(&src_time_n, &state->rcvd_clk_src_req.src_time,
1056 sizeof(struct net_ptp_extended_time));
1057
1058 memcpy(&local_time_n, &global_ds->local_time,
1059 sizeof(struct gptp_uscaled_ns));
1060
1061 if ((src_time_0.second == 0U && src_time_0.fract_nsecond == 0U)
1062 || (local_time_0.high == 0U && local_time_0.low == 0U)) {
1063 memcpy(&src_time_0, &src_time_n,
1064 sizeof(struct net_ptp_extended_time));
1065
1066 memcpy(&local_time_0, &local_time_n,
1067 sizeof(struct gptp_uscaled_ns));
1068
1069 global_ds->gm_rate_ratio = 1.0;
1070
1071 return;
1072 }
1073
1074 /* Take care of the sign of the result */
1075 new_gm_rate = 1.0;
1076
1077 if ((src_time_n.second < src_time_0.second)
1078 || (src_time_n.second == src_time_0.second
1079 && src_time_n.fract_nsecond < src_time_0.fract_nsecond)) {
1080 /* Change result sign and swap src_time_n and src_time_0 */
1081 memcpy(&src_time_t, &src_time_n,
1082 sizeof(struct net_ptp_extended_time));
1083 memcpy(&src_time_n, &src_time_0,
1084 sizeof(struct net_ptp_extended_time));
1085 memcpy(&src_time_0, &src_time_t,
1086 sizeof(struct net_ptp_extended_time));
1087
1088 new_gm_rate *= -1;
1089 }
1090
1091 if ((local_time_n.high < local_time_0.high)
1092 || (local_time_n.high == local_time_0.high
1093 && local_time_n.low < local_time_0.low)) {
1094 /* Change result sign and swap local_time_n and local_time_0 */
1095 memcpy(&local_time_t, &local_time_n,
1096 sizeof(struct gptp_uscaled_ns));
1097 memcpy(&local_time_n, &local_time_0,
1098 sizeof(struct gptp_uscaled_ns));
1099 memcpy(&local_time_0, &local_time_t,
1100 sizeof(struct gptp_uscaled_ns));
1101
1102 new_gm_rate *= -1;
1103 }
1104
1105 /* At this point src_time_n >= src_time_0 */
1106 src_time_n.second -= src_time_0.second;
1107
1108 if (src_time_n.fract_nsecond >= src_time_0.fract_nsecond) {
1109 src_time_n.fract_nsecond -= src_time_0.fract_nsecond;
1110 } else {
1111 src_time_n.second -= 1U;
1112 src_time_n.fract_nsecond = (NSEC_PER_SEC * GPTP_POW2_16)
1113 - src_time_0.fract_nsecond;
1114 }
1115
1116 /* At this point local_time_n >= local_time_0 */
1117 local_time_n.high -= local_time_0.high;
1118
1119 if (local_time_n.low >= local_time_0.low) {
1120 local_time_n.low -= local_time_0.low;
1121 } else {
1122 local_time_n.high -= 1U;
1123 local_time_n.low = UINT64_MAX - local_time_0.low;
1124 }
1125
1126 /* Calculate it in nanoseconds, new_gm_rate is either 1 or -1 here */
1127 new_gm_rate *= ((src_time_n.second * NSEC_PER_SEC)
1128 + (src_time_n.fract_nsecond / GPTP_POW2_16));
1129
1130 new_gm_rate /= local_time_n.low;
1131
1132 global_ds->gm_rate_ratio = new_gm_rate;
1133 }
1134
gptp_mi_clk_master_sync_rcv_state_machine(void)1135 static void gptp_mi_clk_master_sync_rcv_state_machine(void)
1136 {
1137 struct gptp_clk_master_sync_rcv_state *s;
1138 struct gptp_global_ds *global_ds;
1139
1140 #ifdef CONFIG_NET_GPTP_PROBE_CLOCK_SOURCE_ON_DEMAND
1141 struct gptp_clk_src_time_invoke_params invoke_args = {};
1142 uint64_t cur = gptp_get_current_master_time_nanosecond();
1143
1144 invoke_args.src_time.second = cur / NSEC_PER_SEC;
1145 cur -= (invoke_args.src_time.second * NSEC_PER_SEC);
1146
1147 invoke_args.src_time.fract_nsecond = cur * GPTP_POW2_16;
1148
1149 memset(&invoke_args.last_gm_phase_change, 0x0,
1150 sizeof(struct gptp_scaled_ns));
1151 invoke_args.last_gm_freq_change = 0;
1152
1153 gptp_clk_src_time_invoke(&invoke_args);
1154 #endif
1155
1156 global_ds = GPTP_GLOBAL_DS();
1157
1158 s = &GPTP_STATE()->clk_master_sync_receive;
1159 switch (s->state) {
1160 case GPTP_CMS_RCV_INITIALIZING:
1161 s->state = GPTP_CMS_RCV_WAITING;
1162 break;
1163
1164 case GPTP_CMS_RCV_WAITING:
1165 if (s->rcvd_clock_source_req || s->rcvd_local_clock_tick) {
1166 s->state = GPTP_CMS_RCV_SOURCE_TIME;
1167 }
1168
1169 break;
1170
1171 case GPTP_CMS_RCV_SOURCE_TIME:
1172 global_ds->local_time.high = 0U;
1173 global_ds->local_time.low =
1174 gptp_get_current_master_time_nanosecond();
1175
1176 if (s->rcvd_clock_source_req) {
1177 gptp_compute_gm_rate_ratio();
1178
1179 global_ds->clk_src_time_base_indicator_prev =
1180 global_ds->clk_src_time_base_indicator;
1181
1182 global_ds->clk_src_time_base_indicator =
1183 s->rcvd_clk_src_req.time_base_indicator;
1184
1185 memcpy(&global_ds->clk_src_last_gm_phase_change,
1186 &s->rcvd_clk_src_req.last_gm_phase_change,
1187 sizeof(struct gptp_scaled_ns));
1188
1189 global_ds->clk_src_last_gm_freq_change =
1190 s->rcvd_clk_src_req.last_gm_freq_change;
1191 }
1192
1193 s->rcvd_clock_source_req = false;
1194 s->rcvd_local_clock_tick = false;
1195 s->state = GPTP_CMS_RCV_WAITING;
1196 break;
1197
1198 default:
1199 NET_ERR("Unrecognised state %d", s->state);
1200 break;
1201 }
1202 }
1203
copy_path_trace(struct gptp_announce * announce)1204 static void copy_path_trace(struct gptp_announce *announce)
1205 {
1206 int len = ntohs(announce->tlv.len);
1207 struct gptp_path_trace *sys_path_trace;
1208
1209 if (len > GPTP_MAX_PATHTRACE_SIZE) {
1210 NET_ERR("Too long path trace (%d vs %d)",
1211 GPTP_MAX_PATHTRACE_SIZE, len);
1212 return;
1213 }
1214
1215 sys_path_trace = &GPTP_GLOBAL_DS()->path_trace;
1216
1217 sys_path_trace->len = htons(len + GPTP_CLOCK_ID_LEN);
1218
1219 memcpy(sys_path_trace->path_sequence, announce->tlv.path_sequence,
1220 len);
1221
1222 /* Append local clockIdentity. */
1223 memcpy((uint8_t *)sys_path_trace->path_sequence + len,
1224 GPTP_DEFAULT_DS()->clk_id, GPTP_CLOCK_ID_LEN);
1225 }
1226
gptp_mi_qualify_announce(int port,struct net_pkt * announce_msg)1227 static bool gptp_mi_qualify_announce(int port, struct net_pkt *announce_msg)
1228 {
1229 struct gptp_announce *announce;
1230 struct gptp_hdr *hdr;
1231 int i;
1232 uint16_t len;
1233
1234 hdr = GPTP_HDR(announce_msg);
1235 announce = GPTP_ANNOUNCE(announce_msg);
1236
1237 if (memcmp(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id,
1238 GPTP_CLOCK_ID_LEN) == 0) {
1239 return false;
1240 }
1241
1242 len = ntohs(announce->steps_removed);
1243 if (len >= 255U) {
1244 return false;
1245 }
1246
1247 for (i = 0; i < len + 1; i++) {
1248 if (memcmp(announce->tlv.path_sequence[i],
1249 GPTP_DEFAULT_DS()->clk_id,
1250 GPTP_CLOCK_ID_LEN) == 0) {
1251 return false;
1252 }
1253 }
1254
1255 if (GPTP_GLOBAL_DS()->selected_role[port] == GPTP_PORT_SLAVE) {
1256 copy_path_trace(announce);
1257 }
1258
1259 return true;
1260 }
1261
gptp_mi_port_announce_receive_state_machine(int port)1262 static void gptp_mi_port_announce_receive_state_machine(int port)
1263 {
1264 struct gptp_port_ds *port_ds;
1265 struct gptp_port_announce_receive_state *state;
1266 struct gptp_port_bmca_data *bmca_data;
1267
1268 state = &GPTP_PORT_STATE(port)->pa_rcv;
1269 port_ds = GPTP_PORT_DS(port);
1270 bmca_data = GPTP_PORT_BMCA_DATA(port);
1271
1272 if ((!port_ds->ptt_port_enabled) || (!port_ds->as_capable)) {
1273 state->state = GPTP_PA_RCV_DISCARD;
1274 }
1275
1276 switch (state->state) {
1277 case GPTP_PA_RCV_DISCARD:
1278 state->rcvd_announce = false;
1279 bmca_data->rcvd_msg = false;
1280 if (bmca_data->rcvd_announce_ptr != NULL) {
1281 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1282 bmca_data->rcvd_announce_ptr = NULL;
1283 }
1284
1285 state->state = GPTP_PA_RCV_RECEIVE;
1286 break;
1287
1288 case GPTP_PA_RCV_RECEIVE:
1289 /* "portEnabled" is not checked: the interface is always up. */
1290 if (state->rcvd_announce &&
1291 port_ds->ptt_port_enabled &&
1292 port_ds->as_capable &&
1293 !bmca_data->rcvd_msg) {
1294 state->rcvd_announce = false;
1295
1296 bmca_data->rcvd_msg = gptp_mi_qualify_announce(
1297 port, bmca_data->rcvd_announce_ptr);
1298 if (!bmca_data->rcvd_msg) {
1299 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1300 bmca_data->rcvd_announce_ptr = NULL;
1301 }
1302 }
1303
1304 break;
1305 }
1306 }
1307
1308 /*
1309 * Compare a vector to an announce message vector.
1310 * All must be in big endian (network) order.
1311 */
compare_priority_vectors(struct gptp_priority_vector * vector,struct net_pkt * pkt,int port)1312 static enum gptp_received_info compare_priority_vectors(
1313 struct gptp_priority_vector *vector,
1314 struct net_pkt *pkt, int port)
1315 {
1316 struct gptp_hdr *hdr;
1317 struct gptp_announce *announce;
1318 int rsi_cmp, spi_cmp, port_cmp;
1319
1320 hdr = GPTP_HDR(pkt);
1321 announce = GPTP_ANNOUNCE(pkt);
1322
1323 /* Compare rootSystemIdentity and stepsRemoved. */
1324 rsi_cmp = memcmp(&announce->root_system_id,
1325 &vector->root_system_id,
1326 sizeof(struct gptp_root_system_identity) +
1327 sizeof(uint16_t));
1328 if (rsi_cmp < 0) {
1329 /* Better rootSystemIdentity. */
1330 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1331 }
1332
1333 /* Compare sourcePortIdentity. */
1334 spi_cmp = memcmp(&hdr->port_id, &vector->src_port_id,
1335 sizeof(struct gptp_port_identity));
1336
1337 port_cmp = (int)port - ntohs(vector->port_number);
1338
1339 if (spi_cmp == 0) {
1340 if (rsi_cmp == 0) {
1341 if (port_cmp == 0) {
1342 /* Same priority vector. */
1343 return GPTP_RCVD_INFO_REPEATED_MASTER_INFO;
1344 } else if (port_cmp < 0) {
1345 /* Priority vector with better reception port
1346 * number.
1347 */
1348 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1349 }
1350 } else {
1351 /* Same master port but different Grand Master. */
1352 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1353 }
1354 } else if ((spi_cmp < 0) && (rsi_cmp == 0)) {
1355 /* Same Grand Master but better masterPort. */
1356 return GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO;
1357 }
1358
1359 return GPTP_RCVD_INFO_INFERIOR_MASTER_INFO;
1360 }
1361
rcv_info(int port)1362 static enum gptp_received_info rcv_info(int port)
1363 {
1364 /* TODO
1365 * How can we define that a message does not convey the port
1366 * role Master port ?
1367 * It is needed to define that to be able to send
1368 * GPTP_RCVD_INFO_OTHER_INFO.
1369 */
1370 struct gptp_port_bmca_data *bmca_data;
1371 struct gptp_announce *announce;
1372
1373 bmca_data = GPTP_PORT_BMCA_DATA(port);
1374 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1375
1376 bmca_data->message_steps_removed = announce->steps_removed;
1377
1378 return compare_priority_vectors(&bmca_data->port_priority,
1379 bmca_data->rcvd_announce_ptr,
1380 port);
1381 }
1382
record_other_announce_info(int port)1383 static void record_other_announce_info(int port)
1384 {
1385 struct gptp_hdr *hdr;
1386 struct gptp_announce *announce;
1387 struct gptp_port_bmca_data *bmca_data;
1388
1389 bmca_data = GPTP_PORT_BMCA_DATA(port);
1390 hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr);
1391 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1392
1393 /* Copy leap61, leap59, current UTC offset valid, time traceable and
1394 * frequency traceable flags.
1395 */
1396 bmca_data->ann_flags.octets[1] = hdr->flags.octets[1];
1397
1398 bmca_data->ann_current_utc_offset = ntohs(announce->cur_utc_offset);
1399 bmca_data->ann_time_source = announce->time_source;
1400 }
1401
copy_priority_vector(struct gptp_priority_vector * vector,struct net_pkt * pkt,int port)1402 static void copy_priority_vector(struct gptp_priority_vector *vector,
1403 struct net_pkt *pkt, int port)
1404 {
1405 struct gptp_hdr *hdr;
1406 struct gptp_announce *announce;
1407
1408 hdr = GPTP_HDR(pkt);
1409 announce = GPTP_ANNOUNCE(pkt);
1410
1411 memcpy(&vector->root_system_id, &announce->root_system_id,
1412 sizeof(struct gptp_root_system_identity) + sizeof(uint16_t));
1413
1414 memcpy(&vector->src_port_id, &hdr->port_id,
1415 sizeof(struct gptp_port_identity));
1416
1417 vector->port_number = htons(port);
1418 }
1419
gptp_mi_port_announce_information_state_machine(int port)1420 static void gptp_mi_port_announce_information_state_machine(int port)
1421 {
1422 struct gptp_port_ds *port_ds;
1423 struct gptp_global_ds *global_ds;
1424 struct gptp_port_announce_information_state *state;
1425 struct gptp_announce *announce;
1426 struct gptp_hdr *hdr;
1427 struct gptp_port_bmca_data *bmca_data;
1428 struct gptp_pss_rcv_state *pss_rcv;
1429
1430 bmca_data = GPTP_PORT_BMCA_DATA(port);
1431 state = &GPTP_PORT_STATE(port)->pa_info;
1432 port_ds = GPTP_PORT_DS(port);
1433 global_ds = GPTP_GLOBAL_DS();
1434
1435 if ((!port_ds->ptt_port_enabled || !port_ds->as_capable) &&
1436 (bmca_data->info_is != GPTP_INFO_IS_DISABLED)) {
1437 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_DISABLED);
1438 }
1439
1440 switch (state->state) {
1441 case GPTP_PA_INFO_DISABLED:
1442 bmca_data->rcvd_msg = false;
1443 bmca_data->info_is = GPTP_INFO_IS_DISABLED;
1444 SET_RESELECT(global_ds, port);
1445 CLEAR_SELECTED(global_ds, port);
1446 gptp_change_pa_info_state(port, state,
1447 GPTP_PA_INFO_POST_DISABLED);
1448 k_timer_stop(&state->ann_rcpt_expiry_timer);
1449 state->ann_expired = true;
1450 __fallthrough;
1451
1452 case GPTP_PA_INFO_POST_DISABLED:
1453 if (port_ds->ptt_port_enabled && port_ds->as_capable) {
1454 gptp_change_pa_info_state(port, state,
1455 GPTP_PA_INFO_AGED);
1456 } else if (bmca_data->rcvd_msg) {
1457 gptp_change_pa_info_state(port, state,
1458 GPTP_PA_INFO_DISABLED);
1459 }
1460
1461 break;
1462
1463 case GPTP_PA_INFO_AGED:
1464 bmca_data->info_is = GPTP_INFO_IS_AGED;
1465 CLEAR_SELECTED(global_ds, port);
1466 SET_RESELECT(global_ds, port);
1467 /* Transition will be actually tested in UPDATE state. */
1468 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_UPDATE);
1469 break;
1470
1471 case GPTP_PA_INFO_UPDATE:
1472 if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) {
1473 memcpy(&bmca_data->port_priority,
1474 &bmca_data->master_priority,
1475 sizeof(struct gptp_priority_vector));
1476
1477 bmca_data->port_steps_removed =
1478 global_ds->master_steps_removed;
1479 bmca_data->updt_info = false;
1480 bmca_data->info_is = GPTP_INFO_IS_MINE;
1481 bmca_data->new_info = true;
1482 gptp_change_pa_info_state(port, state,
1483 GPTP_PA_INFO_CURRENT);
1484 }
1485
1486 break;
1487
1488 case GPTP_PA_INFO_CURRENT:
1489 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
1490 if (IS_SELECTED(global_ds, port) && bmca_data->updt_info) {
1491 gptp_change_pa_info_state(port, state,
1492 GPTP_PA_INFO_UPDATE);
1493 } else if (bmca_data->rcvd_msg && !bmca_data->updt_info) {
1494 gptp_change_pa_info_state(port, state,
1495 GPTP_PA_INFO_RECEIVE);
1496 } else if ((bmca_data->info_is == GPTP_INFO_IS_RECEIVED) &&
1497 !bmca_data->updt_info &&
1498 !bmca_data->rcvd_msg &&
1499 (state->ann_expired ||
1500 (global_ds->gm_present &&
1501 pss_rcv->rcv_sync_receipt_timeout_timer_expired))) {
1502 gptp_change_pa_info_state(port, state,
1503 GPTP_PA_INFO_AGED);
1504 }
1505
1506 break;
1507
1508 case GPTP_PA_INFO_RECEIVE:
1509 switch (rcv_info(port)) {
1510 case GPTP_RCVD_INFO_SUPERIOR_MASTER_INFO:
1511 gptp_change_pa_info_state(port, state,
1512 GPTP_PA_INFO_SUPERIOR_MASTER_PORT);
1513 break;
1514 case GPTP_RCVD_INFO_REPEATED_MASTER_INFO:
1515 gptp_change_pa_info_state(port, state,
1516 GPTP_PA_INFO_REPEATED_MASTER_PORT);
1517 break;
1518 case GPTP_RCVD_INFO_INFERIOR_MASTER_INFO:
1519 __fallthrough;
1520 case GPTP_RCVD_INFO_OTHER_INFO:
1521 gptp_change_pa_info_state(port, state,
1522 GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT);
1523 break;
1524 }
1525
1526 break;
1527
1528 case GPTP_PA_INFO_SUPERIOR_MASTER_PORT:
1529 /* We copy directly the content of the message to the port
1530 * priority vector without using an intermediate
1531 * messagePriority structure.
1532 */
1533
1534 if (!bmca_data->rcvd_announce_ptr) {
1535 /* Shouldn't be reached. Checked for safety reason. */
1536 bmca_data->rcvd_msg = false;
1537 gptp_change_pa_info_state(port, state,
1538 GPTP_PA_INFO_CURRENT);
1539 break;
1540 }
1541
1542 copy_priority_vector(&bmca_data->port_priority,
1543 bmca_data->rcvd_announce_ptr, port);
1544
1545 announce = GPTP_ANNOUNCE(bmca_data->rcvd_announce_ptr);
1546 bmca_data->port_steps_removed = ntohs(announce->steps_removed);
1547 record_other_announce_info(port);
1548 hdr = GPTP_HDR(bmca_data->rcvd_announce_ptr);
1549 gptp_set_time_itv(&bmca_data->ann_rcpt_timeout_time_interval,
1550 port_ds->announce_receipt_timeout,
1551 hdr->log_msg_interval);
1552 bmca_data->info_is = GPTP_INFO_IS_RECEIVED;
1553 CLEAR_SELECTED(global_ds, port);
1554 SET_RESELECT(global_ds, port);
1555 __fallthrough;
1556
1557 case GPTP_PA_INFO_REPEATED_MASTER_PORT:
1558 k_timer_stop(&state->ann_rcpt_expiry_timer);
1559 state->ann_expired = false;
1560 k_timer_start(&state->ann_rcpt_expiry_timer,
1561 K_MSEC(gptp_uscaled_ns_to_timer_ms(
1562 &bmca_data->ann_rcpt_timeout_time_interval)),
1563 K_NO_WAIT);
1564 __fallthrough;
1565
1566 case GPTP_PA_INFO_INFERIOR_MASTER_OR_OTHER_PORT:
1567 if (bmca_data->rcvd_announce_ptr != NULL) {
1568 net_pkt_unref(bmca_data->rcvd_announce_ptr);
1569 bmca_data->rcvd_announce_ptr = NULL;
1570 }
1571
1572 bmca_data->rcvd_msg = false;
1573 gptp_change_pa_info_state(port, state, GPTP_PA_INFO_CURRENT);
1574 break;
1575 }
1576 }
1577
gptp_updt_role_disabled_tree(void)1578 static void gptp_updt_role_disabled_tree(void)
1579 {
1580 struct gptp_global_ds *global_ds;
1581 int port;
1582
1583 global_ds = GPTP_GLOBAL_DS();
1584
1585 /* Set all elements of the selectedRole array to DisabledPort. */
1586 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1587 gptp_change_port_state(port, GPTP_PORT_DISABLED);
1588 }
1589
1590 /* Set lastGmPriority to all ones. */
1591 (void)memset(&global_ds->last_gm_priority, 0xFF,
1592 sizeof(struct gptp_priority_vector));
1593
1594 /* Set pathTrace array to contain the single element thisClock. */
1595 global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN);
1596 memcpy(global_ds->path_trace.path_sequence, GPTP_DEFAULT_DS()->clk_id,
1597 GPTP_CLOCK_ID_LEN);
1598 }
1599
gptp_clear_reselect_tree(void)1600 static void gptp_clear_reselect_tree(void)
1601 {
1602 /* Set all the elements of the reselect array to FALSE. */
1603 GPTP_GLOBAL_DS()->reselect_array = 0;
1604 }
1605
compute_best_vector(void)1606 static int compute_best_vector(void)
1607 {
1608 struct gptp_priority_vector *gm_prio;
1609 struct gptp_default_ds *default_ds;
1610 struct gptp_global_ds *global_ds;
1611 struct gptp_priority_vector *best_vector, *challenger;
1612 int best_port, port, tmp;
1613 struct gptp_pss_rcv_state *pss_rcv;
1614 struct gptp_port_announce_information_state *pa_info_state;
1615
1616 default_ds = GPTP_DEFAULT_DS();
1617 global_ds = GPTP_GLOBAL_DS();
1618 best_port = 0;
1619 gm_prio = &global_ds->gm_priority;
1620
1621 /* Write systemPriority into grandmaster. */
1622 (void)memset(gm_prio, 0, sizeof(struct gptp_priority_vector));
1623 gm_prio->root_system_id.grand_master_prio1 = default_ds->priority1;
1624 gm_prio->root_system_id.grand_master_prio2 = default_ds->priority2;
1625 gm_prio->root_system_id.clk_quality.clock_class =
1626 default_ds->clk_quality.clock_class;
1627 gm_prio->root_system_id.clk_quality.clock_accuracy =
1628 default_ds->clk_quality.clock_accuracy;
1629 gm_prio->root_system_id.clk_quality.offset_scaled_log_var =
1630 htons(default_ds->clk_quality.offset_scaled_log_var);
1631
1632 memcpy(gm_prio->src_port_id.clk_id, default_ds->clk_id,
1633 GPTP_CLOCK_ID_LEN);
1634 memcpy(gm_prio->root_system_id.grand_master_id, default_ds->clk_id,
1635 GPTP_CLOCK_ID_LEN);
1636
1637 best_vector = gm_prio;
1638
1639 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1640 challenger = &GPTP_PORT_BMCA_DATA(port)->port_priority;
1641 pa_info_state = &GPTP_PORT_STATE(port)->pa_info;
1642 pss_rcv = &GPTP_PORT_STATE(port)->pss_rcv;
1643
1644 if (pa_info_state->ann_expired ||
1645 (global_ds->gm_present &&
1646 pss_rcv->rcv_sync_receipt_timeout_timer_expired)) {
1647 continue;
1648 }
1649
1650 if (memcmp(challenger->src_port_id.clk_id, default_ds->clk_id,
1651 GPTP_CLOCK_ID_LEN) == 0) {
1652 /* Discard this challenger. */
1653 continue;
1654 }
1655
1656 if (best_port == 0) {
1657 tmp = memcmp(&challenger->root_system_id,
1658 &best_vector->root_system_id,
1659 sizeof(struct gptp_root_system_identity));
1660 if (tmp < 0) {
1661 best_vector = challenger;
1662 best_port = port;
1663 } else if (tmp > 0) {
1664 continue;
1665 }
1666
1667 tmp = (int)challenger->steps_removed -
1668 ((int)ntohs(best_vector->steps_removed) + 1);
1669 if (tmp < 0) {
1670 best_vector = challenger;
1671 best_port = port;
1672 } else if (tmp > 0) {
1673 continue;
1674 }
1675
1676 tmp = memcmp(&challenger->src_port_id,
1677 &best_vector->src_port_id,
1678 sizeof(struct gptp_port_identity));
1679 if (tmp < 0) {
1680 best_vector = challenger;
1681 best_port = port;
1682 } else if (tmp > 0) {
1683 continue;
1684 }
1685
1686 if (ntohs(challenger->port_number) <
1687 ntohs(best_vector->port_number)) {
1688 best_vector = challenger;
1689 best_port = port;
1690 }
1691
1692 } else {
1693 /* We can compare portPriority vectors without
1694 * calculating pathPriority vectors.
1695 */
1696 if (memcmp(challenger, best_vector,
1697 sizeof(struct gptp_priority_vector)) < 0) {
1698 best_vector = challenger;
1699 best_port = port;
1700 }
1701 }
1702 }
1703
1704 if (best_port != 0) {
1705 if (&global_ds->gm_priority.root_system_id !=
1706 &best_vector->root_system_id) {
1707 memcpy(&global_ds->gm_priority.root_system_id,
1708 &best_vector->root_system_id,
1709 sizeof(struct gptp_root_system_identity));
1710 }
1711
1712 global_ds->gm_priority.steps_removed =
1713 ntohs(best_vector->steps_removed) + 1;
1714
1715 if (&global_ds->gm_priority.src_port_id !=
1716 &best_vector->src_port_id) {
1717 memcpy(&global_ds->gm_priority.src_port_id,
1718 &best_vector->src_port_id,
1719 sizeof(struct gptp_port_identity));
1720 }
1721
1722 global_ds->gm_priority.port_number = best_vector->port_number;
1723 }
1724
1725 return best_port;
1726 }
1727
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)1728 static void update_bmca(int port,
1729 int best_port,
1730 struct gptp_global_ds *global_ds,
1731 struct gptp_default_ds *default_ds,
1732 struct gptp_priority_vector *gm_prio)
1733 {
1734 struct gptp_port_bmca_data *bmca_data = GPTP_PORT_BMCA_DATA(port);
1735
1736 /* Update masterPriorityVector for the port. */
1737 if (best_port == 0) {
1738 memcpy(&bmca_data->master_priority, gm_prio,
1739 sizeof(struct gptp_priority_vector));
1740
1741 bmca_data->master_priority.port_number = htons(port);
1742 bmca_data->master_priority.src_port_id.port_number =
1743 htons(port);
1744 } else {
1745 memcpy(&bmca_data->master_priority.root_system_id,
1746 &gm_prio->root_system_id,
1747 sizeof(struct gptp_root_system_identity));
1748 memcpy(bmca_data->master_priority.src_port_id.clk_id,
1749 default_ds->clk_id, GPTP_CLOCK_ID_LEN);
1750 bmca_data->master_priority.port_number = htons(port);
1751 bmca_data->master_priority.src_port_id.port_number =
1752 htons(port);
1753 }
1754
1755 switch (bmca_data->info_is) {
1756 case GPTP_INFO_IS_DISABLED:
1757 gptp_change_port_state(port, GPTP_PORT_DISABLED);
1758 break;
1759
1760 case GPTP_INFO_IS_AGED:
1761 bmca_data->updt_info = true;
1762 gptp_change_port_state(port, GPTP_PORT_MASTER);
1763 break;
1764
1765 case GPTP_INFO_IS_MINE:
1766 gptp_change_port_state(port, GPTP_PORT_MASTER);
1767
1768 if ((memcmp(&bmca_data->port_priority,
1769 &bmca_data->master_priority,
1770 sizeof(struct gptp_priority_vector)) != 0) ||
1771 (bmca_data->port_steps_removed !=
1772 global_ds->master_steps_removed)) {
1773 bmca_data->updt_info = true;
1774 }
1775
1776 break;
1777
1778 case GPTP_INFO_IS_RECEIVED:
1779 if (best_port == port) {
1780 /* gmPriorityVector is now derived from
1781 * portPriorityVector.
1782 */
1783 gptp_change_port_state(port, GPTP_PORT_SLAVE);
1784 bmca_data->updt_info = false;
1785 } else if (memcmp(&bmca_data->port_priority,
1786 &bmca_data->master_priority,
1787 sizeof(struct gptp_priority_vector)) <= 0) {
1788 /* The masterPriorityVector is not better than
1789 * the portPriorityVector.
1790 */
1791 gptp_change_port_state(port, GPTP_PORT_PASSIVE);
1792
1793 if (memcmp(bmca_data->port_priority.src_port_id.clk_id,
1794 default_ds->clk_id,
1795 GPTP_CLOCK_ID_LEN)) {
1796 /* The sourcePortIdentity component of
1797 * the portPriorityVector does not
1798 * reflect another port on the
1799 * time-aware system.
1800 */
1801 bmca_data->updt_info = true;
1802 } else {
1803 bmca_data->updt_info = false;
1804 }
1805 } else {
1806 gptp_change_port_state(port, GPTP_PORT_MASTER);
1807 bmca_data->updt_info = true;
1808 }
1809
1810 break;
1811 }
1812 }
1813
gptp_updt_roles_tree(void)1814 static void gptp_updt_roles_tree(void)
1815 {
1816 struct gptp_global_ds *global_ds;
1817 struct gptp_default_ds *default_ds;
1818 struct gptp_priority_vector *gm_prio, *last_gm_prio;
1819 struct gptp_port_bmca_data *bmca_data;
1820 int port, best_port;
1821
1822 global_ds = GPTP_GLOBAL_DS();
1823 default_ds = GPTP_DEFAULT_DS();
1824
1825 gm_prio = &global_ds->gm_priority;
1826 last_gm_prio = &global_ds->last_gm_priority;
1827
1828 /* Save gmPriority. */
1829 memcpy(last_gm_prio, gm_prio, sizeof(struct gptp_priority_vector));
1830
1831 best_port = compute_best_vector();
1832
1833 /* If the best vector was the systemPriorityVector. */
1834 if (best_port == 0) {
1835 /* Copy leap61, leap59, current UTC offset valid,
1836 * time traceable and frequency traceable flags.
1837 */
1838 global_ds->global_flags.octets[1] =
1839 global_ds->sys_flags.octets[1];
1840 global_ds->current_utc_offset =
1841 global_ds->sys_current_utc_offset;
1842 global_ds->time_source = global_ds->sys_time_source;
1843 global_ds->master_steps_removed = 0U;
1844 } else {
1845 bmca_data = GPTP_PORT_BMCA_DATA(best_port);
1846
1847 /* Copy leap61, leap59, current UTC offset valid,
1848 * time traceable and frequency traceable flags.
1849 */
1850 global_ds->global_flags.octets[1] =
1851 bmca_data->ann_flags.octets[1];
1852 global_ds->current_utc_offset =
1853 global_ds->sys_current_utc_offset;
1854 global_ds->time_source = bmca_data->ann_time_source;
1855 global_ds->master_steps_removed =
1856 htons(ntohs(bmca_data->message_steps_removed) + 1);
1857 }
1858
1859 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1860 update_bmca(port, best_port, global_ds, default_ds, gm_prio);
1861 }
1862
1863 /* Update gmPresent. */
1864 global_ds->gm_present =
1865 (gm_prio->root_system_id.grand_master_prio1 == 255U) ?
1866 false : true;
1867
1868 /* Assign the port role for port 0. */
1869 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
1870 if (global_ds->selected_role[port] == GPTP_PORT_SLAVE) {
1871 gptp_change_port_state(0, GPTP_PORT_PASSIVE);
1872 break;
1873 }
1874 }
1875
1876 if (port == GPTP_PORT_END) {
1877 gptp_change_port_state(0, GPTP_PORT_SLAVE);
1878 }
1879
1880 /* If current system is the Grand Master, set pathTrace array. */
1881 if (memcmp(default_ds->clk_id, gm_prio->root_system_id.grand_master_id,
1882 GPTP_CLOCK_ID_LEN) == 0) {
1883 global_ds->path_trace.len = htons(GPTP_CLOCK_ID_LEN);
1884 memcpy(global_ds->path_trace.path_sequence,
1885 default_ds->clk_id, GPTP_CLOCK_ID_LEN);
1886 }
1887 }
1888
gptp_set_selected_tree(void)1889 static void gptp_set_selected_tree(void)
1890 {
1891 /* Set all the elements of the selected array to TRUE. */
1892 GPTP_GLOBAL_DS()->selected_array = ~0;
1893 }
1894
gptp_mi_port_role_selection_state_machine(void)1895 static void gptp_mi_port_role_selection_state_machine(void)
1896 {
1897 struct gptp_port_role_selection_state *state;
1898
1899 state = &GPTP_STATE()->pr_sel;
1900
1901 switch (state->state) {
1902 case GPTP_PR_SELECTION_INIT_BRIDGE:
1903 gptp_updt_role_disabled_tree();
1904 state->state = GPTP_PR_SELECTION_ROLE_SELECTION;
1905
1906 /* Be sure to enter the "if" statement immediately after. */
1907 GPTP_GLOBAL_DS()->reselect_array = ~0;
1908 __fallthrough;
1909
1910 case GPTP_PR_SELECTION_ROLE_SELECTION:
1911 if (GPTP_GLOBAL_DS()->reselect_array != 0) {
1912 gptp_clear_reselect_tree();
1913 gptp_updt_roles_tree();
1914 gptp_set_selected_tree();
1915 }
1916
1917 break;
1918 }
1919 }
1920
tx_announce(int port)1921 static void tx_announce(int port)
1922 {
1923 struct net_pkt *pkt;
1924
1925 pkt = gptp_prepare_announce(port);
1926 if (pkt) {
1927 gptp_send_announce(port, pkt);
1928 }
1929 }
1930
gptp_mi_port_announce_transmit_state_machine(int port)1931 static void gptp_mi_port_announce_transmit_state_machine(int port)
1932 {
1933 struct gptp_port_ds *port_ds;
1934 struct gptp_global_ds *global_ds;
1935 struct gptp_port_announce_transmit_state *state;
1936 struct gptp_port_bmca_data *bmca_data;
1937
1938 port_ds = GPTP_PORT_DS(port);
1939 global_ds = GPTP_GLOBAL_DS();
1940 bmca_data = GPTP_PORT_BMCA_DATA(port);
1941 state = &GPTP_PORT_STATE(port)->pa_transmit;
1942
1943 /* Reset interval as defined in AnnounceIntervalSetting
1944 * state machine.
1945 */
1946 if (port_ds->ptt_port_enabled && !port_ds->prev_ptt_port_enabled) {
1947 gptp_update_announce_interval(port, GPTP_ITV_SET_TO_INIT);
1948 }
1949
1950 switch (state->state) {
1951 case GPTP_PA_TRANSMIT_INIT:
1952 bmca_data->new_info = true;
1953 __fallthrough;
1954
1955 case GPTP_PA_TRANSMIT_IDLE:
1956 k_timer_stop(&state->ann_send_periodic_timer);
1957 state->ann_trigger = false;
1958 k_timer_start(&state->ann_send_periodic_timer,
1959 K_MSEC(gptp_uscaled_ns_to_timer_ms(
1960 &bmca_data->announce_interval)),
1961 K_NO_WAIT);
1962
1963 state->state = GPTP_PA_TRANSMIT_POST_IDLE;
1964 __fallthrough;
1965
1966 case GPTP_PA_TRANSMIT_POST_IDLE:
1967 if (IS_SELECTED(global_ds, port) &&
1968 !bmca_data->updt_info &&
1969 state->ann_trigger) {
1970
1971 state->state = GPTP_PA_TRANSMIT_PERIODIC;
1972
1973 } else if (IS_SELECTED(global_ds, port) &&
1974 !bmca_data->updt_info &&
1975 !state->ann_trigger &&
1976 (global_ds->selected_role[port] ==
1977 GPTP_PORT_MASTER) &&
1978 bmca_data->new_info) {
1979
1980 bmca_data->new_info = false;
1981 tx_announce(port);
1982 state->state = GPTP_PA_TRANSMIT_IDLE;
1983 }
1984
1985 break;
1986
1987 case GPTP_PA_TRANSMIT_PERIODIC:
1988 if (global_ds->selected_role[port] == GPTP_PORT_MASTER) {
1989 bmca_data->new_info = true;
1990 }
1991 state->state = GPTP_PA_TRANSMIT_IDLE;
1992 break;
1993 }
1994 }
1995
1996
gptp_mi_port_sync_state_machines(int port)1997 void gptp_mi_port_sync_state_machines(int port)
1998 {
1999 gptp_mi_pss_rcv_state_machine(port);
2000 gptp_mi_pss_send_state_machine(port);
2001 }
2002
gptp_mi_port_bmca_state_machines(int port)2003 void gptp_mi_port_bmca_state_machines(int port)
2004 {
2005 gptp_mi_port_announce_receive_state_machine(port);
2006 gptp_mi_port_announce_information_state_machine(port);
2007 gptp_mi_port_announce_transmit_state_machine(port);
2008 }
2009
gptp_mi_state_machines(void)2010 void gptp_mi_state_machines(void)
2011 {
2012 gptp_mi_site_sync_sync_state_machine();
2013 gptp_mi_clk_slave_sync_state_machine();
2014 gptp_mi_port_role_selection_state_machine();
2015 gptp_mi_clk_master_sync_offset_state_machine();
2016 #if defined(CONFIG_NET_GPTP_GM_CAPABLE)
2017 gptp_mi_clk_master_sync_snd_state_machine();
2018 #endif
2019 gptp_mi_clk_master_sync_rcv_state_machine();
2020 }
2021
