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
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
245 NET_ASSERT(sync);
246 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
247 iface = GPTP_PORT_IFACE(port);
248 NET_ASSERT(iface);
249
250 pkt = setup_gptp_frame(iface, sizeof(struct gptp_follow_up));
251 if (!pkt) {
252 NET_DBG("Cannot get gPTP frame");
253 return NULL;
254 }
255
256 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
257
258 hdr = GPTP_HDR(pkt);
259 fup = GPTP_FOLLOW_UP(pkt);
260 sync_hdr = GPTP_HDR(sync);
261
262 /*
263 * Header configuration.
264 *
265 * Some fields are set by gptp_md_follow_up_prepare().
266 */
267 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
268 hdr->message_type = GPTP_FOLLOWUP_MESSAGE;
269 hdr->ptp_version = GPTP_VERSION;
270 hdr->sequence_id = sync_hdr->sequence_id;
271 hdr->domain_number = 0U;
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_IC);
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_IC);
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_CA);
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 struct gptp_priority_vector *gm_prio;
478
479 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
480 global_ds = GPTP_GLOBAL_DS();
481 default_ds = GPTP_DEFAULT_DS();
482 iface = GPTP_PORT_IFACE(port);
483 NET_ASSERT(iface);
484
485 pkt = setup_gptp_frame(iface, sizeof(struct gptp_announce) - 8 +
486 ntohs(global_ds->path_trace.len));
487 if (!pkt) {
488 NET_DBG("Cannot get gPTP frame");
489 return NULL;
490 }
491
492 net_pkt_set_priority(pkt, NET_PRIORITY_CA);
493
494 hdr = GPTP_HDR(pkt);
495 ann = GPTP_ANNOUNCE(pkt);
496 port_ds = GPTP_PORT_DS(port);
497
498 hdr->message_type = GPTP_ANNOUNCE_MESSAGE;
499 hdr->transport_specific = GPTP_TRANSPORT_802_1_AS;
500 hdr->ptp_version = GPTP_VERSION;
501
502 hdr->domain_number = 0U;
503 hdr->correction_field = 0;
504 hdr->flags.octets[0] = 0U;
505
506 /* Copy leap61, leap59, current UTC offset valid, time traceable and
507 * frequency traceable flags.
508 */
509 hdr->flags.octets[1] =
510 global_ds->global_flags.octets[1] | GPTP_FLAG_PTP_TIMESCALE;
511
512 memcpy(hdr->port_id.clk_id, GPTP_DEFAULT_DS()->clk_id,
513 GPTP_CLOCK_ID_LEN);
514
515 hdr->port_id.port_number = htons(port);
516 hdr->control = GPTP_OTHER_CONTROL_VALUE;
517 hdr->log_msg_interval = port_ds->cur_log_announce_itv;
518
519 /* Clear reserved fields. */
520 hdr->reserved0 = 0U;
521 hdr->reserved1 = 0U;
522 hdr->reserved2 = 0U;
523
524 ann->cur_utc_offset = htons(global_ds->current_utc_offset);
525 ann->time_source = global_ds->time_source;
526
527 gm_prio = &global_ds->gm_priority;
528 switch (GPTP_PORT_BMCA_DATA(port)->info_is) {
529 case GPTP_INFO_IS_MINE:
530 case GPTP_INFO_IS_RECEIVED:
531 memcpy(&ann->root_system_id,
532 &gm_prio->root_system_id,
533 sizeof(struct gptp_root_system_identity));
534 break;
535 default:
536 goto fail;
537 }
538
539 ann->steps_removed = global_ds->master_steps_removed;
540 hdr->sequence_id = htons(port_ds->announce_seq_id);
541 port_ds->announce_seq_id++;
542
543 ann->tlv.type = GPTP_ANNOUNCE_MSG_PATH_SEQ_TYPE;
544
545 /* Clear reserved fields. */
546 (void)memset(ann->reserved1, 0, sizeof(ann->reserved1));
547 ann->reserved2 = 0U;
548
549 hdr->message_length = htons(sizeof(struct gptp_hdr) +
550 sizeof(struct gptp_announce) - 8 +
551 ntohs(global_ds->path_trace.len));
552
553 ann->tlv.len = global_ds->path_trace.len;
554
555 net_pkt_cursor_init(pkt);
556
557 /* setup_gptp_frame() already added all the length, so let's not
558 * add up more with net_pkt_skip/write_new(), let's just overwrite
559 */
560 net_pkt_set_overwrite(pkt, true);
561
562 if (net_pkt_skip(pkt, sizeof(struct gptp_hdr) +
563 sizeof(struct gptp_announce) - 8) ||
564 net_pkt_write(pkt, &global_ds->path_trace.path_sequence[0][0],
565 ntohs(global_ds->path_trace.len))) {
566 goto fail;
567 }
568
569 net_pkt_set_overwrite(pkt, false);
570
571 return pkt;
572
573 fail:
574 net_pkt_unref(pkt);
575
576 return NULL;
577 }
578
gptp_handle_sync(int port,struct net_pkt * pkt)579 void gptp_handle_sync(int port, struct net_pkt *pkt)
580 {
581 struct gptp_sync_rcv_state *state;
582 struct gptp_hdr *hdr;
583 uint64_t upstream_sync_itv;
584 k_timeout_t duration;
585
586 state = &GPTP_PORT_STATE(port)->sync_rcv;
587 hdr = GPTP_HDR(state->rcvd_sync_ptr);
588
589 upstream_sync_itv = NSEC_PER_SEC * GPTP_POW2(hdr->log_msg_interval);
590
591 /* Convert ns to ms. */
592 duration = K_MSEC((upstream_sync_itv / 1000000U));
593
594 /* Start timeout timer. */
595 k_timer_start(&state->follow_up_discard_timer, duration, K_NO_WAIT);
596 }
597
gptp_handle_follow_up(int port,struct net_pkt * pkt)598 int gptp_handle_follow_up(int port, struct net_pkt *pkt)
599 {
600 struct gptp_sync_rcv_state *state;
601 struct gptp_hdr *sync_hdr, *hdr;
602
603 state = &GPTP_PORT_STATE(port)->sync_rcv;
604
605 sync_hdr = GPTP_HDR(state->rcvd_sync_ptr);
606 hdr = GPTP_HDR(pkt);
607
608 if (sync_hdr->sequence_id != hdr->sequence_id) {
609 NET_WARN("%s sequence id %d %s %s %d",
610 "FOLLOWUP", ntohs(hdr->sequence_id),
611 "does not match",
612 "SYNC", ntohs(sync_hdr->sequence_id));
613 return -EINVAL;
614 }
615
616 GPTP_STATS_INC(port, rx_fup_count);
617
618 return 0;
619 }
620
gptp_handle_pdelay_req(int port,struct net_pkt * pkt)621 void gptp_handle_pdelay_req(int port, struct net_pkt *pkt)
622 {
623 struct net_pkt *reply;
624
625 GPTP_STATS_INC(port, rx_pdelay_req_count);
626
627 if (ts_cb_registered[port - 1] == true) {
628 NET_WARN("Multiple pdelay requests");
629
630 net_if_unregister_timestamp_cb(&pdelay_response_timestamp_cb[port - 1]);
631 net_pkt_unref(pdelay_response_timestamp_cb[port - 1].pkt);
632
633 ts_cb_registered[port - 1] = false;
634 }
635
636 /* Prepare response and send */
637 reply = gptp_prepare_pdelay_resp(port, pkt);
638 if (!reply) {
639 return;
640 }
641
642 net_if_register_timestamp_cb(&pdelay_response_timestamp_cb[port - 1],
643 reply,
644 net_pkt_iface(pkt),
645 gptp_pdelay_response_timestamp_callback);
646
647 /* TS thread will send this back to us so increment ref count so that
648 * the packet is not removed when sending it. This will be unref'ed by
649 * timestamp callback in gptp_pdelay_response_timestamp_callback()
650 */
651 net_pkt_ref(reply);
652
653 ts_cb_registered[port - 1] = true;
654
655 gptp_send_pdelay_resp(port, reply, net_pkt_timestamp(pkt));
656 }
657
gptp_handle_pdelay_resp(int port,struct net_pkt * pkt)658 int gptp_handle_pdelay_resp(int port, struct net_pkt *pkt)
659 {
660 struct gptp_pdelay_req_state *state;
661 struct gptp_default_ds *default_ds;
662 struct gptp_pdelay_resp *resp;
663 struct gptp_hdr *hdr, *req_hdr;
664
665 hdr = GPTP_HDR(pkt);
666 resp = GPTP_PDELAY_RESP(pkt);
667 state = &GPTP_PORT_STATE(port)->pdelay_req;
668 default_ds = GPTP_DEFAULT_DS();
669
670 if (!state->tx_pdelay_req_ptr) {
671 goto reset;
672 }
673
674 req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr);
675
676 /* Check clock identity. */
677 if (memcmp(default_ds->clk_id, resp->requesting_port_id.clk_id,
678 GPTP_CLOCK_ID_LEN)) {
679 NET_WARN("Requesting Clock Identity %s", "does not match");
680 goto reset;
681 }
682 if (memcmp(default_ds->clk_id, hdr->port_id.clk_id,
683 GPTP_CLOCK_ID_LEN) == 0) {
684 NET_WARN("Source Clock Identity is local Clock Identity");
685 goto reset;
686 }
687
688 /* Check port number. */
689 if (resp->requesting_port_id.port_number != htons(port)) {
690 NET_WARN("Requesting Port Number %s", "does not match");
691 goto reset;
692 }
693
694 /* Check sequence id. */
695 if (hdr->sequence_id != req_hdr->sequence_id) {
696 NET_WARN("Sequence Id %d %s %d",
697 ntohs(hdr->sequence_id),
698 "does not match",
699 ntohs(req_hdr->sequence_id));
700 goto reset;
701 }
702
703 GPTP_STATS_INC(port, rx_pdelay_resp_count);
704
705 return 0;
706
707 reset:
708 return -EINVAL;
709 }
710
gptp_handle_pdelay_follow_up(int port,struct net_pkt * pkt)711 int gptp_handle_pdelay_follow_up(int port, struct net_pkt *pkt)
712 {
713 struct gptp_pdelay_resp_follow_up *follow_up;
714 struct gptp_hdr *hdr, *req_hdr, *resp_hdr;
715 struct gptp_pdelay_req_state *state;
716 struct gptp_default_ds *default_ds;
717
718 hdr = GPTP_HDR(pkt);
719 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
720 state = &GPTP_PORT_STATE(port)->pdelay_req;
721 default_ds = GPTP_DEFAULT_DS();
722
723 if (!state->tx_pdelay_req_ptr) {
724 goto reset;
725 }
726
727 req_hdr = GPTP_HDR(state->tx_pdelay_req_ptr);
728
729 if (!state->rcvd_pdelay_resp_ptr) {
730 goto reset;
731 }
732
733 resp_hdr = GPTP_HDR(state->rcvd_pdelay_resp_ptr);
734
735 /* Check clock identity. */
736 if (memcmp(default_ds->clk_id, follow_up->requesting_port_id.clk_id,
737 GPTP_CLOCK_ID_LEN)) {
738 NET_WARN("Requesting Clock Identity %s", "does not match");
739 goto reset;
740 }
741
742 if (memcmp(default_ds->clk_id, hdr->port_id.clk_id,
743 GPTP_CLOCK_ID_LEN) == 0) {
744 NET_WARN("Source Clock Identity is local Clock Identity");
745 goto reset;
746 }
747
748 /* Check port number. */
749 if (follow_up->requesting_port_id.port_number != htons(port)) {
750 NET_WARN("Requesting Port Number %s", "does not match");
751 goto reset;
752 }
753
754 /* Check sequence id. */
755 if (hdr->sequence_id != req_hdr->sequence_id) {
756 NET_WARN("Sequence Id %d %s %d",
757 ntohs(hdr->sequence_id),
758 "does not match",
759 ntohs(req_hdr->sequence_id));
760 goto reset;
761 }
762
763 /* Check source port. */
764 if (memcmp(&hdr->port_id, &resp_hdr->port_id,
765 sizeof(hdr->port_id)) != 0) {
766 NET_WARN("pDelay response and follow up port IDs %s",
767 "does not match");
768 goto reset;
769 }
770
771 GPTP_STATS_INC(port, rx_fup_count);
772
773 return 0;
774
775 reset:
776 return -EINVAL;
777 }
778
gptp_handle_signaling(int port,struct net_pkt * pkt)779 void gptp_handle_signaling(int port, struct net_pkt *pkt)
780 {
781 struct gptp_port_ds *port_ds;
782 struct gptp_signaling *sig;
783
784 sig = GPTP_SIGNALING(pkt);
785 port_ds = GPTP_PORT_DS(port);
786
787 /* If time-synchronization not enabled, drop packet. */
788 if (!port_ds->ptt_port_enabled) {
789 return;
790 }
791
792 /* pDelay interval. */
793 gptp_update_pdelay_req_interval(port, sig->tlv.link_delay_itv);
794
795 /* Sync interval. */
796 gptp_update_sync_interval(port, sig->tlv.time_sync_itv);
797
798 /* Announce interval. */
799 gptp_update_announce_interval(port, sig->tlv.announce_itv);
800
801 port_ds->compute_neighbor_rate_ratio =
802 sig->tlv.compute_neighbor_rate_ratio;
803 port_ds->compute_neighbor_prop_delay =
804 sig->tlv.compute_neighbor_prop_delay;
805 }
806
gptp_send_sync(int port,struct net_pkt * pkt)807 void gptp_send_sync(int port, struct net_pkt *pkt)
808 {
809 if (!sync_cb_registered[port - 1]) {
810 net_if_register_timestamp_cb(&sync_timestamp_cb[port - 1],
811 pkt,
812 net_pkt_iface(pkt),
813 gptp_sync_timestamp_callback);
814 sync_cb_registered[port - 1] = true;
815 }
816
817 GPTP_STATS_INC(port, tx_sync_count);
818
819 /* TS thread will send this back to us so increment ref count
820 * so that the packet is not removed when sending it.
821 * This will be unref'ed by timestamp callback in
822 * gptp_sync_timestamp_callback()
823 */
824 net_pkt_ref(pkt);
825
826 NET_GPTP_INFO("SYNC", pkt);
827
828 net_if_queue_tx(net_pkt_iface(pkt), pkt);
829 }
830
gptp_send_follow_up(int port,struct net_pkt * pkt)831 void gptp_send_follow_up(int port, struct net_pkt *pkt)
832 {
833 GPTP_STATS_INC(port, tx_fup_count);
834
835 NET_GPTP_INFO("FOLLOWUP", pkt);
836
837 net_if_queue_tx(net_pkt_iface(pkt), pkt);
838 }
839
gptp_send_announce(int port,struct net_pkt * pkt)840 void gptp_send_announce(int port, struct net_pkt *pkt)
841 {
842 GPTP_STATS_INC(port, tx_announce_count);
843
844 NET_GPTP_INFO("ANNOUNCE", pkt);
845
846 net_if_queue_tx(net_pkt_iface(pkt), pkt);
847 }
848
gptp_send_pdelay_req(int port)849 void gptp_send_pdelay_req(int port)
850 {
851 struct gptp_pdelay_req_state *state;
852 struct net_pkt *pkt;
853
854 NET_ASSERT((port >= GPTP_PORT_START) && (port <= GPTP_PORT_END));
855 state = &GPTP_PORT_STATE(port)->pdelay_req;
856
857 pkt = gptp_prepare_pdelay_req(port);
858 if (pkt) {
859 if (state->tx_pdelay_req_ptr) {
860 NET_DBG("Unref pending %s %p", "PDELAY_REQ",
861 state->tx_pdelay_req_ptr);
862
863 net_pkt_unref(state->tx_pdelay_req_ptr);
864 }
865
866 /* Keep the buffer alive until pdelay_rate_ratio is computed. */
867 state->tx_pdelay_req_ptr = net_pkt_ref(pkt);
868
869 GPTP_STATS_INC(port, tx_pdelay_req_count);
870
871 NET_GPTP_INFO("PDELAY_REQ", pkt);
872
873 net_if_queue_tx(net_pkt_iface(pkt), pkt);
874 } else {
875 NET_ERR("Failed to prepare %s", "PDELAY_REQ");
876 }
877 }
878
gptp_send_pdelay_resp(int port,struct net_pkt * pkt,struct net_ptp_time * treq)879 void gptp_send_pdelay_resp(int port, struct net_pkt *pkt,
880 struct net_ptp_time *treq)
881 {
882 struct gptp_pdelay_resp *resp;
883 struct gptp_hdr *hdr;
884
885 hdr = GPTP_HDR(pkt);
886
887 /* No Fractional nsec .*/
888 hdr->correction_field = 0;
889
890 resp = GPTP_PDELAY_RESP(pkt);
891 resp->req_receipt_ts_secs_high = htons(treq->_sec.high);
892 resp->req_receipt_ts_secs_low = htonl(treq->_sec.low);
893 resp->req_receipt_ts_nsecs = htonl(treq->nanosecond);
894
895 GPTP_STATS_INC(port, tx_pdelay_resp_count);
896
897 NET_GPTP_INFO("PDELAY_RESP", pkt);
898
899 net_if_queue_tx(net_pkt_iface(pkt), pkt);
900 }
901
gptp_send_pdelay_follow_up(int port,struct net_pkt * pkt,struct net_ptp_time * tresp)902 void gptp_send_pdelay_follow_up(int port, struct net_pkt *pkt,
903 struct net_ptp_time *tresp)
904 {
905 struct gptp_pdelay_resp_follow_up *follow_up;
906 struct gptp_hdr *hdr;
907
908 hdr = GPTP_HDR(pkt);
909
910 /* No Fractional nsec .*/
911 hdr->correction_field = 0;
912
913 follow_up = GPTP_PDELAY_RESP_FOLLOWUP(pkt);
914 follow_up->resp_orig_ts_secs_high = htons(tresp->_sec.high);
915 follow_up->resp_orig_ts_secs_low = htonl(tresp->_sec.low);
916 follow_up->resp_orig_ts_nsecs = htonl(tresp->nanosecond);
917
918 GPTP_STATS_INC(port, tx_pdelay_resp_fup_count);
919
920 NET_GPTP_INFO("PDELAY_FOLLOWUP", pkt);
921
922 net_if_queue_tx(net_pkt_iface(pkt), pkt);
923 }
924
