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 "gptp_messages.h"
11 #include "gptp_md.h"
12 #include "gptp_data_set.h"
13 #include "gptp_private.h"
14
gptp_md_sync_prepare(struct net_pkt * pkt,struct gptp_md_sync_info * sync_send,int port_number)15 static void gptp_md_sync_prepare(struct net_pkt *pkt,
16 struct gptp_md_sync_info *sync_send,
17 int port_number)
18 {
19 struct gptp_hdr *hdr;
20
21 hdr = GPTP_HDR(pkt);
22
23 memcpy(&hdr->port_id.clk_id, &sync_send->src_port_id.clk_id,
24 GPTP_CLOCK_ID_LEN);
25
26 hdr->port_id.port_number = htons(port_number);
27
28 hdr->log_msg_interval = sync_send->log_msg_interval;
29 }
30
gptp_md_follow_up_prepare(struct net_pkt * pkt,struct gptp_md_sync_info * sync_send,int port_number)31 static void gptp_md_follow_up_prepare(struct net_pkt *pkt,
32 struct gptp_md_sync_info *sync_send,
33 int port_number)
34 {
35 struct gptp_hdr *hdr;
36 struct gptp_follow_up *fup;
37
38 hdr = GPTP_HDR(pkt);
39 fup = GPTP_FOLLOW_UP(pkt);
40
41 memcpy(&hdr->port_id.clk_id, &sync_send->src_port_id.clk_id,
42 GPTP_CLOCK_ID_LEN);
43
44 hdr->port_id.port_number = htons(port_number);
45
46 hdr->log_msg_interval = sync_send->log_msg_interval;
47
48 fup->tlv_hdr.type = htons(GPTP_TLV_ORGANIZATION_EXT);
49 fup->tlv_hdr.len = htons(sizeof(struct gptp_follow_up_tlv));
50 fup->tlv.org_id[0] = GPTP_FUP_TLV_ORG_ID_BYTE_0;
51 fup->tlv.org_id[1] = GPTP_FUP_TLV_ORG_ID_BYTE_1;
52 fup->tlv.org_id[2] = GPTP_FUP_TLV_ORG_ID_BYTE_2;
53 fup->tlv.org_sub_type[0] = 0U;
54 fup->tlv.org_sub_type[1] = 0U;
55 fup->tlv.org_sub_type[2] = GPTP_FUP_TLV_ORG_SUB_TYPE;
56
57 fup->tlv.cumulative_scaled_rate_offset =
58 (sync_send->rate_ratio - 1.0) * GPTP_POW2_41;
59 fup->tlv.cumulative_scaled_rate_offset =
60 ntohl(fup->tlv.cumulative_scaled_rate_offset);
61 fup->tlv.gm_time_base_indicator =
62 ntohs(sync_send->gm_time_base_indicator);
63 fup->tlv.last_gm_phase_change.high =
64 ntohl(sync_send->last_gm_phase_change.high);
65 fup->tlv.last_gm_phase_change.low =
66 ntohll(sync_send->last_gm_phase_change.low);
67 fup->tlv.scaled_last_gm_freq_change = sync_send->last_gm_freq_change;
68 fup->tlv.scaled_last_gm_freq_change =
69 ntohl(fup->tlv.scaled_last_gm_freq_change);
70 }
71
gptp_set_md_sync_receive(int port,struct gptp_md_sync_info * sync_rcv)72 static int gptp_set_md_sync_receive(int port,
73 struct gptp_md_sync_info *sync_rcv)
74 {
75 struct gptp_sync_rcv_state *state;
76 struct gptp_port_ds *port_ds;
77 struct gptp_hdr *sync_hdr, *fup_hdr;
78 struct gptp_follow_up *fup;
79 struct net_ptp_time *sync_ts;
80 double prop_delay_rated;
81 double delay_asymmetry_rated;
82
83 state = &GPTP_PORT_STATE(port)->sync_rcv;
84 if (!state->rcvd_sync_ptr || !state->rcvd_follow_up_ptr) {
85 return -EINVAL;
86 }
87
88 port_ds = GPTP_PORT_DS(port);
89
90 sync_hdr = GPTP_HDR(state->rcvd_sync_ptr);
91 fup_hdr = GPTP_HDR(state->rcvd_follow_up_ptr);
92 fup = GPTP_FOLLOW_UP(state->rcvd_follow_up_ptr);
93 sync_ts = &state->rcvd_sync_ptr->timestamp;
94
95 sync_rcv->follow_up_correction_field =
96 (ntohll(fup_hdr->correction_field) >> 16);
97 memcpy(&sync_rcv->src_port_id, &sync_hdr->port_id,
98 sizeof(struct gptp_port_identity));
99 sync_rcv->log_msg_interval = fup_hdr->log_msg_interval;
100 sync_rcv->precise_orig_ts._sec.high =
101 ntohs(fup->prec_orig_ts_secs_high);
102 sync_rcv->precise_orig_ts._sec.low = ntohl(fup->prec_orig_ts_secs_low);
103 sync_rcv->precise_orig_ts.nanosecond = ntohl(fup->prec_orig_ts_nsecs);
104
105 /* Compute time when sync was sent by the remote. */
106 sync_rcv->upstream_tx_time = sync_ts->second;
107 sync_rcv->upstream_tx_time *= NSEC_PER_SEC;
108 sync_rcv->upstream_tx_time += sync_ts->nanosecond;
109
110 prop_delay_rated = port_ds->neighbor_prop_delay;
111 prop_delay_rated /= port_ds->neighbor_rate_ratio;
112
113 sync_rcv->upstream_tx_time -= prop_delay_rated;
114
115 delay_asymmetry_rated = port_ds->delay_asymmetry;
116 delay_asymmetry_rated /= port_ds->neighbor_rate_ratio;
117
118 sync_rcv->upstream_tx_time -= delay_asymmetry_rated;
119
120 sync_rcv->rate_ratio = ntohl(fup->tlv.cumulative_scaled_rate_offset);
121 sync_rcv->rate_ratio /= GPTP_POW2_41;
122 sync_rcv->rate_ratio += 1;
123
124 sync_rcv->gm_time_base_indicator =
125 ntohs(fup->tlv.gm_time_base_indicator);
126 sync_rcv->last_gm_phase_change.high =
127 ntohl(fup->tlv.last_gm_phase_change.high);
128 sync_rcv->last_gm_phase_change.low =
129 ntohll(fup->tlv.last_gm_phase_change.low);
130 sync_rcv->last_gm_freq_change =
131 ntohl(fup->tlv.scaled_last_gm_freq_change);
132
133 return 0;
134 }
135
gptp_md_pdelay_reset(int port)136 static void gptp_md_pdelay_reset(int port)
137 {
138 struct gptp_pdelay_req_state *state;
139 struct gptp_port_ds *port_ds;
140
141 NET_WARN("Reset Pdelay requests");
142
143 state = &GPTP_PORT_STATE(port)->pdelay_req;
144 port_ds = GPTP_PORT_DS(port);
145
146 if (state->lost_responses < port_ds->allowed_lost_responses) {
147 state->lost_responses += 1U;
148 } else {
149 port_ds->is_measuring_delay = false;
150 port_ds->as_capable = false;
151 state->init_pdelay_compute = true;
152 }
153 }
154
gptp_md_pdelay_check_multiple_resp(int port)155 static void gptp_md_pdelay_check_multiple_resp(int port)
156 {
157 struct gptp_pdelay_req_state *state;
158 struct gptp_port_ds *port_ds;
159 int duration;
160
161 state = &GPTP_PORT_STATE(port)->pdelay_req;
162 port_ds = GPTP_PORT_DS(port);
163
164 if ((state->rcvd_pdelay_resp > 1) ||
165 (state->rcvd_pdelay_follow_up > 1)) {
166 port_ds->as_capable = false;
167 NET_WARN("Too many responses (%d / %d)",
168 state->rcvd_pdelay_resp,
169 state->rcvd_pdelay_follow_up);
170 state->multiple_resp_count++;
171 } else {
172 state->multiple_resp_count = 0U;
173 }
174
175 if (state->multiple_resp_count >= 3U) {
176 state->multiple_resp_count = 0U;
177 k_timer_stop(&state->pdelay_timer);
178 state->pdelay_timer_expired = false;
179
180 /* Subtract time spent since last pDelay request. */
181 duration = GPTP_MULTIPLE_PDELAY_RESP_WAIT -
182 gptp_uscaled_ns_to_timer_ms(&port_ds->pdelay_req_itv);
183
184 k_timer_start(&state->pdelay_timer, K_MSEC(duration),
185 K_NO_WAIT);
186 } else {
187 state->state = GPTP_PDELAY_REQ_SEND_REQ;
188 }
189 }
190
gptp_md_compute_pdelay_rate_ratio(int port)191 static void gptp_md_compute_pdelay_rate_ratio(int port)
192 {
193 uint64_t ingress_tstamp = 0U;
194 uint64_t resp_evt_tstamp = 0U;
195 struct gptp_pdelay_resp_follow_up *fup;
196 struct gptp_pdelay_req_state *state;
197 struct gptp_port_ds *port_ds;
198 struct net_pkt *pkt;
199 struct gptp_hdr *hdr;
200 double neighbor_rate_ratio;
201
202 state = &GPTP_PORT_STATE(port)->pdelay_req;
203 port_ds = GPTP_PORT_DS(port);
204
205 /* Get ingress timestamp. */
206 pkt = state->rcvd_pdelay_resp_ptr;
207 if (pkt) {
208 ingress_tstamp =
209 gptp_timestamp_to_nsec(net_pkt_timestamp(pkt));
210 }
211
212 /* Get peer corrected timestamp. */
213 pkt = state->rcvd_pdelay_follow_up_ptr;
214 if (pkt) {
215 hdr = GPTP_HDR(pkt);
216 fup = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
217
218 resp_evt_tstamp = ntohs(fup->resp_orig_ts_secs_high);
219 resp_evt_tstamp <<= 32;
220 resp_evt_tstamp |= ntohl(fup->resp_orig_ts_secs_low);
221 resp_evt_tstamp *= NSEC_PER_SEC;
222 resp_evt_tstamp += ntohl(fup->resp_orig_ts_nsecs);
223 resp_evt_tstamp += (ntohll(hdr->correction_field) >> 16);
224 }
225
226 if (state->init_pdelay_compute) {
227 state->init_pdelay_compute = false;
228
229 state->ini_resp_ingress_tstamp = ingress_tstamp;
230 state->ini_resp_evt_tstamp = resp_evt_tstamp;
231
232 neighbor_rate_ratio = 1.0;
233
234 state->neighbor_rate_ratio_valid = false;
235 } else {
236 if (resp_evt_tstamp == state->ini_resp_evt_tstamp) {
237 state->neighbor_rate_ratio_valid = false;
238 neighbor_rate_ratio = 1.0;
239 } else {
240 neighbor_rate_ratio =
241 (resp_evt_tstamp - state->ini_resp_evt_tstamp);
242 neighbor_rate_ratio /=
243 (ingress_tstamp -
244 state->ini_resp_ingress_tstamp);
245
246 /* Measure the ratio with the previously sent response.
247 */
248 state->ini_resp_ingress_tstamp = ingress_tstamp;
249 state->ini_resp_evt_tstamp = resp_evt_tstamp;
250 state->neighbor_rate_ratio_valid = true;
251 }
252 }
253
254 port_ds->neighbor_rate_ratio = neighbor_rate_ratio;
255 port_ds->neighbor_rate_ratio_valid = state->neighbor_rate_ratio_valid;
256 }
257
gptp_md_compute_prop_time(int port)258 static void gptp_md_compute_prop_time(int port)
259 {
260 uint64_t t1_ns = 0U, t2_ns = 0U, t3_ns = 0U, t4_ns = 0U;
261 struct gptp_pdelay_resp_follow_up *fup;
262 struct gptp_pdelay_req_state *state;
263 struct gptp_pdelay_resp *resp;
264 struct gptp_port_ds *port_ds;
265 struct gptp_hdr *hdr;
266 struct net_pkt *pkt;
267 double prop_time, turn_around;
268
269 state = &GPTP_PORT_STATE(port)->pdelay_req;
270 port_ds = GPTP_PORT_DS(port);
271
272 /* Get egress timestamp. */
273 pkt = state->tx_pdelay_req_ptr;
274 if (pkt) {
275 t1_ns = gptp_timestamp_to_nsec(net_pkt_timestamp(pkt));
276 }
277
278 /* Get ingress timestamp. */
279 pkt = state->rcvd_pdelay_resp_ptr;
280 if (pkt) {
281 t4_ns = gptp_timestamp_to_nsec(net_pkt_timestamp(pkt));
282 }
283
284 /* Get peer corrected timestamps. */
285 pkt = state->rcvd_pdelay_resp_ptr;
286 if (pkt) {
287 hdr = GPTP_HDR(pkt);
288 resp = GPTP_PDELAY_RESP(pkt);
289
290 t2_ns = ((uint64_t)ntohs(resp->req_receipt_ts_secs_high)) << 32;
291 t2_ns |= ntohl(resp->req_receipt_ts_secs_low);
292 t2_ns *= NSEC_PER_SEC;
293 t2_ns += ntohl(resp->req_receipt_ts_nsecs);
294 t2_ns += (ntohll(hdr->correction_field) >> 16);
295 }
296
297 pkt = state->rcvd_pdelay_follow_up_ptr;
298 if (pkt) {
299 hdr = GPTP_HDR(pkt);
300 fup = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
301
302 t3_ns = ((uint64_t)ntohs(fup->resp_orig_ts_secs_high)) << 32;
303 t3_ns |= ntohl(fup->resp_orig_ts_secs_low);
304 t3_ns *= NSEC_PER_SEC;
305 t3_ns += ntohl(fup->resp_orig_ts_nsecs);
306 t3_ns += (ntohll(hdr->correction_field) >> 16);
307 }
308
309 prop_time = t4_ns - t1_ns;
310
311 turn_around = t3_ns - t2_ns;
312
313 /* Adjusting the turn-around time for peer to local clock rate
314 * difference. The check is implemented the same way as how Avnu/gptp
315 * daemon is doing it. This comment is also found in their source
316 * for the magic values "TODO: Are these .998 and 1.002 specifically
317 * defined in the standard?"
318 */
319 if (port_ds->neighbor_rate_ratio > .998 &&
320 port_ds->neighbor_rate_ratio < 1.002) {
321 turn_around *= port_ds->neighbor_rate_ratio;
322 }
323
324 prop_time -= turn_around;
325 prop_time /= 2;
326
327 port_ds->neighbor_prop_delay = prop_time;
328 }
329
gptp_md_pdelay_compute(int port)330 static void gptp_md_pdelay_compute(int port)
331 {
332 struct gptp_pdelay_req_state *state;
333 struct gptp_port_ds *port_ds;
334 struct gptp_hdr *hdr;
335 struct net_pkt *pkt;
336 bool local_clock;
337
338 state = &GPTP_PORT_STATE(port)->pdelay_req;
339 port_ds = GPTP_PORT_DS(port);
340
341 if (!state->tx_pdelay_req_ptr || !state->rcvd_pdelay_resp_ptr ||
342 !state->rcvd_pdelay_follow_up_ptr) {
343 NET_ERR("Compute path delay called without buffer ready");
344 port_ds->as_capable = false;
345 goto out;
346 }
347
348 if (port_ds->compute_neighbor_rate_ratio) {
349 gptp_md_compute_pdelay_rate_ratio(port);
350 }
351
352 if (port_ds->compute_neighbor_prop_delay) {
353 gptp_md_compute_prop_time(port);
354
355 NET_DBG("Neighbor prop delay %d",
356 (int32_t)port_ds->neighbor_prop_delay);
357 }
358
359 state->lost_responses = 0U;
360 port_ds->is_measuring_delay = true;
361
362 pkt = state->rcvd_pdelay_follow_up_ptr;
363 hdr = GPTP_HDR(pkt);
364
365 local_clock = !memcmp(gptp_domain.default_ds.clk_id,
366 hdr->port_id.clk_id,
367 GPTP_CLOCK_ID_LEN);
368 if (local_clock) {
369 NET_WARN("Discard path delay response from local clock.");
370 goto out;
371 }
372
373 if (!state->neighbor_rate_ratio_valid) {
374 goto out;
375 }
376
377 /*
378 * Currently, if the computed delay is negative, this means
379 * that it is negligible enough compared to other factors.
380 */
381 if ((port_ds->neighbor_prop_delay <=
382 port_ds->neighbor_prop_delay_thresh)) {
383 port_ds->as_capable = true;
384 } else {
385 port_ds->as_capable = false;
386
387 NET_WARN("Not AS capable: %u ns > %u ns",
388 (uint32_t)port_ds->neighbor_prop_delay,
389 (uint32_t)port_ds->neighbor_prop_delay_thresh);
390
391 GPTP_STATS_INC(port, neighbor_prop_delay_exceeded);
392 }
393
394 out:
395 /* Release buffers. */
396 if (state->tx_pdelay_req_ptr) {
397 net_pkt_unref(state->tx_pdelay_req_ptr);
398 state->tx_pdelay_req_ptr = NULL;
399 }
400
401 if (state->rcvd_pdelay_resp_ptr) {
402 net_pkt_unref(state->rcvd_pdelay_resp_ptr);
403 state->rcvd_pdelay_resp_ptr = NULL;
404 }
405
406 if (state->rcvd_pdelay_follow_up_ptr) {
407 net_pkt_unref(state->rcvd_pdelay_follow_up_ptr);
408 state->rcvd_pdelay_follow_up_ptr = NULL;
409 }
410 }
411
gptp_md_pdelay_req_timeout(struct k_timer * timer)412 static void gptp_md_pdelay_req_timeout(struct k_timer *timer)
413 {
414 struct gptp_pdelay_req_state *state;
415 int port;
416
417 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
418 state = &GPTP_PORT_STATE(port)->pdelay_req;
419 if (timer == &state->pdelay_timer) {
420 state->pdelay_timer_expired = true;
421
422 if (state->rcvd_pdelay_resp == 0U) {
423 GPTP_STATS_INC(port,
424 pdelay_allowed_lost_resp_exceed_count);
425 }
426 }
427 }
428 }
429
gptp_md_start_pdelay_req(int port)430 static void gptp_md_start_pdelay_req(int port)
431 {
432 struct gptp_pdelay_req_state *state;
433 struct gptp_port_ds *port_ds;
434
435 port_ds = GPTP_PORT_DS(port);
436 state = &GPTP_PORT_STATE(port)->pdelay_req;
437
438 port_ds->neighbor_rate_ratio = 1.0;
439 port_ds->is_measuring_delay = false;
440 port_ds->as_capable = false;
441 state->lost_responses = 0U;
442 state->rcvd_pdelay_resp = 0U;
443 state->rcvd_pdelay_follow_up = 0U;
444 state->multiple_resp_count = 0U;
445 }
446
gptp_md_follow_up_receipt_timeout(struct k_timer * timer)447 static void gptp_md_follow_up_receipt_timeout(struct k_timer *timer)
448 {
449 struct gptp_sync_rcv_state *state;
450 int port;
451
452 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
453 state = &GPTP_PORT_STATE(port)->sync_rcv;
454 if (timer == &state->follow_up_discard_timer) {
455 NET_WARN("No %s received after %s message",
456 "FOLLOWUP", "SYNC");
457 state->follow_up_timeout_expired = true;
458 }
459 }
460 }
461
gptp_md_init_pdelay_req_state_machine(int port)462 static void gptp_md_init_pdelay_req_state_machine(int port)
463 {
464 struct gptp_pdelay_req_state *state;
465
466 state = &GPTP_PORT_STATE(port)->pdelay_req;
467
468 k_timer_init(&state->pdelay_timer, gptp_md_pdelay_req_timeout, NULL);
469
470 state->state = GPTP_PDELAY_REQ_NOT_ENABLED;
471
472 state->neighbor_rate_ratio_valid = false;
473 state->init_pdelay_compute = true;
474 state->rcvd_pdelay_resp = 0U;
475 state->rcvd_pdelay_follow_up = 0U;
476 state->pdelay_timer_expired = false;
477
478 state->rcvd_pdelay_resp_ptr = NULL;
479 state->rcvd_pdelay_follow_up_ptr = NULL;
480 state->tx_pdelay_req_ptr = NULL;
481
482 state->ini_resp_evt_tstamp = 0U;
483 state->ini_resp_ingress_tstamp = 0U;
484 state->lost_responses = 0U;
485 }
486
gptp_md_init_pdelay_resp_state_machine(int port)487 static void gptp_md_init_pdelay_resp_state_machine(int port)
488 {
489 struct gptp_pdelay_resp_state *state;
490
491 state = &GPTP_PORT_STATE(port)->pdelay_resp;
492
493 state->state = GPTP_PDELAY_RESP_NOT_ENABLED;
494 }
495
gptp_md_init_sync_rcv_state_machine(int port)496 static void gptp_md_init_sync_rcv_state_machine(int port)
497 {
498 struct gptp_sync_rcv_state *state;
499
500 state = &GPTP_PORT_STATE(port)->sync_rcv;
501
502 k_timer_init(&state->follow_up_discard_timer,
503 gptp_md_follow_up_receipt_timeout, NULL);
504
505 state->rcvd_sync = false;
506 state->rcvd_follow_up = false;
507 state->rcvd_sync_ptr = NULL;
508 state->rcvd_follow_up_ptr = NULL;
509
510 state->follow_up_timeout_expired = false;
511 state->follow_up_receipt_timeout = 0U;
512
513 state->state = GPTP_SYNC_RCV_DISCARD;
514 }
515
gptp_md_init_sync_send_state_machine(int port)516 static void gptp_md_init_sync_send_state_machine(int port)
517 {
518 struct gptp_sync_send_state *state;
519
520 state = &GPTP_PORT_STATE(port)->sync_send;
521
522 state->rcvd_md_sync = false;
523 state->md_sync_timestamp_avail = false;
524 state->sync_send_ptr = NULL;
525 state->sync_ptr = NULL;
526
527 state->state = GPTP_SYNC_SEND_INITIALIZING;
528 }
529
gptp_md_init_state_machine(void)530 void gptp_md_init_state_machine(void)
531 {
532 int port;
533
534 for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
535 gptp_md_init_pdelay_req_state_machine(port);
536 gptp_md_init_pdelay_resp_state_machine(port);
537 gptp_md_init_sync_rcv_state_machine(port);
538 gptp_md_init_sync_send_state_machine(port);
539 }
540 }
541
gptp_md_pdelay_req_state_machine(int port)542 static void gptp_md_pdelay_req_state_machine(int port)
543 {
544 struct gptp_port_ds *port_ds;
545 struct gptp_pdelay_req_state *state;
546 struct net_pkt *pkt;
547
548 state = &GPTP_PORT_STATE(port)->pdelay_req;
549 port_ds = GPTP_PORT_DS(port);
550
551 /* Unset AS-Capable if multiple responses to a pDelay request have been
552 * received.
553 */
554 if (state->rcvd_pdelay_resp > 1 || state->rcvd_pdelay_follow_up > 1) {
555 port_ds->as_capable = false;
556 }
557
558 if (!port_ds->ptt_port_enabled) {
559 /* Make sure the timer is stopped. */
560 k_timer_stop(&state->pdelay_timer);
561 state->state = GPTP_PDELAY_REQ_NOT_ENABLED;
562 }
563
564 switch (state->state) {
565 case GPTP_PDELAY_REQ_NOT_ENABLED:
566 if (port_ds->ptt_port_enabled) {
567 /* (Re)Init interval (as defined in
568 * LinkDelaySyncIntervalSetting state machine).
569 */
570 port_ds->cur_log_pdelay_req_itv =
571 port_ds->ini_log_pdelay_req_itv;
572
573 gptp_set_time_itv(&port_ds->pdelay_req_itv, 1,
574 port_ds->cur_log_pdelay_req_itv);
575
576 port_ds->compute_neighbor_rate_ratio = true;
577 port_ds->compute_neighbor_prop_delay = true;
578
579 state->pdelay_timer_expired = true;
580 state->state = GPTP_PDELAY_REQ_INITIAL_SEND_REQ;
581 }
582
583 break;
584
585 case GPTP_PDELAY_REQ_RESET:
586 gptp_md_pdelay_reset(port);
587 /* Send a request on the next timer expiry. */
588 state->state = GPTP_PDELAY_REQ_WAIT_ITV_TIMER;
589 break;
590
591 case GPTP_PDELAY_REQ_INITIAL_SEND_REQ:
592 gptp_md_start_pdelay_req(port);
593 __fallthrough;
594
595 case GPTP_PDELAY_REQ_SEND_REQ:
596 if (state->tx_pdelay_req_ptr) {
597 net_pkt_unref(state->tx_pdelay_req_ptr);
598 state->tx_pdelay_req_ptr = NULL;
599 }
600
601 if (state->rcvd_pdelay_resp_ptr) {
602 net_pkt_unref(state->rcvd_pdelay_resp_ptr);
603 state->rcvd_pdelay_resp_ptr = NULL;
604 }
605
606 if (state->rcvd_pdelay_follow_up_ptr) {
607 net_pkt_unref(state->rcvd_pdelay_follow_up_ptr);
608 state->rcvd_pdelay_follow_up_ptr = NULL;
609 }
610
611 gptp_send_pdelay_req(port);
612
613 k_timer_stop(&state->pdelay_timer);
614 state->pdelay_timer_expired = false;
615 k_timer_start(&state->pdelay_timer,
616 K_MSEC(gptp_uscaled_ns_to_timer_ms(
617 &port_ds->pdelay_req_itv)),
618 K_NO_WAIT);
619 /*
620 * Transition directly to GPTP_PDELAY_REQ_WAIT_RESP.
621 * Check for the TX timestamp will be done during
622 * the computation of the path delay.
623 */
624 state->state = GPTP_PDELAY_REQ_WAIT_RESP;
625 break;
626
627 case GPTP_PDELAY_REQ_WAIT_RESP:
628 if (state->pdelay_timer_expired) {
629 state->state = GPTP_PDELAY_REQ_RESET;
630 } else if (state->rcvd_pdelay_resp != 0U) {
631 pkt = state->rcvd_pdelay_resp_ptr;
632 if (!gptp_handle_pdelay_resp(port, pkt)) {
633 state->state = GPTP_PDELAY_REQ_WAIT_FOLLOW_UP;
634 } else {
635 state->state = GPTP_PDELAY_REQ_RESET;
636 }
637 }
638
639 break;
640
641 case GPTP_PDELAY_REQ_WAIT_FOLLOW_UP:
642 if (state->pdelay_timer_expired) {
643 state->state = GPTP_PDELAY_REQ_RESET;
644 } else if (state->rcvd_pdelay_follow_up != 0U) {
645 pkt = state->rcvd_pdelay_follow_up_ptr;
646 if (!gptp_handle_pdelay_follow_up(port, pkt)) {
647 gptp_md_pdelay_compute(port);
648 state->state = GPTP_PDELAY_REQ_WAIT_ITV_TIMER;
649 } else {
650 state->state = GPTP_PDELAY_REQ_RESET;
651 }
652 }
653
654 break;
655
656 case GPTP_PDELAY_REQ_WAIT_ITV_TIMER:
657 if (state->pdelay_timer_expired) {
658 gptp_md_pdelay_check_multiple_resp(port);
659
660 state->rcvd_pdelay_resp = 0U;
661 state->rcvd_pdelay_follow_up = 0U;
662 }
663
664 break;
665 }
666 }
667
gptp_md_pdelay_resp_state_machine(int port)668 static void gptp_md_pdelay_resp_state_machine(int port)
669 {
670 struct gptp_port_ds *port_ds;
671 struct gptp_pdelay_resp_state *state;
672
673 state = &GPTP_PORT_STATE(port)->pdelay_resp;
674 port_ds = GPTP_PORT_DS(port);
675
676 if (!port_ds->ptt_port_enabled) {
677 state->state = GPTP_PDELAY_RESP_NOT_ENABLED;
678 }
679
680 switch (state->state) {
681 case GPTP_PDELAY_RESP_NOT_ENABLED:
682 if (port_ds->ptt_port_enabled) {
683 state->state = GPTP_PDELAY_RESP_INITIAL_WAIT_REQ;
684 }
685
686 break;
687
688 case GPTP_PDELAY_RESP_INITIAL_WAIT_REQ:
689 case GPTP_PDELAY_RESP_WAIT_REQ:
690 /* Handled in gptp_handle_msg for latency considerations. */
691 break;
692
693 case GPTP_PDELAY_RESP_WAIT_TSTAMP:
694 /* Handled in gptp_follow_up_callback. */
695 break;
696 }
697
698 }
699
gptp_md_sync_receive_state_machine(int port)700 static void gptp_md_sync_receive_state_machine(int port)
701 {
702 struct gptp_port_ds *port_ds;
703 struct gptp_sync_rcv_state *state;
704 struct gptp_pss_rcv_state *pss_state;
705
706 state = &GPTP_PORT_STATE(port)->sync_rcv;
707 pss_state = &GPTP_PORT_STATE(port)->pss_rcv;
708 port_ds = GPTP_PORT_DS(port);
709
710 if ((!port_ds->ptt_port_enabled) || !port_ds->as_capable) {
711 /* Make sure the timer is stopped. */
712 k_timer_stop(&state->follow_up_discard_timer);
713
714 /* Discard all received messages. */
715 if (state->rcvd_sync_ptr) {
716 net_pkt_unref(state->rcvd_sync_ptr);
717 state->rcvd_sync_ptr = NULL;
718 }
719
720 if (state->rcvd_follow_up_ptr) {
721 net_pkt_unref(state->rcvd_follow_up_ptr);
722 state->rcvd_follow_up_ptr = NULL;
723 }
724
725 state->rcvd_sync = false;
726 state->rcvd_follow_up = false;
727 state->state = GPTP_SYNC_RCV_DISCARD;
728 return;
729 }
730
731 switch (state->state) {
732 case GPTP_SYNC_RCV_DISCARD:
733 case GPTP_SYNC_RCV_WAIT_SYNC:
734 if (state->rcvd_sync) {
735 gptp_handle_sync(port, state->rcvd_sync_ptr);
736 state->rcvd_sync = false;
737 state->state = GPTP_SYNC_RCV_WAIT_FOLLOW_UP;
738 } else if (state->rcvd_follow_up) {
739 /* Delete late/early message. */
740 if (state->rcvd_follow_up_ptr) {
741 net_pkt_unref(state->rcvd_follow_up_ptr);
742 state->rcvd_follow_up_ptr = NULL;
743 }
744
745 state->rcvd_follow_up = false;
746 }
747
748 break;
749
750 case GPTP_SYNC_RCV_WAIT_FOLLOW_UP:
751 /* Never received a follow up for a sync message. */
752 if (state->follow_up_timeout_expired) {
753 k_timer_stop(&state->follow_up_discard_timer);
754 state->follow_up_timeout_expired = false;
755 state->state = GPTP_SYNC_RCV_DISCARD;
756 if (state->rcvd_sync_ptr) {
757 net_pkt_unref(state->rcvd_sync_ptr);
758 state->rcvd_sync_ptr = NULL;
759 }
760
761 state->rcvd_sync = false;
762 } else if (state->rcvd_sync) {
763 /* Handle received extra sync. */
764 gptp_handle_sync(port, state->rcvd_sync_ptr);
765 state->rcvd_sync = false;
766 } else if (state->rcvd_follow_up) {
767 if (!gptp_handle_follow_up(
768 port, state->rcvd_follow_up_ptr)) {
769
770 /*
771 * Fill the structure to be sent to
772 * PortSyncSyncReceive.
773 */
774 gptp_set_md_sync_receive(port,
775 &pss_state->sync_rcv);
776
777 pss_state->rcvd_md_sync = true;
778
779 state->state = GPTP_SYNC_RCV_WAIT_SYNC;
780
781 /* Buffers can be released now. */
782 if (state->rcvd_sync_ptr) {
783 net_pkt_unref(state->rcvd_sync_ptr);
784 state->rcvd_sync_ptr = NULL;
785 }
786
787 k_timer_stop(&state->follow_up_discard_timer);
788 state->follow_up_timeout_expired = false;
789 }
790 }
791
792 if (state->rcvd_follow_up_ptr) {
793 net_pkt_unref(state->rcvd_follow_up_ptr);
794 state->rcvd_follow_up_ptr = NULL;
795 }
796
797 state->rcvd_follow_up = false;
798 break;
799 }
800 }
801
gptp_md_sync_send_state_machine(int port)802 static void gptp_md_sync_send_state_machine(int port)
803 {
804 struct gptp_port_ds *port_ds;
805 struct gptp_sync_send_state *state;
806 struct net_pkt *pkt;
807
808 state = &GPTP_PORT_STATE(port)->sync_send;
809 port_ds = GPTP_PORT_DS(port);
810
811 if ((!port_ds->ptt_port_enabled) || !port_ds->as_capable) {
812 state->rcvd_md_sync = false;
813 state->state = GPTP_SYNC_SEND_INITIALIZING;
814
815 /* Sync sequence id is initialized in the port_ds init function
816 */
817 return;
818 }
819
820 switch (state->state) {
821 case GPTP_SYNC_SEND_INITIALIZING:
822 state->state = GPTP_SYNC_SEND_SEND_SYNC;
823 break;
824
825 case GPTP_SYNC_SEND_SEND_SYNC:
826 if (state->rcvd_md_sync) {
827 pkt = gptp_prepare_sync(port);
828 if (pkt) {
829 /* Reference message to track timestamp info */
830 state->sync_ptr = net_pkt_ref(pkt);
831 gptp_md_sync_prepare(pkt,
832 state->sync_send_ptr,
833 port);
834 gptp_send_sync(port, pkt);
835 }
836
837 state->rcvd_md_sync = false;
838 state->state = GPTP_SYNC_SEND_SEND_FUP;
839 }
840
841 break;
842
843 case GPTP_SYNC_SEND_SEND_FUP:
844 if (state->md_sync_timestamp_avail) {
845 state->md_sync_timestamp_avail = false;
846
847 if (!state->sync_ptr) {
848 NET_ERR("Sync message not available");
849 break;
850 }
851
852 pkt = gptp_prepare_follow_up(port, state->sync_ptr);
853 if (pkt) {
854 gptp_md_follow_up_prepare(pkt,
855 state->sync_send_ptr,
856 port);
857 gptp_send_follow_up(port, pkt);
858 }
859
860 net_pkt_unref(state->sync_ptr);
861 state->sync_ptr = NULL;
862
863 state->state = GPTP_SYNC_SEND_SEND_SYNC;
864 }
865
866 break;
867 }
868 }
869
gptp_md_state_machines(int port)870 void gptp_md_state_machines(int port)
871 {
872 gptp_md_pdelay_req_state_machine(port);
873 gptp_md_pdelay_resp_state_machine(port);
874 gptp_md_sync_receive_state_machine(port);
875 gptp_md_sync_send_state_machine(port);
876 }
877
