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