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