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 *hdr = GPTP_HDR(pkt); \
30 \
31 if (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(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(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_CA);
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 net_if *iface;
242 struct net_pkt *pkt;
243
244 NET_ASSERT(sync);
245 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
246 iface = GPTP_PORT_IFACE(port);
247 NET_ASSERT(iface);
248
249 pkt = setup_gptp_frame(iface, sizeof(struct gptp_follow_up));
250 if (!pkt) {
251 NET_DBG("Cannot get gPTP frame");
252 return NULL;
253 }
254
255 net_pkt_set_priority(pkt, NET_PRIORITY_IC);
256
257 hdr = GPTP_HDR(pkt);
258 sync_hdr = GPTP_HDR(sync);
259
260 /*
261 * Header configuration.
262 *
263 * Some fields are set by gptp_md_follow_up_prepare().
264 */
265 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
266 hdr->message_type = GPTP_FOLLOWUP_MESSAGE;
267 hdr->ptp_version = GPTP_VERSION;
268 hdr->sequence_id = sync_hdr->sequence_id;
269 hdr->domain_number = 0U;
270 /* Store timestamp value in correction field. */
271 hdr->correction_field = gptp_timestamp_to_nsec(&sync->timestamp);
272 hdr->flags.octets[0] = 0U;
273 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
274 hdr->message_length = htons(sizeof(struct gptp_hdr) +
275 sizeof(struct gptp_follow_up));
276 hdr->control = GPTP_FUP_CONTROL_VALUE;
277
278 /* Clear reserved fields. */
279 hdr->reserved0 = 0U;
280 hdr->reserved1 = 0U;
281 hdr->reserved2 = 0U;
282
283 /* PTP configuration will be set by the MDSyncSend state machine. */
284
285 return pkt;
286 }
287
gptp_prepare_pdelay_req(int port)288 struct net_pkt *gptp_prepare_pdelay_req(int port)
289 {
290 struct gptp_pdelay_req *req;
291 struct gptp_port_ds *port_ds;
292 struct net_if *iface;
293 struct net_pkt *pkt;
294 struct gptp_hdr *hdr;
295
296 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
297 iface = GPTP_PORT_IFACE(port);
298 NET_ASSERT(iface);
299
300 pkt = setup_gptp_frame(iface, sizeof(struct gptp_pdelay_req));
301 if (!pkt) {
302 NET_DBG("Cannot get gPTP frame");
303 return NULL;
304 }
305
306 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
307
308 port_ds = GPTP_PORT_DS(port);
309 req = GPTP_PDELAY_REQ(pkt);
310 hdr = GPTP_HDR(pkt);
311
312 /* Header configuration. */
313 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
314 hdr->message_type = GPTP_PATH_DELAY_REQ_MESSAGE;
315 hdr->ptp_version = GPTP_VERSION;
316 hdr->sequence_id = htons(port_ds->pdelay_req_seq_id);
317 hdr->domain_number = 0U;
318 hdr->correction_field = 0;
319 hdr->flags.octets[0] = 0U;
320 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
321
322 hdr->message_length = htons(sizeof(struct gptp_hdr) +
323 sizeof(struct gptp_pdelay_req));
324 hdr->port_id.port_number = htons(port_ds->port_id.port_number);
325 hdr->control = GPTP_OTHER_CONTROL_VALUE;
326 hdr->log_msg_interval = port_ds->cur_log_pdelay_req_itv;
327
328 /* Clear reserved fields. */
329 hdr->reserved0 = 0U;
330 hdr->reserved1 = 0U;
331 hdr->reserved2 = 0U;
332
333 memcpy(hdr->port_id.clk_id,
334 port_ds->port_id.clk_id, GPTP_CLOCK_ID_LEN);
335
336 /* PTP configuration. */
337 (void)memset(&req->reserved1, 0, sizeof(req->reserved1));
338 (void)memset(&req->reserved2, 0, sizeof(req->reserved2));
339
340 /* Update sequence number. */
341 port_ds->pdelay_req_seq_id++;
342
343 return pkt;
344 }
345
gptp_prepare_pdelay_resp(int port,struct net_pkt * req)346 struct net_pkt *gptp_prepare_pdelay_resp(int port,
347 struct net_pkt *req)
348 {
349 struct net_if *iface = net_pkt_iface(req);
350 struct gptp_pdelay_resp *pdelay_resp;
351 struct gptp_hdr *hdr, *query;
352 struct gptp_port_ds *port_ds;
353 struct net_pkt *pkt;
354
355 pkt = setup_gptp_frame(iface, sizeof(struct gptp_pdelay_resp));
356 if (!pkt) {
357 NET_DBG("Cannot get gPTP frame");
358 return NULL;
359 }
360
361 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
362
363 port_ds = GPTP_PORT_DS(port);
364
365 pdelay_resp = GPTP_PDELAY_RESP(pkt);
366 hdr = GPTP_HDR(pkt);
367
368 query = GPTP_HDR(req);
369
370 /* Header configuration. */
371 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
372 hdr->message_type = GPTP_PATH_DELAY_RESP_MESSAGE;
373 hdr->ptp_version = GPTP_VERSION;
374 hdr->sequence_id = query->sequence_id;
375 hdr->domain_number = query->domain_number;
376 hdr->correction_field = query->correction_field;
377 hdr->flags.octets[0] = GPTP_FLAG_TWO_STEP;
378 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
379
380 hdr->message_length = htons(sizeof(struct gptp_hdr) +
381 sizeof(struct gptp_pdelay_resp));
382 hdr->port_id.port_number = htons(port_ds->port_id.port_number);
383 hdr->control = GPTP_OTHER_CONTROL_VALUE;
384 hdr->log_msg_interval = GPTP_RESP_LOG_MSG_ITV;
385
386 /* Clear reserved fields. */
387 hdr->reserved0 = 0U;
388 hdr->reserved1 = 0U;
389 hdr->reserved2 = 0U;
390
391 memcpy(hdr->port_id.clk_id, port_ds->port_id.clk_id,
392 GPTP_CLOCK_ID_LEN);
393
394 /* PTP configuration. */
395 pdelay_resp->req_receipt_ts_secs_high = 0U;
396 pdelay_resp->req_receipt_ts_secs_low = 0U;
397 pdelay_resp->req_receipt_ts_nsecs = 0U;
398
399 memcpy(&pdelay_resp->requesting_port_id,
400 &query->port_id, sizeof(struct gptp_port_identity));
401
402 return pkt;
403 }
404
gptp_prepare_pdelay_follow_up(int port,struct net_pkt * resp)405 struct net_pkt *gptp_prepare_pdelay_follow_up(int port,
406 struct net_pkt *resp)
407 {
408 struct net_if *iface = net_pkt_iface(resp);
409 struct gptp_pdelay_resp_follow_up *follow_up;
410 struct gptp_pdelay_resp *pdelay_resp;
411 struct gptp_hdr *hdr, *resp_hdr;
412 struct gptp_port_ds *port_ds;
413 struct net_pkt *pkt;
414
415 pkt = setup_gptp_frame(iface,
416 sizeof(struct gptp_pdelay_resp_follow_up));
417 if (!pkt) {
418 NET_DBG("Cannot get gPTP frame");
419 return NULL;
420 }
421
422 net_pkt_set_priority(pkt, NET_PRIORITY_IC);
423
424 port_ds = GPTP_PORT_DS(port);
425
426 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
427 hdr = GPTP_HDR(pkt);
428
429 pdelay_resp = GPTP_PDELAY_RESP(resp);
430 resp_hdr = GPTP_HDR(resp);
431
432 /* Header configuration. */
433 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
434 hdr->ptp_version = GPTP_VERSION;
435 hdr->message_type = GPTP_PATH_DELAY_FOLLOWUP_MESSAGE;
436 hdr->sequence_id = resp_hdr->sequence_id;
437 hdr->domain_number = resp_hdr->domain_number;
438 hdr->correction_field = 0;
439 hdr->message_length = htons(sizeof(struct gptp_hdr) +
440 sizeof(struct gptp_pdelay_resp_follow_up));
441 hdr->port_id.port_number = htons(port_ds->port_id.port_number);
442 hdr->control = GPTP_OTHER_CONTROL_VALUE;
443 hdr->log_msg_interval = GPTP_RESP_LOG_MSG_ITV;
444
445 hdr->flags.octets[0] = 0U;
446 hdr->flags.octets[1] = GPTP_FLAG_PTP_TIMESCALE;
447
448 /* Clear reserved fields. */
449 hdr->reserved0 = 0U;
450 hdr->reserved1 = 0U;
451 hdr->reserved2 = 0U;
452
453 memcpy(hdr->port_id.clk_id, port_ds->port_id.clk_id,
454 GPTP_CLOCK_ID_LEN);
455
456 /* PTP configuration. */
457 follow_up->resp_orig_ts_secs_high = 0U;
458 follow_up->resp_orig_ts_secs_low = 0U;
459 follow_up->resp_orig_ts_nsecs = 0U;
460
461 memcpy(&follow_up->requesting_port_id,
462 &pdelay_resp->requesting_port_id,
463 sizeof(struct gptp_port_identity));
464
465 return pkt;
466 }
467
gptp_prepare_announce(int port)468 struct net_pkt *gptp_prepare_announce(int port)
469 {
470 struct gptp_global_ds *global_ds;
471 struct gptp_default_ds *default_ds;
472 struct gptp_port_ds *port_ds;
473 struct gptp_announce *ann;
474 struct net_if *iface;
475 struct net_pkt *pkt;
476 struct gptp_hdr *hdr;
477
478 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
479 global_ds = GPTP_GLOBAL_DS();
480 default_ds = GPTP_DEFAULT_DS();
481 iface = GPTP_PORT_IFACE(port);
482 NET_ASSERT(iface);
483
484 pkt = setup_gptp_frame(iface, sizeof(struct gptp_announce) - 8 +
485 ntohs(global_ds->path_trace.len));
486 if (!pkt) {
487 NET_DBG("Cannot get gPTP frame");
488 return NULL;
489 }
490
491 net_pkt_set_priority(pkt, NET_PRIORITY_IC);
492
493 hdr = GPTP_HDR(pkt);
494 ann = GPTP_ANNOUNCE(pkt);
495 port_ds = GPTP_PORT_DS(port);
496
497 hdr->message_type = GPTP_ANNOUNCE_MESSAGE;
498 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
499 hdr->ptp_version = GPTP_VERSION;
500
501 hdr->domain_number = 0U;
502 hdr->correction_field = 0;
503 hdr->flags.octets[0] = 0U;
504
505 /* Copy leap61, leap59, current UTC offset valid, time traceable and
506 * frequency traceable flags.
507 */
508 hdr->flags.octets[1] =
509 global_ds->global_flags.octets[1] | GPTP_FLAG_PTP_TIMESCALE;
510
511 memcpy(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id,
512 GPTP_CLOCK_ID_LEN);
513
514 hdr->port_id.port_number = htons(port);
515 hdr->control = GPTP_OTHER_CONTROL_VALUE;
516 hdr->log_msg_interval = port_ds->cur_log_announce_itv;
517
518 /* Clear reserved fields. */
519 hdr->reserved0 = 0U;
520 hdr->reserved1 = 0U;
521 hdr->reserved2 = 0U;
522
523 ann->cur_utc_offset = global_ds->current_utc_offset;
524 ann->time_source = global_ds->time_source;
525
526 switch (GPTP_PORT_BMCA_DATA(port)->info_is) {
527 case GPTP_INFO_IS_MINE:
528 ann->root_system_id.grand_master_prio1 = default_ds->priority1;
529 ann->root_system_id.grand_master_prio2 = default_ds->priority2;
530
531 memcpy(&ann->root_system_id.clk_quality,
532 &default_ds->clk_quality,
533 sizeof(struct gptp_clock_quality));
534
535 memcpy(&ann->root_system_id.grand_master_id,
536 default_ds->clk_id,
537 GPTP_CLOCK_ID_LEN);
538 break;
539 case GPTP_INFO_IS_RECEIVED:
540 memcpy(&ann->root_system_id,
541 &GPTP_PORT_BMCA_DATA(port)->
542 master_priority.root_system_id,
543 sizeof(struct gptp_root_system_identity));
544 break;
545 default:
546 goto fail;
547 }
548
549 ann->steps_removed = global_ds->master_steps_removed;
550 hdr->sequence_id = htons(port_ds->announce_seq_id);
551 port_ds->announce_seq_id++;
552
553 ann->tlv.type = GPTP_ANNOUNCE_MSG_PATH_SEQ_TYPE;
554
555 /* Clear reserved fields. */
556 (void)memset(ann->reserved1, 0, sizeof(ann->reserved1));
557 ann->reserved2 = 0U;
558
559 hdr->message_length = htons(sizeof(struct gptp_hdr) +
560 sizeof(struct gptp_announce) - 8 +
561 ntohs(global_ds->path_trace.len));
562
563 ann->tlv.len = global_ds->path_trace.len;
564
565 net_pkt_cursor_init(pkt);
566
567 /* setup_gptp_frame() already added all the length, so let's not
568 * add up more with net_pkt_skip/write_new(), let's just overwrite
569 */
570 net_pkt_set_overwrite(pkt, true);
571
572 if (net_pkt_skip(pkt, sizeof(struct gptp_hdr) +
573 sizeof(struct gptp_announce) - 8) ||
574 net_pkt_write(pkt, &global_ds->path_trace.path_sequence[0][0],
575 ntohs(global_ds->path_trace.len))) {
576 goto fail;
577 }
578
579 net_pkt_set_overwrite(pkt, false);
580
581 return pkt;
582
583 fail:
584 net_pkt_unref(pkt);
585
586 return NULL;
587 }
588
gptp_handle_sync(int port,struct net_pkt * pkt)589 void gptp_handle_sync(int port, struct net_pkt *pkt)
590 {
591 struct gptp_sync_rcv_state *state;
592 struct gptp_hdr *hdr;
593 uint64_t upstream_sync_itv;
594 k_timeout_t duration;
595
596 state = &GPTP_PORT_STATE(port)->sync_rcv;
597 hdr = GPTP_HDR(state->rcvd_sync_ptr);
598
599 upstream_sync_itv = NSEC_PER_SEC * GPTP_POW2(hdr->log_msg_interval);
600
601 /* Convert ns to ms. */
602 duration = K_MSEC((upstream_sync_itv / 1000000U));
603
604 /* Start timeout timer. */
605 k_timer_start(&state->follow_up_discard_timer, duration, K_NO_WAIT);
606 }
607
gptp_handle_follow_up(int port,struct net_pkt * pkt)608 int gptp_handle_follow_up(int port, struct net_pkt *pkt)
609 {
610 struct gptp_sync_rcv_state *state;
611 struct gptp_hdr *sync_hdr, *hdr;
612
613 state = &GPTP_PORT_STATE(port)->sync_rcv;
614
615 sync_hdr = GPTP_HDR(state->rcvd_sync_ptr);
616 hdr = GPTP_HDR(pkt);
617
618 if (sync_hdr->sequence_id != hdr->sequence_id) {
619 NET_WARN("%s sequence id %d %s %s %d",
620 "FOLLOWUP", ntohs(hdr->sequence_id),
621 "does not match",
622 "SYNC", ntohs(sync_hdr->sequence_id));
623 return -EINVAL;
624 }
625
626 GPTP_STATS_INC(port, rx_fup_count);
627
628 return 0;
629 }
630
gptp_handle_pdelay_req(int port,struct net_pkt * pkt)631 void gptp_handle_pdelay_req(int port, struct net_pkt *pkt)
632 {
633 struct net_pkt *reply;
634
635 GPTP_STATS_INC(port, rx_pdelay_req_count);
636
637 if (ts_cb_registered == true) {
638 NET_WARN("Multiple pdelay requests");
639
640 net_if_unregister_timestamp_cb(&pdelay_response_timestamp_cb);
641 net_pkt_unref(pdelay_response_timestamp_cb.pkt);
642
643 ts_cb_registered = false;
644 }
645
646 /* Prepare response and send */
647 reply = gptp_prepare_pdelay_resp(port, pkt);
648 if (!reply) {
649 return;
650 }
651
652 net_if_register_timestamp_cb(&pdelay_response_timestamp_cb,
653 reply,
654 net_pkt_iface(pkt),
655 gptp_pdelay_response_timestamp_callback);
656
657 /* TS thread will send this back to us so increment ref count so that
658 * the packet is not removed when sending it. This will be unref'ed by
659 * timestamp callback in gptp_pdelay_response_timestamp_callback()
660 */
661 net_pkt_ref(reply);
662
663 ts_cb_registered = true;
664
665 gptp_send_pdelay_resp(port, reply, net_pkt_timestamp(pkt));
666 }
667
gptp_handle_pdelay_resp(int port,struct net_pkt * pkt)668 int gptp_handle_pdelay_resp(int port, struct net_pkt *pkt)
669 {
670 struct gptp_pdelay_req_state *state;
671 struct gptp_default_ds *default_ds;
672 struct gptp_pdelay_resp *resp;
673 struct gptp_hdr *hdr, *req_hdr;
674
675 hdr = GPTP_HDR(pkt);
676 resp = GPTP_PDELAY_RESP(pkt);
677 state = &GPTP_PORT_STATE(port)->pdelay_req;
678 default_ds = GPTP_DEFAULT_DS();
679
680 if (!state->tx_pdelay_req_ptr) {
681 goto reset;
682 }
683
684 req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr);
685
686 /* Check clock identity. */
687 if (memcmp(default_ds->clk_id, resp->requesting_port_id.clk_id,
688 GPTP_CLOCK_ID_LEN)) {
689 NET_WARN("Requesting Clock Identity %s", "does not match");
690 goto reset;
691 }
692 if (memcmp(default_ds->clk_id, hdr->port_id.clk_id,
693 GPTP_CLOCK_ID_LEN) == 0) {
694 NET_WARN("Source Clock Identity is local Clock Identity");
695 goto reset;
696 }
697
698 /* Check port number. */
699 if (resp->requesting_port_id.port_number != htons(port)) {
700 NET_WARN("Requesting Port Number %s", "does not match");
701 goto reset;
702 }
703
704 /* Check sequence id. */
705 if (hdr->sequence_id != req_hdr->sequence_id) {
706 NET_WARN("Sequence Id %d %s %d",
707 ntohs(hdr->sequence_id),
708 "does not match",
709 ntohs(req_hdr->sequence_id));
710 goto reset;
711 }
712
713 GPTP_STATS_INC(port, rx_pdelay_resp_count);
714
715 return 0;
716
717 reset:
718 return -EINVAL;
719 }
720
gptp_handle_pdelay_follow_up(int port,struct net_pkt * pkt)721 int gptp_handle_pdelay_follow_up(int port, struct net_pkt *pkt)
722 {
723 struct gptp_pdelay_resp_follow_up *follow_up;
724 struct gptp_hdr *hdr, *req_hdr, *resp_hdr;
725 struct gptp_pdelay_req_state *state;
726 struct gptp_default_ds *default_ds;
727
728 hdr = GPTP_HDR(pkt);
729 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
730 state = &GPTP_PORT_STATE(port)->pdelay_req;
731 default_ds = GPTP_DEFAULT_DS();
732
733 if (!state->tx_pdelay_req_ptr) {
734 goto reset;
735 }
736
737 req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr);
738
739 if (!state->rcvd_pdelay_resp_ptr) {
740 goto reset;
741 }
742
743 resp_hdr = GPTP_HDR(state->rcvd_pdelay_resp_ptr);
744
745 /* Check clock identity. */
746 if (memcmp(default_ds->clk_id, follow_up->requesting_port_id.clk_id,
747 GPTP_CLOCK_ID_LEN)) {
748 NET_WARN("Requesting Clock Identity %s", "does not match");
749 goto reset;
750 }
751
752 if (memcmp(default_ds->clk_id, hdr->port_id.clk_id,
753 GPTP_CLOCK_ID_LEN) == 0) {
754 NET_WARN("Source Clock Identity is local Clock Identity");
755 goto reset;
756 }
757
758 /* Check port number. */
759 if (follow_up->requesting_port_id.port_number != htons(port)) {
760 NET_WARN("Requesting Port Number %s", "does not match");
761 goto reset;
762 }
763
764 /* Check sequence id. */
765 if (hdr->sequence_id != req_hdr->sequence_id) {
766 NET_WARN("Sequence Id %d %s %d",
767 ntohs(hdr->sequence_id),
768 "does not match",
769 ntohs(req_hdr->sequence_id));
770 goto reset;
771 }
772
773 /* Check source port. */
774 if (memcmp(&hdr->port_id, &resp_hdr->port_id,
775 sizeof(hdr->port_id)) != 0) {
776 NET_WARN("pDelay response and follow up port IDs %s",
777 "does not match");
778 goto reset;
779 }
780
781 GPTP_STATS_INC(port, rx_fup_count);
782
783 return 0;
784
785 reset:
786 return -EINVAL;
787 }
788
gptp_handle_signaling(int port,struct net_pkt * pkt)789 void gptp_handle_signaling(int port, struct net_pkt *pkt)
790 {
791 struct gptp_port_ds *port_ds;
792 struct gptp_signaling *sig;
793
794 sig = GPTP_SIGNALING(pkt);
795 port_ds = GPTP_PORT_DS(port);
796
797 /* If time-synchronization not enabled, drop packet. */
798 if (!port_ds->ptt_port_enabled) {
799 return;
800 }
801
802 /* pDelay interval. */
803 gptp_update_pdelay_req_interval(port, sig->tlv.link_delay_itv);
804
805 /* Sync interval. */
806 gptp_update_sync_interval(port, sig->tlv.time_sync_itv);
807
808 /* Announce interval. */
809 gptp_update_announce_interval(port, sig->tlv.announce_itv);
810
811 port_ds->compute_neighbor_rate_ratio =
812 sig->tlv.compute_neighbor_rate_ratio;
813 port_ds->compute_neighbor_prop_delay =
814 sig->tlv.compute_neighbor_prop_delay;
815 }
816
gptp_send_sync(int port,struct net_pkt * pkt)817 void gptp_send_sync(int port, struct net_pkt *pkt)
818 {
819 if (!sync_cb_registered) {
820 net_if_register_timestamp_cb(&sync_timestamp_cb,
821 pkt,
822 net_pkt_iface(pkt),
823 gptp_sync_timestamp_callback);
824 sync_cb_registered = true;
825 }
826
827 GPTP_STATS_INC(port, tx_sync_count);
828
829 /* TS thread will send this back to us so increment ref count
830 * so that the packet is not removed when sending it.
831 * This will be unref'ed by timestamp callback in
832 * gptp_sync_timestamp_callback()
833 */
834 net_pkt_ref(pkt);
835
836 NET_GPTP_INFO("SYNC", pkt);
837
838 net_if_queue_tx(net_pkt_iface(pkt), pkt);
839 }
840
gptp_send_follow_up(int port,struct net_pkt * pkt)841 void gptp_send_follow_up(int port, struct net_pkt *pkt)
842 {
843 GPTP_STATS_INC(port, tx_fup_count);
844
845 NET_GPTP_INFO("FOLLOWUP", pkt);
846
847 net_if_queue_tx(net_pkt_iface(pkt), pkt);
848 }
849
gptp_send_announce(int port,struct net_pkt * pkt)850 void gptp_send_announce(int port, struct net_pkt *pkt)
851 {
852 GPTP_STATS_INC(port, tx_announce_count);
853
854 NET_GPTP_INFO("ANNOUNCE", pkt);
855
856 net_if_queue_tx(net_pkt_iface(pkt), pkt);
857 }
858
gptp_send_pdelay_req(int port)859 void gptp_send_pdelay_req(int port)
860 {
861 struct gptp_pdelay_req_state *state;
862 struct net_pkt *pkt;
863
864 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
865 state = &GPTP_PORT_STATE(port)->pdelay_req;
866
867 pkt = gptp_prepare_pdelay_req(port);
868 if (pkt) {
869 if (state->tx_pdelay_req_ptr) {
870 NET_DBG("Unref pending %s %p", "PDELAY_REQ",
871 state->tx_pdelay_req_ptr);
872
873 net_pkt_unref(state->tx_pdelay_req_ptr);
874 }
875
876 /* Keep the buffer alive until pdelay_rate_ratio is computed. */
877 state->tx_pdelay_req_ptr = net_pkt_ref(pkt);
878
879 GPTP_STATS_INC(port, tx_pdelay_req_count);
880
881 NET_GPTP_INFO("PDELAY_REQ", pkt);
882
883 net_if_queue_tx(net_pkt_iface(pkt), pkt);
884 } else {
885 NET_ERR("Failed to prepare %s", "PDELAY_REQ");
886 }
887 }
888
gptp_send_pdelay_resp(int port,struct net_pkt * pkt,struct net_ptp_time * treq)889 void gptp_send_pdelay_resp(int port, struct net_pkt *pkt,
890 struct net_ptp_time *treq)
891 {
892 struct gptp_pdelay_resp *resp;
893 struct gptp_hdr *hdr;
894
895 hdr = GPTP_HDR(pkt);
896
897 /* No Fractional nsec .*/
898 hdr->correction_field = 0;
899
900 resp = GPTP_PDELAY_RESP(pkt);
901 resp->req_receipt_ts_secs_high = htons(treq->_sec.high);
902 resp->req_receipt_ts_secs_low = htonl(treq->_sec.low);
903 resp->req_receipt_ts_nsecs = htonl(treq->nanosecond);
904
905 GPTP_STATS_INC(port, tx_pdelay_resp_count);
906
907 NET_GPTP_INFO("PDELAY_RESP", pkt);
908
909 net_if_queue_tx(net_pkt_iface(pkt), pkt);
910 }
911
gptp_send_pdelay_follow_up(int port,struct net_pkt * pkt,struct net_ptp_time * tresp)912 void gptp_send_pdelay_follow_up(int port, struct net_pkt *pkt,
913 struct net_ptp_time *tresp)
914 {
915 struct gptp_pdelay_resp_follow_up *follow_up;
916 struct gptp_hdr *hdr;
917
918 hdr = GPTP_HDR(pkt);
919
920 /* No Fractional nsec .*/
921 hdr->correction_field = 0;
922
923 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
924 follow_up->resp_orig_ts_secs_high = htons(tresp->_sec.high);
925 follow_up->resp_orig_ts_secs_low = htonl(tresp->_sec.low);
926 follow_up->resp_orig_ts_nsecs = htonl(tresp->nanosecond);
927
928 GPTP_STATS_INC(port, tx_pdelay_resp_fup_count);
929
930 NET_GPTP_INFO("PDELAY_FOLLOWUP", pkt);
931
932 net_if_queue_tx(net_pkt_iface(pkt), pkt);
933 }
934
