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/net/net_if.h>
11
12 #include "gptp_messages.h"
13 #include "gptp_data_set.h"
14 #include "gptp_md.h"
15 #include "gptp_private.h"
16
17 #define NET_BUF_TIMEOUT K_MSEC(100)
18
19 static struct net_if_timestamp_cb sync_timestamp_cb;
20 static struct net_if_timestamp_cb pdelay_response_timestamp_cb;
21 static bool sync_cb_registered;
22 static bool ts_cb_registered;
23
24 static const struct net_eth_addr gptp_multicast_eth_addr = {
25 { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e } };
26
27 #define NET_GPTP_INFO(msg, pkt) \
28 if (CONFIG_NET_GPTP_LOG_LEVEL >= LOG_LEVEL_DBG) { \
29 struct gptp_hdr *one_hdr = GPTP_HDR(pkt); \
30 \
31 if (one_hdr->message_type == GPTP_ANNOUNCE_MESSAGE) { \
32 struct gptp_announce *ann = GPTP_ANNOUNCE(pkt); \
33 char output[sizeof("xx:xx:xx:xx:xx:xx:xx:xx")]; \
34 \
35 gptp_sprint_clock_id( \
36 ann->root_system_id.grand_master_id, \
37 output, \
38 sizeof(output)); \
39 \
40 NET_DBG("Sending %s seq %d pkt %p", \
41 msg, \
42 ntohs(one_hdr->sequence_id), pkt); \
43 \
44 NET_DBG(" GM %d/%d/0x%x/%d/%s",\
45 ann->root_system_id.grand_master_prio1, \
46 ann->root_system_id.clk_quality.clock_class, \
47 ann->root_system_id.clk_quality.clock_accuracy,\
48 ann->root_system_id.grand_master_prio2, \
49 output); \
50 } else { \
51 NET_DBG("Sending %s seq %d pkt %p", \
52 msg, \
53 ntohs(one_hdr->sequence_id), pkt); \
54 } \
55 }
56
gptp_get_hdr(struct net_pkt * pkt)57 struct gptp_hdr *gptp_get_hdr(struct net_pkt *pkt)
58 {
59 struct net_buf *buf = pkt->frags;
60
61 NET_ASSERT(buf);
62
63 if (sizeof(struct gptp_hdr) <= buf->len) {
64 return (struct gptp_hdr *)buf->data;
65 }
66
67 /* Check if there is a link layer buf in the front and skip it
68 * if needed.
69 */
70 buf = buf->frags;
71 if (!buf) {
72 /* Do not return null here but let the caller failure
73 * checks to fail the packet.
74 */
75 return (struct gptp_hdr *)pkt->frags->data;
76 }
77
78 return (struct gptp_hdr *)buf->data;
79 }
80
gptp_sync_timestamp_callback(struct net_pkt * pkt)81 static void gptp_sync_timestamp_callback(struct net_pkt *pkt)
82 {
83 int port = 0;
84 struct gptp_sync_send_state *state;
85 struct gptp_hdr *hdr;
86
87 port = gptp_get_port_number(net_pkt_iface(pkt));
88 if (port == -ENODEV) {
89 NET_DBG("No port found for ptp buffer");
90 return;
91 }
92
93 state = &GPTP_PORT_STATE(port)->sync_send;
94
95 hdr = GPTP_HDR(pkt);
96
97 /* If this buffer is a sync, flag it to the state machine. */
98 if (hdr->message_type == GPTP_SYNC_MESSAGE) {
99 state->md_sync_timestamp_avail = true;
100
101 net_if_unregister_timestamp_cb(&sync_timestamp_cb);
102 sync_cb_registered = false;
103
104 /* The pkt was ref'ed in gptp_send_sync() */
105 net_pkt_unref(pkt);
106 }
107 }
108
gptp_pdelay_response_timestamp_callback(struct net_pkt * pkt)109 static void gptp_pdelay_response_timestamp_callback(struct net_pkt *pkt)
110 {
111 int port = 0;
112 struct net_pkt *follow_up;
113 struct gptp_hdr *hdr;
114
115 port = gptp_get_port_number(net_pkt_iface(pkt));
116 if (port == -ENODEV) {
117 NET_DBG("No port found for ptp buffer");
118 goto out;
119 }
120
121 hdr = GPTP_HDR(pkt);
122
123 /* If this buffer is a path delay response, send the follow up. */
124 if (hdr->message_type == GPTP_PATH_DELAY_RESP_MESSAGE) {
125 follow_up = gptp_prepare_pdelay_follow_up(port, pkt);
126 if (!follow_up) {
127 /* Cannot handle the follow up, abort */
128 NET_ERR("Could not get buffer");
129 goto out;
130 }
131
132 net_if_unregister_timestamp_cb(&pdelay_response_timestamp_cb);
133 ts_cb_registered = false;
134
135 gptp_send_pdelay_follow_up(port, follow_up,
136 net_pkt_timestamp(pkt));
137
138 out:
139 /* The pkt was ref'ed in gptp_handle_pdelay_req() */
140 net_pkt_unref(pkt);
141 }
142 }
143
144 #if defined(CONFIG_NET_DEBUG_NET_PKT_ALLOC)
setup_gptp_frame_debug(struct net_if * iface,size_t extra_header,const char * caller,int line)145 static struct net_pkt *setup_gptp_frame_debug(struct net_if *iface,
146 size_t extra_header,
147 const char *caller,
148 int line)
149 #define setup_gptp_frame(iface, extra_header) \
150 setup_gptp_frame_debug(iface, extra_header, __func__, __LINE__)
151 #else
152 static struct net_pkt *setup_gptp_frame(struct net_if *iface,
153 size_t extra_header)
154 #endif
155 {
156 struct net_pkt *pkt;
157
158 #if defined(CONFIG_NET_DEBUG_NET_PKT_ALLOC)
159 pkt = net_pkt_alloc_with_buffer_debug(iface, sizeof(struct gptp_hdr) +
160 extra_header, AF_UNSPEC, 0,
161 NET_BUF_TIMEOUT, caller, line);
162 #else
163 pkt = net_pkt_alloc_with_buffer(iface, sizeof(struct gptp_hdr) +
164 extra_header, AF_UNSPEC, 0,
165 NET_BUF_TIMEOUT);
166 #endif
167 if (!pkt) {
168 return NULL;
169 }
170
171 net_buf_add(pkt->buffer, sizeof(struct gptp_hdr) + extra_header);
172 net_pkt_set_ptp(pkt, true);
173
174 net_pkt_lladdr_src(pkt)->addr = net_if_get_link_addr(iface)->addr;
175 net_pkt_lladdr_src(pkt)->len = net_if_get_link_addr(iface)->len;
176
177 net_pkt_lladdr_dst(pkt)->addr = (uint8_t *)&gptp_multicast_eth_addr;
178 net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_eth_addr);
179
180 return pkt;
181 }
182
gptp_prepare_sync(int port)183 struct net_pkt *gptp_prepare_sync(int port)
184 {
185 struct gptp_port_ds *port_ds;
186 struct gptp_sync *sync;
187 struct net_if *iface;
188 struct net_pkt *pkt;
189 struct gptp_hdr *hdr;
190
191 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
192 iface = GPTP_PORT_IFACE(port);
193 NET_ASSERT(iface);
194
195 pkt = setup_gptp_frame(iface, sizeof(struct gptp_sync));
196 if (!pkt) {
197 NET_DBG("Cannot get gPTP frame");
198 return NULL;
199 }
200
201 net_pkt_set_priority(pkt, NET_PRIORITY_IC);
202
203 port_ds = GPTP_PORT_DS(port);
204 sync = GPTP_SYNC(pkt);
205 hdr = GPTP_HDR(pkt);
206
207 /*
208 * Header configuration.
209 *
210 * Some fields are set by gptp_md_sync_send_prepare().
211 */
212 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
213 hdr->message_type = GPTP_SYNC_MESSAGE;
214 hdr->ptp_version = GPTP_VERSION;
215 hdr->sequence_id = htons(port_ds->sync_seq_id);
216 hdr->domain_number = 0U;
217 hdr->correction_field = 0;
218 hdr->flags.octets[0] = GPTP_FLAG_TWO_STEP;
219 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
220 hdr->message_length = htons(sizeof(struct gptp_hdr) +
221 sizeof(struct gptp_sync));
222 hdr->control = GPTP_SYNC_CONTROL_VALUE;
223
224 /* Clear reserved fields. */
225 hdr->reserved0 = 0U;
226 hdr->reserved1 = 0U;
227 hdr->reserved2 = 0U;
228
229 /* PTP configuration. */
230 (void)memset(&sync->reserved, 0, sizeof(sync->reserved));
231
232 /* Update sequence number. */
233 port_ds->sync_seq_id++;
234
235 return pkt;
236 }
237
gptp_prepare_follow_up(int port,struct net_pkt * sync)238 struct net_pkt *gptp_prepare_follow_up(int port, struct net_pkt *sync)
239 {
240 struct gptp_hdr *hdr, *sync_hdr;
241 struct gptp_follow_up *fup;
242 struct net_if *iface;
243 struct net_pkt *pkt;
244 struct net_ptp_time *sync_ts;
245
246 NET_ASSERT(sync);
247 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
248 iface = GPTP_PORT_IFACE(port);
249 NET_ASSERT(iface);
250
251 pkt = setup_gptp_frame(iface, sizeof(struct gptp_follow_up));
252 if (!pkt) {
253 NET_DBG("Cannot get gPTP frame");
254 return NULL;
255 }
256
257 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
258
259 hdr = GPTP_HDR(pkt);
260 fup = GPTP_FOLLOW_UP(pkt);
261 sync_hdr = GPTP_HDR(sync);
262 sync_ts = net_pkt_timestamp(sync);
263
264 /*
265 * Header configuration.
266 *
267 * Some fields are set by gptp_md_follow_up_prepare().
268 */
269 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
270 hdr->message_type = GPTP_FOLLOWUP_MESSAGE;
271 hdr->ptp_version = GPTP_VERSION;
272 hdr->sequence_id = sync_hdr->sequence_id;
273 hdr->domain_number = 0U;
274 /*
275 * Grand master clock should keep correction_field at zero,
276 * according to IEEE802.1AS Table 11-6 and 10.6.2.2.9
277 */
278 hdr->correction_field = 0LL;
279 hdr->flags.octets[0] = 0U;
280 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
281 hdr->message_length = htons(sizeof(struct gptp_hdr) +
282 sizeof(struct gptp_follow_up));
283 hdr->control = GPTP_FUP_CONTROL_VALUE;
284
285 /* Clear reserved fields. */
286 hdr->reserved0 = 0U;
287 hdr->reserved1 = 0U;
288 hdr->reserved2 = 0U;
289
290 /*
291 * Get preciseOriginTimestamp from previous sync message
292 * according to IEEE802.1AS 11.4.4.2.1 syncEventEgressTimestamp
293 */
294 fup->prec_orig_ts_secs_high = htons(sync_ts->_sec.high);
295 fup->prec_orig_ts_secs_low = htonl(sync_ts->_sec.low);
296 fup->prec_orig_ts_nsecs = htonl(sync_ts->nanosecond);
297
298 /* PTP configuration will be set by the MDSyncSend state machine. */
299
300 return pkt;
301 }
302
gptp_prepare_pdelay_req(int port)303 struct net_pkt *gptp_prepare_pdelay_req(int port)
304 {
305 struct gptp_pdelay_req *req;
306 struct gptp_port_ds *port_ds;
307 struct net_if *iface;
308 struct net_pkt *pkt;
309 struct gptp_hdr *hdr;
310
311 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
312 iface = GPTP_PORT_IFACE(port);
313 NET_ASSERT(iface);
314
315 pkt = setup_gptp_frame(iface, sizeof(struct gptp_pdelay_req));
316 if (!pkt) {
317 NET_DBG("Cannot get gPTP frame");
318 return NULL;
319 }
320
321 net_pkt_set_priority(pkt, NET_PRIORITY_IC);
322
323 port_ds = GPTP_PORT_DS(port);
324 req = GPTP_PDELAY_REQ(pkt);
325 hdr = GPTP_HDR(pkt);
326
327 /* Header configuration. */
328 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
329 hdr->message_type = GPTP_PATH_DELAY_REQ_MESSAGE;
330 hdr->ptp_version = GPTP_VERSION;
331 hdr->sequence_id = htons(port_ds->pdelay_req_seq_id);
332 hdr->domain_number = 0U;
333 hdr->correction_field = 0;
334 hdr->flags.octets[0] = 0U;
335 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
336
337 hdr->message_length = htons(sizeof(struct gptp_hdr) +
338 sizeof(struct gptp_pdelay_req));
339 hdr->port_id.port_number = htons(port_ds->port_id.port_number);
340 hdr->control = GPTP_OTHER_CONTROL_VALUE;
341 hdr->log_msg_interval = port_ds->cur_log_pdelay_req_itv;
342
343 /* Clear reserved fields. */
344 hdr->reserved0 = 0U;
345 hdr->reserved1 = 0U;
346 hdr->reserved2 = 0U;
347
348 memcpy(hdr->port_id.clk_id,
349 port_ds->port_id.clk_id, GPTP_CLOCK_ID_LEN);
350
351 /* PTP configuration. */
352 (void)memset(&req->reserved1, 0, sizeof(req->reserved1));
353 (void)memset(&req->reserved2, 0, sizeof(req->reserved2));
354
355 /* Update sequence number. */
356 port_ds->pdelay_req_seq_id++;
357
358 return pkt;
359 }
360
gptp_prepare_pdelay_resp(int port,struct net_pkt * req)361 struct net_pkt *gptp_prepare_pdelay_resp(int port,
362 struct net_pkt *req)
363 {
364 struct net_if *iface = net_pkt_iface(req);
365 struct gptp_pdelay_resp *pdelay_resp;
366 struct gptp_hdr *hdr, *query;
367 struct gptp_port_ds *port_ds;
368 struct net_pkt *pkt;
369
370 pkt = setup_gptp_frame(iface, sizeof(struct gptp_pdelay_resp));
371 if (!pkt) {
372 NET_DBG("Cannot get gPTP frame");
373 return NULL;
374 }
375
376 net_pkt_set_priority(pkt, NET_PRIORITY_IC);
377
378 port_ds = GPTP_PORT_DS(port);
379
380 pdelay_resp = GPTP_PDELAY_RESP(pkt);
381 hdr = GPTP_HDR(pkt);
382
383 query = GPTP_HDR(req);
384
385 /* Header configuration. */
386 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
387 hdr->message_type = GPTP_PATH_DELAY_RESP_MESSAGE;
388 hdr->ptp_version = GPTP_VERSION;
389 hdr->sequence_id = query->sequence_id;
390 hdr->domain_number = query->domain_number;
391 hdr->correction_field = query->correction_field;
392 hdr->flags.octets[0] = GPTP_FLAG_TWO_STEP;
393 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
394
395 hdr->message_length = htons(sizeof(struct gptp_hdr) +
396 sizeof(struct gptp_pdelay_resp));
397 hdr->port_id.port_number = htons(port_ds->port_id.port_number);
398 hdr->control = GPTP_OTHER_CONTROL_VALUE;
399 hdr->log_msg_interval = GPTP_RESP_LOG_MSG_ITV;
400
401 /* Clear reserved fields. */
402 hdr->reserved0 = 0U;
403 hdr->reserved1 = 0U;
404 hdr->reserved2 = 0U;
405
406 memcpy(hdr->port_id.clk_id, port_ds->port_id.clk_id,
407 GPTP_CLOCK_ID_LEN);
408
409 /* PTP configuration. */
410 pdelay_resp->req_receipt_ts_secs_high = 0U;
411 pdelay_resp->req_receipt_ts_secs_low = 0U;
412 pdelay_resp->req_receipt_ts_nsecs = 0U;
413
414 memcpy(&pdelay_resp->requesting_port_id,
415 &query->port_id, sizeof(struct gptp_port_identity));
416
417 return pkt;
418 }
419
gptp_prepare_pdelay_follow_up(int port,struct net_pkt * resp)420 struct net_pkt *gptp_prepare_pdelay_follow_up(int port,
421 struct net_pkt *resp)
422 {
423 struct net_if *iface = net_pkt_iface(resp);
424 struct gptp_pdelay_resp_follow_up *follow_up;
425 struct gptp_pdelay_resp *pdelay_resp;
426 struct gptp_hdr *hdr, *resp_hdr;
427 struct gptp_port_ds *port_ds;
428 struct net_pkt *pkt;
429
430 pkt = setup_gptp_frame(iface,
431 sizeof(struct gptp_pdelay_resp_follow_up));
432 if (!pkt) {
433 NET_DBG("Cannot get gPTP frame");
434 return NULL;
435 }
436
437 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
438
439 port_ds = GPTP_PORT_DS(port);
440
441 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
442 hdr = GPTP_HDR(pkt);
443
444 pdelay_resp = GPTP_PDELAY_RESP(resp);
445 resp_hdr = GPTP_HDR(resp);
446
447 /* Header configuration. */
448 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
449 hdr->ptp_version = GPTP_VERSION;
450 hdr->message_type = GPTP_PATH_DELAY_FOLLOWUP_MESSAGE;
451 hdr->sequence_id = resp_hdr->sequence_id;
452 hdr->domain_number = resp_hdr->domain_number;
453 hdr->correction_field = 0;
454 hdr->message_length = htons(sizeof(struct gptp_hdr) +
455 sizeof(struct gptp_pdelay_resp_follow_up));
456 hdr->port_id.port_number = htons(port_ds->port_id.port_number);
457 hdr->control = GPTP_OTHER_CONTROL_VALUE;
458 hdr->log_msg_interval = GPTP_RESP_LOG_MSG_ITV;
459
460 hdr->flags.octets[0] = 0U;
461 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
462
463 /* Clear reserved fields. */
464 hdr->reserved0 = 0U;
465 hdr->reserved1 = 0U;
466 hdr->reserved2 = 0U;
467
468 memcpy(hdr->port_id.clk_id, port_ds->port_id.clk_id,
469 GPTP_CLOCK_ID_LEN);
470
471 /* PTP configuration. */
472 follow_up->resp_orig_ts_secs_high = 0U;
473 follow_up->resp_orig_ts_secs_low = 0U;
474 follow_up->resp_orig_ts_nsecs = 0U;
475
476 memcpy(&follow_up->requesting_port_id,
477 &pdelay_resp->requesting_port_id,
478 sizeof(struct gptp_port_identity));
479
480 return pkt;
481 }
482
gptp_prepare_announce(int port)483 struct net_pkt *gptp_prepare_announce(int port)
484 {
485 struct gptp_global_ds *global_ds;
486 struct gptp_default_ds *default_ds;
487 struct gptp_port_ds *port_ds;
488 struct gptp_announce *ann;
489 struct net_if *iface;
490 struct net_pkt *pkt;
491 struct gptp_hdr *hdr;
492 struct gptp_priority_vector *gm_prio;
493
494 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
495 global_ds = GPTP_GLOBAL_DS();
496 default_ds = GPTP_DEFAULT_DS();
497 iface = GPTP_PORT_IFACE(port);
498 NET_ASSERT(iface);
499
500 pkt = setup_gptp_frame(iface, sizeof(struct gptp_announce) - 8 +
501 ntohs(global_ds->path_trace.len));
502 if (!pkt) {
503 NET_DBG("Cannot get gPTP frame");
504 return NULL;
505 }
506
507 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
508
509 hdr = GPTP_HDR(pkt);
510 ann = GPTP_ANNOUNCE(pkt);
511 port_ds = GPTP_PORT_DS(port);
512
513 hdr->message_type = GPTP_ANNOUNCE_MESSAGE;
514 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
515 hdr->ptp_version = GPTP_VERSION;
516
517 hdr->domain_number = 0U;
518 hdr->correction_field = 0;
519 hdr->flags.octets[0] = 0U;
520
521 /* Copy leap61, leap59, current UTC offset valid, time traceable and
522 * frequency traceable flags.
523 */
524 hdr->flags.octets[1] =
525 global_ds->global_flags.octets[1] | GPTP_FLAG_PTP_TIMESCALE;
526
527 memcpy(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id,
528 GPTP_CLOCK_ID_LEN);
529
530 hdr->port_id.port_number = htons(port);
531 hdr->control = GPTP_OTHER_CONTROL_VALUE;
532 hdr->log_msg_interval = port_ds->cur_log_announce_itv;
533
534 /* Clear reserved fields. */
535 hdr->reserved0 = 0U;
536 hdr->reserved1 = 0U;
537 hdr->reserved2 = 0U;
538
539 ann->cur_utc_offset = htons(global_ds->current_utc_offset);
540 ann->time_source = global_ds->time_source;
541
542 gm_prio = &global_ds->gm_priority;
543 switch (GPTP_PORT_BMCA_DATA(port)->info_is) {
544 case GPTP_INFO_IS_MINE:
545 case GPTP_INFO_IS_RECEIVED:
546 memcpy(&ann->root_system_id,
547 &gm_prio->root_system_id,
548 sizeof(struct gptp_root_system_identity));
549 break;
550 default:
551 goto fail;
552 }
553
554 ann->steps_removed = global_ds->master_steps_removed;
555 hdr->sequence_id = htons(port_ds->announce_seq_id);
556 port_ds->announce_seq_id++;
557
558 ann->tlv.type = GPTP_ANNOUNCE_MSG_PATH_SEQ_TYPE;
559
560 /* Clear reserved fields. */
561 (void)memset(ann->reserved1, 0, sizeof(ann->reserved1));
562 ann->reserved2 = 0U;
563
564 hdr->message_length = htons(sizeof(struct gptp_hdr) +
565 sizeof(struct gptp_announce) - 8 +
566 ntohs(global_ds->path_trace.len));
567
568 ann->tlv.len = global_ds->path_trace.len;
569
570 net_pkt_cursor_init(pkt);
571
572 /* setup_gptp_frame() already added all the length, so let's not
573 * add up more with net_pkt_skip/write_new(), let's just overwrite
574 */
575 net_pkt_set_overwrite(pkt, true);
576
577 if (net_pkt_skip(pkt, sizeof(struct gptp_hdr) +
578 sizeof(struct gptp_announce) - 8) ||
579 net_pkt_write(pkt, &global_ds->path_trace.path_sequence[0][0],
580 ntohs(global_ds->path_trace.len))) {
581 goto fail;
582 }
583
584 net_pkt_set_overwrite(pkt, false);
585
586 return pkt;
587
588 fail:
589 net_pkt_unref(pkt);
590
591 return NULL;
592 }
593
gptp_handle_sync(int port,struct net_pkt * pkt)594 void gptp_handle_sync(int port, struct net_pkt *pkt)
595 {
596 struct gptp_sync_rcv_state *state;
597 struct gptp_hdr *hdr;
598 uint64_t upstream_sync_itv;
599 k_timeout_t duration;
600
601 state = &GPTP_PORT_STATE(port)->sync_rcv;
602 hdr = GPTP_HDR(state->rcvd_sync_ptr);
603
604 upstream_sync_itv = NSEC_PER_SEC * GPTP_POW2(hdr->log_msg_interval);
605
606 /* Convert ns to ms. */
607 duration = K_MSEC((upstream_sync_itv / 1000000U));
608
609 /* Start timeout timer. */
610 k_timer_start(&state->follow_up_discard_timer, duration, K_NO_WAIT);
611 }
612
gptp_handle_follow_up(int port,struct net_pkt * pkt)613 int gptp_handle_follow_up(int port, struct net_pkt *pkt)
614 {
615 struct gptp_sync_rcv_state *state;
616 struct gptp_hdr *sync_hdr, *hdr;
617
618 state = &GPTP_PORT_STATE(port)->sync_rcv;
619
620 sync_hdr = GPTP_HDR(state->rcvd_sync_ptr);
621 hdr = GPTP_HDR(pkt);
622
623 if (sync_hdr->sequence_id != hdr->sequence_id) {
624 NET_WARN("%s sequence id %d %s %s %d",
625 "FOLLOWUP", ntohs(hdr->sequence_id),
626 "does not match",
627 "SYNC", ntohs(sync_hdr->sequence_id));
628 return -EINVAL;
629 }
630
631 GPTP_STATS_INC(port, rx_fup_count);
632
633 return 0;
634 }
635
gptp_handle_pdelay_req(int port,struct net_pkt * pkt)636 void gptp_handle_pdelay_req(int port, struct net_pkt *pkt)
637 {
638 struct net_pkt *reply;
639
640 GPTP_STATS_INC(port, rx_pdelay_req_count);
641
642 if (ts_cb_registered == true) {
643 NET_WARN("Multiple pdelay requests");
644
645 net_if_unregister_timestamp_cb(&pdelay_response_timestamp_cb);
646 net_pkt_unref(pdelay_response_timestamp_cb.pkt);
647
648 ts_cb_registered = false;
649 }
650
651 /* Prepare response and send */
652 reply = gptp_prepare_pdelay_resp(port, pkt);
653 if (!reply) {
654 return;
655 }
656
657 net_if_register_timestamp_cb(&pdelay_response_timestamp_cb,
658 reply,
659 net_pkt_iface(pkt),
660 gptp_pdelay_response_timestamp_callback);
661
662 /* TS thread will send this back to us so increment ref count so that
663 * the packet is not removed when sending it. This will be unref'ed by
664 * timestamp callback in gptp_pdelay_response_timestamp_callback()
665 */
666 net_pkt_ref(reply);
667
668 ts_cb_registered = true;
669
670 gptp_send_pdelay_resp(port, reply, net_pkt_timestamp(pkt));
671 }
672
gptp_handle_pdelay_resp(int port,struct net_pkt * pkt)673 int gptp_handle_pdelay_resp(int port, struct net_pkt *pkt)
674 {
675 struct gptp_pdelay_req_state *state;
676 struct gptp_default_ds *default_ds;
677 struct gptp_pdelay_resp *resp;
678 struct gptp_hdr *hdr, *req_hdr;
679
680 hdr = GPTP_HDR(pkt);
681 resp = GPTP_PDELAY_RESP(pkt);
682 state = &GPTP_PORT_STATE(port)->pdelay_req;
683 default_ds = GPTP_DEFAULT_DS();
684
685 if (!state->tx_pdelay_req_ptr) {
686 goto reset;
687 }
688
689 req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr);
690
691 /* Check clock identity. */
692 if (memcmp(default_ds->clk_id, resp->requesting_port_id.clk_id,
693 GPTP_CLOCK_ID_LEN)) {
694 NET_WARN("Requesting Clock Identity %s", "does not match");
695 goto reset;
696 }
697 if (memcmp(default_ds->clk_id, hdr->port_id.clk_id,
698 GPTP_CLOCK_ID_LEN) == 0) {
699 NET_WARN("Source Clock Identity is local Clock Identity");
700 goto reset;
701 }
702
703 /* Check port number. */
704 if (resp->requesting_port_id.port_number != htons(port)) {
705 NET_WARN("Requesting Port Number %s", "does not match");
706 goto reset;
707 }
708
709 /* Check sequence id. */
710 if (hdr->sequence_id != req_hdr->sequence_id) {
711 NET_WARN("Sequence Id %d %s %d",
712 ntohs(hdr->sequence_id),
713 "does not match",
714 ntohs(req_hdr->sequence_id));
715 goto reset;
716 }
717
718 GPTP_STATS_INC(port, rx_pdelay_resp_count);
719
720 return 0;
721
722 reset:
723 return -EINVAL;
724 }
725
gptp_handle_pdelay_follow_up(int port,struct net_pkt * pkt)726 int gptp_handle_pdelay_follow_up(int port, struct net_pkt *pkt)
727 {
728 struct gptp_pdelay_resp_follow_up *follow_up;
729 struct gptp_hdr *hdr, *req_hdr, *resp_hdr;
730 struct gptp_pdelay_req_state *state;
731 struct gptp_default_ds *default_ds;
732
733 hdr = GPTP_HDR(pkt);
734 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
735 state = &GPTP_PORT_STATE(port)->pdelay_req;
736 default_ds = GPTP_DEFAULT_DS();
737
738 if (!state->tx_pdelay_req_ptr) {
739 goto reset;
740 }
741
742 req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr);
743
744 if (!state->rcvd_pdelay_resp_ptr) {
745 goto reset;
746 }
747
748 resp_hdr = GPTP_HDR(state->rcvd_pdelay_resp_ptr);
749
750 /* Check clock identity. */
751 if (memcmp(default_ds->clk_id, follow_up->requesting_port_id.clk_id,
752 GPTP_CLOCK_ID_LEN)) {
753 NET_WARN("Requesting Clock Identity %s", "does not match");
754 goto reset;
755 }
756
757 if (memcmp(default_ds->clk_id, hdr->port_id.clk_id,
758 GPTP_CLOCK_ID_LEN) == 0) {
759 NET_WARN("Source Clock Identity is local Clock Identity");
760 goto reset;
761 }
762
763 /* Check port number. */
764 if (follow_up->requesting_port_id.port_number != htons(port)) {
765 NET_WARN("Requesting Port Number %s", "does not match");
766 goto reset;
767 }
768
769 /* Check sequence id. */
770 if (hdr->sequence_id != req_hdr->sequence_id) {
771 NET_WARN("Sequence Id %d %s %d",
772 ntohs(hdr->sequence_id),
773 "does not match",
774 ntohs(req_hdr->sequence_id));
775 goto reset;
776 }
777
778 /* Check source port. */
779 if (memcmp(&hdr->port_id, &resp_hdr->port_id,
780 sizeof(hdr->port_id)) != 0) {
781 NET_WARN("pDelay response and follow up port IDs %s",
782 "does not match");
783 goto reset;
784 }
785
786 GPTP_STATS_INC(port, rx_fup_count);
787
788 return 0;
789
790 reset:
791 return -EINVAL;
792 }
793
gptp_handle_signaling(int port,struct net_pkt * pkt)794 void gptp_handle_signaling(int port, struct net_pkt *pkt)
795 {
796 struct gptp_port_ds *port_ds;
797 struct gptp_signaling *sig;
798
799 sig = GPTP_SIGNALING(pkt);
800 port_ds = GPTP_PORT_DS(port);
801
802 /* If time-synchronization not enabled, drop packet. */
803 if (!port_ds->ptt_port_enabled) {
804 return;
805 }
806
807 /* pDelay interval. */
808 gptp_update_pdelay_req_interval(port, sig->tlv.link_delay_itv);
809
810 /* Sync interval. */
811 gptp_update_sync_interval(port, sig->tlv.time_sync_itv);
812
813 /* Announce interval. */
814 gptp_update_announce_interval(port, sig->tlv.announce_itv);
815
816 port_ds->compute_neighbor_rate_ratio =
817 sig->tlv.compute_neighbor_rate_ratio;
818 port_ds->compute_neighbor_prop_delay =
819 sig->tlv.compute_neighbor_prop_delay;
820 }
821
gptp_send_sync(int port,struct net_pkt * pkt)822 void gptp_send_sync(int port, struct net_pkt *pkt)
823 {
824 if (!sync_cb_registered) {
825 net_if_register_timestamp_cb(&sync_timestamp_cb,
826 pkt,
827 net_pkt_iface(pkt),
828 gptp_sync_timestamp_callback);
829 sync_cb_registered = true;
830 }
831
832 GPTP_STATS_INC(port, tx_sync_count);
833
834 /* TS thread will send this back to us so increment ref count
835 * so that the packet is not removed when sending it.
836 * This will be unref'ed by timestamp callback in
837 * gptp_sync_timestamp_callback()
838 */
839 net_pkt_ref(pkt);
840
841 NET_GPTP_INFO("SYNC", pkt);
842
843 net_if_queue_tx(net_pkt_iface(pkt), pkt);
844 }
845
gptp_send_follow_up(int port,struct net_pkt * pkt)846 void gptp_send_follow_up(int port, struct net_pkt *pkt)
847 {
848 GPTP_STATS_INC(port, tx_fup_count);
849
850 NET_GPTP_INFO("FOLLOWUP", pkt);
851
852 net_if_queue_tx(net_pkt_iface(pkt), pkt);
853 }
854
gptp_send_announce(int port,struct net_pkt * pkt)855 void gptp_send_announce(int port, struct net_pkt *pkt)
856 {
857 GPTP_STATS_INC(port, tx_announce_count);
858
859 NET_GPTP_INFO("ANNOUNCE", pkt);
860
861 net_if_queue_tx(net_pkt_iface(pkt), pkt);
862 }
863
gptp_send_pdelay_req(int port)864 void gptp_send_pdelay_req(int port)
865 {
866 struct gptp_pdelay_req_state *state;
867 struct net_pkt *pkt;
868
869 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
870 state = &GPTP_PORT_STATE(port)->pdelay_req;
871
872 pkt = gptp_prepare_pdelay_req(port);
873 if (pkt) {
874 if (state->tx_pdelay_req_ptr) {
875 NET_DBG("Unref pending %s %p", "PDELAY_REQ",
876 state->tx_pdelay_req_ptr);
877
878 net_pkt_unref(state->tx_pdelay_req_ptr);
879 }
880
881 /* Keep the buffer alive until pdelay_rate_ratio is computed. */
882 state->tx_pdelay_req_ptr = net_pkt_ref(pkt);
883
884 GPTP_STATS_INC(port, tx_pdelay_req_count);
885
886 NET_GPTP_INFO("PDELAY_REQ", pkt);
887
888 net_if_queue_tx(net_pkt_iface(pkt), pkt);
889 } else {
890 NET_ERR("Failed to prepare %s", "PDELAY_REQ");
891 }
892 }
893
gptp_send_pdelay_resp(int port,struct net_pkt * pkt,struct net_ptp_time * treq)894 void gptp_send_pdelay_resp(int port, struct net_pkt *pkt,
895 struct net_ptp_time *treq)
896 {
897 struct gptp_pdelay_resp *resp;
898 struct gptp_hdr *hdr;
899
900 hdr = GPTP_HDR(pkt);
901
902 /* No Fractional nsec .*/
903 hdr->correction_field = 0;
904
905 resp = GPTP_PDELAY_RESP(pkt);
906 resp->req_receipt_ts_secs_high = htons(treq->_sec.high);
907 resp->req_receipt_ts_secs_low = htonl(treq->_sec.low);
908 resp->req_receipt_ts_nsecs = htonl(treq->nanosecond);
909
910 GPTP_STATS_INC(port, tx_pdelay_resp_count);
911
912 NET_GPTP_INFO("PDELAY_RESP", pkt);
913
914 net_if_queue_tx(net_pkt_iface(pkt), pkt);
915 }
916
gptp_send_pdelay_follow_up(int port,struct net_pkt * pkt,struct net_ptp_time * tresp)917 void gptp_send_pdelay_follow_up(int port, struct net_pkt *pkt,
918 struct net_ptp_time *tresp)
919 {
920 struct gptp_pdelay_resp_follow_up *follow_up;
921 struct gptp_hdr *hdr;
922
923 hdr = GPTP_HDR(pkt);
924
925 /* No Fractional nsec .*/
926 hdr->correction_field = 0;
927
928 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
929 follow_up->resp_orig_ts_secs_high = htons(tresp->_sec.high);
930 follow_up->resp_orig_ts_secs_low = htonl(tresp->_sec.low);
931 follow_up->resp_orig_ts_nsecs = htonl(tresp->nanosecond);
932
933 GPTP_STATS_INC(port, tx_pdelay_resp_fup_count);
934
935 NET_GPTP_INFO("PDELAY_FOLLOWUP", pkt);
936
937 net_if_queue_tx(net_pkt_iface(pkt), pkt);
938 }
939
