1 /*
2 * Copyright (c) 2024 BayLibre SAS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(ptp_msg, CONFIG_PTP_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/net/udp.h>
12 #include <zephyr/net/ptp.h>
13
14 #include "clock.h"
15 #include "msg.h"
16 #include "port.h"
17 #include "tlv.h"
18 #include "transport.h"
19
20 static struct k_mem_slab msg_slab;
21
22 K_MEM_SLAB_DEFINE_STATIC(msg_slab, sizeof(struct ptp_msg), CONFIG_PTP_MSG_POLL_SIZE, 8);
23
msg_type_str(struct ptp_msg * msg)24 static const char *msg_type_str(struct ptp_msg *msg)
25 {
26 switch (ptp_msg_type(msg)) {
27 case PTP_MSG_SYNC:
28 return "Sync";
29 case PTP_MSG_DELAY_REQ:
30 return "Delay_Req";
31 case PTP_MSG_PDELAY_REQ:
32 return "Pdelay_Req";
33 case PTP_MSG_PDELAY_RESP:
34 return "Pdelay_Resp";
35 case PTP_MSG_FOLLOW_UP:
36 return "Follow_Up";
37 case PTP_MSG_DELAY_RESP:
38 return "Delay_Resp";
39 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
40 return "Pdelay_Resp_Follow_Up";
41 case PTP_MSG_ANNOUNCE:
42 return "Announce";
43 case PTP_MSG_SIGNALING:
44 return "Signaling";
45 case PTP_MSG_MANAGEMENT:
46 return "Management";
47 default:
48 return "Not recognized";
49 }
50 }
51
msg_timestamp_post_recv(struct ptp_msg * msg,struct ptp_timestamp * ts)52 static void msg_timestamp_post_recv(struct ptp_msg *msg, struct ptp_timestamp *ts)
53 {
54 msg->timestamp.protocol._sec.high = ntohs(ts->seconds_high);
55 msg->timestamp.protocol._sec.low = ntohl(ts->seconds_low);
56 msg->timestamp.protocol.nanosecond = ntohl(ts->nanoseconds);
57 }
58
msg_timestamp_pre_send(struct ptp_timestamp * ts)59 static void msg_timestamp_pre_send(struct ptp_timestamp *ts)
60 {
61 ts->seconds_high = htons(ts->seconds_high);
62 ts->seconds_low = htonl(ts->seconds_low);
63 ts->nanoseconds = htonl(ts->nanoseconds);
64 }
65
msg_port_id_post_recv(struct ptp_port_id * port_id)66 static void msg_port_id_post_recv(struct ptp_port_id *port_id)
67 {
68 port_id->port_number = ntohs(port_id->port_number);
69 }
70
msg_port_id_pre_send(struct ptp_port_id * port_id)71 static void msg_port_id_pre_send(struct ptp_port_id *port_id)
72 {
73 port_id->port_number = htons(port_id->port_number);
74 }
75
msg_header_post_recv(struct ptp_header * header)76 static int msg_header_post_recv(struct ptp_header *header)
77 {
78 if ((header->version & 0xF) != PTP_MAJOR_VERSION) {
79 /* Incompatible protocol version */
80 return -1;
81 }
82
83 header->msg_length = ntohs(header->msg_length);
84 header->correction = ntohll(header->correction);
85 header->sequence_id = ntohs(header->sequence_id);
86
87 msg_port_id_post_recv(&header->src_port_id);
88
89 return 0;
90 }
91
msg_header_pre_send(struct ptp_header * header)92 static void msg_header_pre_send(struct ptp_header *header)
93 {
94 header->msg_length = htons(header->msg_length);
95 header->correction = htonll(header->correction);
96 header->sequence_id = htons(header->sequence_id);
97
98 msg_port_id_pre_send(&header->src_port_id);
99 }
100
msg_suffix(struct ptp_msg * msg)101 static uint8_t *msg_suffix(struct ptp_msg *msg)
102 {
103 uint8_t *suffix = NULL;
104
105 switch (ptp_msg_type(msg)) {
106 case PTP_MSG_SYNC:
107 suffix = msg->sync.suffix;
108 break;
109 case PTP_MSG_DELAY_REQ:
110 suffix = msg->delay_req.suffix;
111 break;
112 case PTP_MSG_PDELAY_REQ:
113 suffix = msg->pdelay_req.suffix;
114 break;
115 case PTP_MSG_PDELAY_RESP:
116 suffix = msg->pdelay_resp.suffix;
117 break;
118 case PTP_MSG_FOLLOW_UP:
119 suffix = msg->follow_up.suffix;
120 break;
121 case PTP_MSG_DELAY_RESP:
122 suffix = msg->delay_resp.suffix;
123 break;
124 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
125 suffix = msg->pdelay_resp_follow_up.suffix;
126 break;
127 case PTP_MSG_ANNOUNCE:
128 suffix = msg->announce.suffix;
129 break;
130 case PTP_MSG_SIGNALING:
131 suffix = msg->signaling.suffix;
132 break;
133 case PTP_MSG_MANAGEMENT:
134 suffix = msg->management.suffix;
135 break;
136 }
137
138 return suffix;
139 }
140
msg_tlv_post_recv(struct ptp_msg * msg,int length)141 static int msg_tlv_post_recv(struct ptp_msg *msg, int length)
142 {
143 int suffix_len = 0, ret = 0;
144 struct ptp_tlv_container *tlv_container;
145 uint8_t *suffix = msg_suffix(msg);
146
147 if (!suffix) {
148 LOG_DBG("No TLV attached to the message");
149 return 0;
150 }
151
152 while (length >= sizeof(struct ptp_tlv)) {
153 tlv_container = ptp_tlv_alloc();
154 if (!tlv_container) {
155 return -ENOMEM;
156 }
157
158 tlv_container->tlv = (struct ptp_tlv *)suffix;
159 tlv_container->tlv->type = ntohs(tlv_container->tlv->type);
160 tlv_container->tlv->length = ntohs(tlv_container->tlv->length);
161
162 if (tlv_container->tlv->length % 2) {
163 /* IEEE 1588-2019 Section 5.3.8 - length is an even number */
164 LOG_ERR("Incorrect length of TLV");
165 ptp_tlv_free(tlv_container);
166 return -EBADMSG;
167 }
168
169 length -= sizeof(struct ptp_tlv);
170 suffix += sizeof(struct ptp_tlv);
171 suffix_len += sizeof(struct ptp_tlv);
172
173 if (tlv_container->tlv->length > length) {
174 LOG_ERR("Incorrect length of TLV");
175 ptp_tlv_free(tlv_container);
176 return -EBADMSG;
177 }
178
179 length -= tlv_container->tlv->length;
180 suffix += tlv_container->tlv->length;
181 suffix_len += tlv_container->tlv->length;
182
183 ret = ptp_tlv_post_recv(tlv_container->tlv);
184 if (ret) {
185 ptp_tlv_free(tlv_container);
186 return ret;
187 }
188
189 sys_slist_append(&msg->tlvs, &tlv_container->node);
190 }
191
192 return suffix_len;
193 }
194
msg_tlv_free(struct ptp_msg * msg)195 static void msg_tlv_free(struct ptp_msg *msg)
196 {
197 struct ptp_tlv_container *tlv_container;
198
199 for (sys_snode_t *iter = sys_slist_get(&msg->tlvs);
200 iter;
201 iter = sys_slist_get(&msg->tlvs)) {
202 tlv_container = CONTAINER_OF(iter, struct ptp_tlv_container, node);
203 ptp_tlv_free(tlv_container);
204 }
205 }
206
msg_tlv_pre_send(struct ptp_msg * msg)207 static void msg_tlv_pre_send(struct ptp_msg *msg)
208 {
209 struct ptp_tlv_container *tlv_container;
210
211 SYS_SLIST_FOR_EACH_CONTAINER(&msg->tlvs, tlv_container, node) {
212 ptp_tlv_pre_send(tlv_container->tlv);
213 }
214
215 /* No need to track TLVs attached to the message. */
216 msg_tlv_free(msg);
217 }
218
ptp_msg_alloc(void)219 struct ptp_msg *ptp_msg_alloc(void)
220 {
221 struct ptp_msg *msg = NULL;
222 int ret = k_mem_slab_alloc(&msg_slab, (void **)&msg, K_FOREVER);
223
224 if (ret) {
225 LOG_ERR("Couldn't allocate memory for the message");
226 return NULL;
227 }
228
229 memset(msg, 0, sizeof(*msg));
230 sys_slist_init(&msg->tlvs);
231 atomic_inc(&msg->ref);
232
233 return msg;
234 }
235
ptp_msg_unref(struct ptp_msg * msg)236 void ptp_msg_unref(struct ptp_msg *msg)
237 {
238 __ASSERT_NO_MSG(msg != NULL);
239
240 atomic_t val = atomic_dec(&msg->ref);
241
242 if (val > 1) {
243 return;
244 }
245
246 msg_tlv_free(msg);
247 k_mem_slab_free(&msg_slab, (void *)msg);
248 }
249
ptp_msg_ref(struct ptp_msg * msg)250 void ptp_msg_ref(struct ptp_msg *msg)
251 {
252 __ASSERT_NO_MSG(msg != NULL);
253
254 atomic_inc(&msg->ref);
255 }
256
ptp_msg_type(const struct ptp_msg * msg)257 enum ptp_msg_type ptp_msg_type(const struct ptp_msg *msg)
258 {
259 return (enum ptp_msg_type)(msg->header.type_major_sdo_id & 0xF);
260 }
261
ptp_msg_from_pkt(struct net_pkt * pkt)262 struct ptp_msg *ptp_msg_from_pkt(struct net_pkt *pkt)
263 {
264 static const size_t eth_hdr_len = IS_ENABLED(CONFIG_NET_VLAN) ?
265 sizeof(struct net_eth_vlan_hdr) :
266 sizeof(struct net_eth_hdr);
267 struct net_udp_hdr *hdr;
268 struct ptp_msg *msg;
269 int port, payload;
270
271 if (pkt->buffer->len == eth_hdr_len) {
272 /* Packet contain Ethernet header at the beginning. */
273 struct net_buf *buf;
274
275 /* remove packet temporarily. */
276 buf = pkt->buffer;
277 pkt->buffer = buf->frags;
278
279 hdr = net_udp_get_hdr(pkt, NULL);
280
281 /* insert back temporarily femoved frag. */
282 net_pkt_frag_insert(pkt, buf);
283 } else {
284 hdr = net_udp_get_hdr(pkt, NULL);
285 }
286
287 if (!hdr) {
288 LOG_ERR("Couldn't retrieve UDP header from the net packet");
289 return NULL;
290 }
291
292 payload = ntohs(hdr->len) - NET_UDPH_LEN;
293 port = ntohs(hdr->dst_port);
294
295 if (port != PTP_SOCKET_PORT_EVENT && port != PTP_SOCKET_PORT_GENERAL) {
296 LOG_ERR("Couldn't retrieve PTP message from the net packet");
297 return NULL;
298 }
299
300 msg = (struct ptp_msg *)((uintptr_t)hdr + NET_UDPH_LEN);
301
302 if (payload == ntohs(msg->header.msg_length)) {
303 return msg;
304 }
305
306 return NULL;
307 }
308
ptp_msg_pre_send(struct ptp_msg * msg)309 void ptp_msg_pre_send(struct ptp_msg *msg)
310 {
311 int64_t current;
312
313 msg_header_pre_send(&msg->header);
314
315 switch (ptp_msg_type(msg)) {
316 case PTP_MSG_SYNC:
317 break;
318 case PTP_MSG_DELAY_REQ:
319 current = k_uptime_get();
320
321 msg->timestamp.host.second = (uint64_t)(current / MSEC_PER_SEC);
322 msg->timestamp.host.nanosecond = (current % MSEC_PER_SEC) * NSEC_PER_MSEC;
323 break;
324 case PTP_MSG_PDELAY_REQ:
325 break;
326 case PTP_MSG_PDELAY_RESP:
327 msg_timestamp_pre_send(&msg->pdelay_resp.req_receipt_timestamp);
328 msg_port_id_pre_send(&msg->pdelay_resp.req_port_id);
329 break;
330 case PTP_MSG_FOLLOW_UP:
331 msg_timestamp_pre_send(&msg->follow_up.precise_origin_timestamp);
332 break;
333 case PTP_MSG_DELAY_RESP:
334 msg_timestamp_pre_send(&msg->delay_resp.receive_timestamp);
335 msg_port_id_pre_send(&msg->delay_resp.req_port_id);
336 break;
337 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
338 msg_timestamp_pre_send(&msg->pdelay_resp_follow_up.resp_origin_timestamp);
339 msg_port_id_pre_send(&msg->pdelay_resp_follow_up.req_port_id);
340 break;
341 case PTP_MSG_ANNOUNCE:
342 msg->announce.current_utc_offset = htons(msg->announce.current_utc_offset);
343 msg->announce.gm_clk_quality.offset_scaled_log_variance =
344 htons(msg->announce.gm_clk_quality.offset_scaled_log_variance);
345 msg->announce.steps_rm = htons(msg->announce.steps_rm);
346 break;
347 case PTP_MSG_SIGNALING:
348 msg_port_id_pre_send(&msg->signaling.target_port_id);
349 break;
350 case PTP_MSG_MANAGEMENT:
351 msg_port_id_pre_send(&msg->management.target_port_id);
352 break;
353 }
354
355 msg_tlv_pre_send(msg);
356 }
357
ptp_msg_post_recv(struct ptp_port * port,struct ptp_msg * msg,int cnt)358 int ptp_msg_post_recv(struct ptp_port *port, struct ptp_msg *msg, int cnt)
359 {
360 static const int msg_size[] = {
361 [PTP_MSG_SYNC] = sizeof(struct ptp_sync_msg),
362 [PTP_MSG_DELAY_REQ] = sizeof(struct ptp_delay_req_msg),
363 [PTP_MSG_PDELAY_REQ] = sizeof(struct ptp_pdelay_req_msg),
364 [PTP_MSG_PDELAY_RESP] = sizeof(struct ptp_pdelay_resp_msg),
365 [PTP_MSG_FOLLOW_UP] = sizeof(struct ptp_follow_up_msg),
366 [PTP_MSG_DELAY_RESP] = sizeof(struct ptp_delay_resp_msg),
367 [PTP_MSG_PDELAY_RESP_FOLLOW_UP] = sizeof(struct ptp_pdelay_resp_follow_up_msg),
368 [PTP_MSG_ANNOUNCE] = sizeof(struct ptp_announce_msg),
369 [PTP_MSG_SIGNALING] = sizeof(struct ptp_signaling_msg),
370 [PTP_MSG_MANAGEMENT] = sizeof(struct ptp_management_msg),
371 };
372 enum ptp_msg_type type = ptp_msg_type(msg);
373 int64_t current;
374 int tlv_len;
375
376 if (msg_size[type] > cnt) {
377 LOG_ERR("Received message with incorrect length");
378 return -EBADMSG;
379 }
380
381 if (msg_header_post_recv(&msg->header)) {
382 LOG_ERR("Received message incomplient with supported PTP version");
383 return -EBADMSG;
384 }
385
386 LOG_DBG("Port %d received %s message", port->port_ds.id.port_number, msg_type_str(msg));
387
388 switch (type) {
389 case PTP_MSG_SYNC:
390 msg_timestamp_post_recv(msg, &msg->sync.origin_timestamp);
391 break;
392 case PTP_MSG_DELAY_REQ:
393 break;
394 case PTP_MSG_PDELAY_REQ:
395 break;
396 case PTP_MSG_PDELAY_RESP:
397 msg_timestamp_post_recv(msg, &msg->pdelay_resp.req_receipt_timestamp);
398 msg_port_id_post_recv(&msg->pdelay_resp.req_port_id);
399 break;
400 case PTP_MSG_FOLLOW_UP:
401 msg_timestamp_post_recv(msg, &msg->follow_up.precise_origin_timestamp);
402 break;
403 case PTP_MSG_DELAY_RESP:
404 msg_timestamp_post_recv(msg, &msg->delay_resp.receive_timestamp);
405 msg_port_id_post_recv(&msg->delay_resp.req_port_id);
406 break;
407 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
408 msg_timestamp_post_recv(msg, &msg->pdelay_resp_follow_up.resp_origin_timestamp);
409 msg_port_id_post_recv(&msg->pdelay_resp_follow_up.req_port_id);
410 break;
411 case PTP_MSG_ANNOUNCE:
412 current = k_uptime_get();
413
414 msg->timestamp.host.second = (uint64_t)(current / MSEC_PER_SEC);
415 msg->timestamp.host.nanosecond = (current % MSEC_PER_SEC) * NSEC_PER_MSEC;
416 msg_timestamp_post_recv(msg, &msg->announce.origin_timestamp);
417 msg->announce.current_utc_offset = ntohs(msg->announce.current_utc_offset);
418 msg->announce.gm_clk_quality.offset_scaled_log_variance =
419 ntohs(msg->announce.gm_clk_quality.offset_scaled_log_variance);
420 msg->announce.steps_rm = ntohs(msg->announce.steps_rm);
421 break;
422 case PTP_MSG_SIGNALING:
423 msg_port_id_post_recv(&msg->signaling.target_port_id);
424 break;
425 case PTP_MSG_MANAGEMENT:
426 msg_port_id_post_recv(&msg->management.target_port_id);
427 break;
428 }
429
430 tlv_len = msg_tlv_post_recv(msg, cnt - msg_size[type]);
431 if (tlv_len < 0) {
432 LOG_ERR("Failed processing TLVs");
433 return -EBADMSG;
434 }
435
436 if (msg_size[type] + tlv_len != msg->header.msg_length) {
437 LOG_ERR("Length and TLVs don't correspond with specified in the message");
438 return -EMSGSIZE;
439 }
440
441 return 0;
442 }
443
ptp_msg_add_tlv(struct ptp_msg * msg,int length)444 struct ptp_tlv *ptp_msg_add_tlv(struct ptp_msg *msg, int length)
445 {
446 struct ptp_tlv_container *tlv_container;
447 uint8_t *suffix = msg_suffix(msg);
448
449 if (!suffix) {
450 return NULL;
451 }
452
453 tlv_container = (struct ptp_tlv_container *)sys_slist_peek_tail(&msg->tlvs);
454 if (tlv_container) {
455 suffix = (uint8_t *)tlv_container->tlv;
456 suffix += sizeof(*tlv_container->tlv);
457 suffix += tlv_container->tlv->length;
458 }
459
460 if ((intptr_t)(suffix + length) >= (intptr_t)&msg->ref) {
461 LOG_ERR("Not enough space for TLV of %d length", length);
462 return NULL;
463 }
464
465 tlv_container = ptp_tlv_alloc();
466 if (tlv_container) {
467 tlv_container->tlv = (struct ptp_tlv *)suffix;
468 msg->header.msg_length += length;
469 }
470
471 return tlv_container ? tlv_container->tlv : NULL;
472 }
473
ptp_msg_announce_cmp(const struct ptp_announce_msg * m1,const struct ptp_announce_msg * m2)474 int ptp_msg_announce_cmp(const struct ptp_announce_msg *m1, const struct ptp_announce_msg *m2)
475 {
476 int len = sizeof(m1->gm_priority1) + sizeof(m1->gm_clk_quality) +
477 sizeof(m1->gm_priority1) + sizeof(m1->gm_id) +
478 sizeof(m1->steps_rm);
479
480 return memcmp(&m1->gm_priority1, &m2->gm_priority1, len);
481 }
482
ptp_msg_current_parent(const struct ptp_msg * msg)483 bool ptp_msg_current_parent(const struct ptp_msg *msg)
484 {
485 const struct ptp_parent_ds *pds = ptp_clock_parent_ds();
486
487 return ptp_port_id_eq(&pds->port_id, &msg->header.src_port_id);
488 }
489
