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