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, 4);
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 = net_ntohs(ts->seconds_high);
55 msg->timestamp.protocol._sec.low = net_ntohl(ts->seconds_low);
56 msg->timestamp.protocol.nanosecond = net_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 = net_htons(ts->seconds_high);
62 ts->seconds_low = net_htonl(ts->seconds_low);
63 ts->nanoseconds = net_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 = net_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 = net_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 = net_ntohs(header->msg_length);
84 header->correction = net_ntohll(header->correction);
85 header->sequence_id = net_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 = net_htons(header->msg_length);
95 header->correction = net_htonll(header->correction);
96 header->sequence_id = net_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 = net_ntohs(tlv_container->tlv->type);
160 tlv_container->tlv->length = net_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 buf->frags = NULL;
279
280 hdr = net_udp_get_hdr(pkt, NULL);
281
282 /* insert back temporarily femoved frag. */
283 net_pkt_frag_insert(pkt, buf);
284 } else {
285 hdr = net_udp_get_hdr(pkt, NULL);
286 }
287
288 if (!hdr) {
289 LOG_ERR("Couldn't retrieve UDP header from the net packet");
290 return NULL;
291 }
292
293 payload = net_ntohs(hdr->len) - NET_UDPH_LEN;
294 port = net_ntohs(hdr->dst_port);
295
296 if (port != PTP_SOCKET_PORT_EVENT && port != PTP_SOCKET_PORT_GENERAL) {
297 LOG_ERR("Couldn't retrieve PTP message from the net packet");
298 return NULL;
299 }
300
301 msg = (struct ptp_msg *)((uintptr_t)hdr + NET_UDPH_LEN);
302
303 if (payload == net_ntohs(msg->header.msg_length)) {
304 return msg;
305 }
306
307 return NULL;
308 }
309
ptp_msg_pre_send(struct ptp_msg * msg)310 void ptp_msg_pre_send(struct ptp_msg *msg)
311 {
312 int64_t current;
313
314 msg_header_pre_send(&msg->header);
315
316 switch (ptp_msg_type(msg)) {
317 case PTP_MSG_SYNC:
318 break;
319 case PTP_MSG_DELAY_REQ:
320 current = k_uptime_get();
321
322 msg->timestamp.host.second = (uint64_t)(current / MSEC_PER_SEC);
323 msg->timestamp.host.nanosecond = (current % MSEC_PER_SEC) * NSEC_PER_MSEC;
324 break;
325 case PTP_MSG_PDELAY_REQ:
326 break;
327 case PTP_MSG_PDELAY_RESP:
328 msg_timestamp_pre_send(&msg->pdelay_resp.req_receipt_timestamp);
329 msg_port_id_pre_send(&msg->pdelay_resp.req_port_id);
330 break;
331 case PTP_MSG_FOLLOW_UP:
332 msg_timestamp_pre_send(&msg->follow_up.precise_origin_timestamp);
333 break;
334 case PTP_MSG_DELAY_RESP:
335 msg_timestamp_pre_send(&msg->delay_resp.receive_timestamp);
336 msg_port_id_pre_send(&msg->delay_resp.req_port_id);
337 break;
338 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
339 msg_timestamp_pre_send(&msg->pdelay_resp_follow_up.resp_origin_timestamp);
340 msg_port_id_pre_send(&msg->pdelay_resp_follow_up.req_port_id);
341 break;
342 case PTP_MSG_ANNOUNCE:
343 msg->announce.current_utc_offset = net_htons(msg->announce.current_utc_offset);
344 msg->announce.gm_clk_quality.offset_scaled_log_variance =
345 net_htons(msg->announce.gm_clk_quality.offset_scaled_log_variance);
346 msg->announce.steps_rm = net_htons(msg->announce.steps_rm);
347 break;
348 case PTP_MSG_SIGNALING:
349 msg_port_id_pre_send(&msg->signaling.target_port_id);
350 break;
351 case PTP_MSG_MANAGEMENT:
352 msg_port_id_pre_send(&msg->management.target_port_id);
353 break;
354 }
355
356 msg_tlv_pre_send(msg);
357 }
358
ptp_msg_post_recv(struct ptp_port * port,struct ptp_msg * msg,int cnt)359 int ptp_msg_post_recv(struct ptp_port *port, struct ptp_msg *msg, int cnt)
360 {
361 static const int msg_size[] = {
362 [PTP_MSG_SYNC] = sizeof(struct ptp_sync_msg),
363 [PTP_MSG_DELAY_REQ] = sizeof(struct ptp_delay_req_msg),
364 [PTP_MSG_PDELAY_REQ] = sizeof(struct ptp_pdelay_req_msg),
365 [PTP_MSG_PDELAY_RESP] = sizeof(struct ptp_pdelay_resp_msg),
366 [PTP_MSG_FOLLOW_UP] = sizeof(struct ptp_follow_up_msg),
367 [PTP_MSG_DELAY_RESP] = sizeof(struct ptp_delay_resp_msg),
368 [PTP_MSG_PDELAY_RESP_FOLLOW_UP] = sizeof(struct ptp_pdelay_resp_follow_up_msg),
369 [PTP_MSG_ANNOUNCE] = sizeof(struct ptp_announce_msg),
370 [PTP_MSG_SIGNALING] = sizeof(struct ptp_signaling_msg),
371 [PTP_MSG_MANAGEMENT] = sizeof(struct ptp_management_msg),
372 };
373 enum ptp_msg_type type = ptp_msg_type(msg);
374 int64_t current;
375 int tlv_len;
376
377 if (msg_size[type] > cnt) {
378 LOG_ERR("Received message with incorrect length");
379 return -EBADMSG;
380 }
381
382 if (msg_header_post_recv(&msg->header)) {
383 LOG_ERR("Received message incomplient with supported PTP version");
384 return -EBADMSG;
385 }
386
387 LOG_DBG("Port %d received %s message", port->port_ds.id.port_number, msg_type_str(msg));
388
389 switch (type) {
390 case PTP_MSG_SYNC:
391 msg_timestamp_post_recv(msg, &msg->sync.origin_timestamp);
392 break;
393 case PTP_MSG_DELAY_REQ:
394 break;
395 case PTP_MSG_PDELAY_REQ:
396 break;
397 case PTP_MSG_PDELAY_RESP:
398 msg_timestamp_post_recv(msg, &msg->pdelay_resp.req_receipt_timestamp);
399 msg_port_id_post_recv(&msg->pdelay_resp.req_port_id);
400 break;
401 case PTP_MSG_FOLLOW_UP:
402 msg_timestamp_post_recv(msg, &msg->follow_up.precise_origin_timestamp);
403 break;
404 case PTP_MSG_DELAY_RESP:
405 msg_timestamp_post_recv(msg, &msg->delay_resp.receive_timestamp);
406 msg_port_id_post_recv(&msg->delay_resp.req_port_id);
407 break;
408 case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
409 msg_timestamp_post_recv(msg, &msg->pdelay_resp_follow_up.resp_origin_timestamp);
410 msg_port_id_post_recv(&msg->pdelay_resp_follow_up.req_port_id);
411 break;
412 case PTP_MSG_ANNOUNCE:
413 current = k_uptime_get();
414
415 msg->timestamp.host.second = (uint64_t)(current / MSEC_PER_SEC);
416 msg->timestamp.host.nanosecond = (current % MSEC_PER_SEC) * NSEC_PER_MSEC;
417 msg_timestamp_post_recv(msg, &msg->announce.origin_timestamp);
418 msg->announce.current_utc_offset = net_ntohs(msg->announce.current_utc_offset);
419 msg->announce.gm_clk_quality.offset_scaled_log_variance =
420 net_ntohs(msg->announce.gm_clk_quality.offset_scaled_log_variance);
421 msg->announce.steps_rm = net_ntohs(msg->announce.steps_rm);
422 break;
423 case PTP_MSG_SIGNALING:
424 msg_port_id_post_recv(&msg->signaling.target_port_id);
425 break;
426 case PTP_MSG_MANAGEMENT:
427 msg_port_id_post_recv(&msg->management.target_port_id);
428 break;
429 }
430
431 tlv_len = msg_tlv_post_recv(msg, cnt - msg_size[type]);
432 if (tlv_len < 0) {
433 LOG_ERR("Failed processing TLVs");
434 return -EBADMSG;
435 }
436
437 if (msg_size[type] + tlv_len != msg->header.msg_length) {
438 LOG_ERR("Length and TLVs don't correspond with specified in the message");
439 return -EMSGSIZE;
440 }
441
442 return 0;
443 }
444
ptp_msg_add_tlv(struct ptp_msg * msg,int length)445 struct ptp_tlv *ptp_msg_add_tlv(struct ptp_msg *msg, int length)
446 {
447 struct ptp_tlv_container *tlv_container;
448 uint8_t *suffix = msg_suffix(msg);
449
450 if (!suffix) {
451 return NULL;
452 }
453
454 tlv_container = (struct ptp_tlv_container *)sys_slist_peek_tail(&msg->tlvs);
455 if (tlv_container) {
456 suffix = (uint8_t *)tlv_container->tlv;
457 suffix += sizeof(*tlv_container->tlv);
458 suffix += tlv_container->tlv->length;
459 }
460
461 if ((intptr_t)(suffix + length) >= (intptr_t)&msg->ref) {
462 LOG_ERR("Not enough space for TLV of %d length", length);
463 return NULL;
464 }
465
466 tlv_container = ptp_tlv_alloc();
467 if (tlv_container) {
468 tlv_container->tlv = (struct ptp_tlv *)suffix;
469 msg->header.msg_length += length;
470 }
471
472 return tlv_container ? tlv_container->tlv : NULL;
473 }
474
ptp_msg_announce_cmp(const struct ptp_announce_msg * m1,const struct ptp_announce_msg * m2)475 int ptp_msg_announce_cmp(const struct ptp_announce_msg *m1, const struct ptp_announce_msg *m2)
476 {
477 int len = sizeof(m1->gm_priority1) + sizeof(m1->gm_clk_quality) +
478 sizeof(m1->gm_priority1) + sizeof(m1->gm_id) +
479 sizeof(m1->steps_rm);
480
481 return memcmp(&m1->gm_priority1, &m2->gm_priority1, len);
482 }
483
ptp_msg_current_parent(const struct ptp_msg * msg)484 bool ptp_msg_current_parent(const struct ptp_msg *msg)
485 {
486 const struct ptp_parent_ds *pds = ptp_clock_parent_ds();
487
488 return ptp_port_id_eq(&pds->port_id, &msg->header.src_port_id);
489 }
490
