ProtoCore v0.0.2
Deterministic, zero-heap network stack for embedded targets
Loading...
Searching...
No Matches
ptp.cpp
Go to the documentation of this file.
1// Copyright (C) 2026 Douglas Quigg (dstroy0) <dquigg123@gmail.com>
2// SPDX-License-Identifier: AGPL-3.0-or-later
3
4/**
5 * @file ptp.cpp
6 * @brief PTP / IEEE 1588-2008 (PTPv2) message codec + slave math (pure, host-tested).
7 */
8
9#include "services/timing_position/ptp/ptp.h"
10
11#if PC_ENABLE_PTP
12
13#include <string.h>
14
15// -- big-endian field helpers --
16
17static void put_u16(uint8_t *p, uint16_t v)
18{
19 p[0] = (uint8_t)(v >> 8);
20 p[1] = (uint8_t)v;
21}
22
23static uint16_t get_u16(const uint8_t *p)
24{
25 return (uint16_t)(((uint16_t)p[0] << 8) | p[1]);
26}
27
28static void put_u32(uint8_t *p, uint32_t v)
29{
30 p[0] = (uint8_t)(v >> 24);
31 p[1] = (uint8_t)(v >> 16);
32 p[2] = (uint8_t)(v >> 8);
33 p[3] = (uint8_t)v;
34}
35
36static uint32_t get_u32(const uint8_t *p)
37{
38 return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) | ((uint32_t)p[2] << 8) | (uint32_t)p[3];
39}
40
41static void put_u64(uint8_t *p, uint64_t v)
42{
43 for (int i = 0; i < 8; i++)
44 {
45 p[i] = (uint8_t)(v >> (8 * (7 - i)));
46 }
47}
48
49static uint64_t get_u64(const uint8_t *p)
50{
51 uint64_t v = 0;
52 for (int i = 0; i < 8; i++)
53 {
54 v = (v << 8) | p[i];
55 }
56 return v;
57}
58
59// -- timestamp --
60
61void pc_ptp_ts_write(uint8_t *p, const pc_ptp_timestamp *ts)
62{
63 uint64_t s = ts->seconds;
64 p[0] = (uint8_t)(s >> 40);
65 p[1] = (uint8_t)(s >> 32);
66 p[2] = (uint8_t)(s >> 24);
67 p[3] = (uint8_t)(s >> 16);
68 p[4] = (uint8_t)(s >> 8);
69 p[5] = (uint8_t)s;
70 put_u32(p + 6, ts->nanoseconds);
71}
72
73void pc_ptp_ts_read(const uint8_t *p, pc_ptp_timestamp *ts)
74{
75 ts->seconds = ((uint64_t)p[0] << 40) | ((uint64_t)p[1] << 32) | ((uint64_t)p[2] << 24) | ((uint64_t)p[3] << 16) |
76 ((uint64_t)p[4] << 8) | (uint64_t)p[5];
77 ts->nanoseconds = get_u32(p + 6);
78}
79
80int64_t pc_ptp_ts_to_ns(const pc_ptp_timestamp *ts)
81{
82 return (int64_t)ts->seconds * 1000000000LL + (int64_t)ts->nanoseconds;
83}
84
85void pc_ptp_ts_from_ns(int64_t ns, pc_ptp_timestamp *ts)
86{
87 if (ns < 0)
88 {
89 ns = 0; // on-wire timestamps are unsigned
90 }
91 ts->seconds = (uint64_t)(ns / 1000000000LL);
92 ts->nanoseconds = (uint32_t)(ns % 1000000000LL);
93}
94
95// -- header --
96
97size_t pc_ptp_build_header(uint8_t *buf, size_t cap, const pc_ptp_header *h, uint16_t body_len)
98{
99 if (!buf || !h || cap < PC_PTP_HEADER_LEN)
100 {
101 return 0;
102 }
103 memset(buf, 0, PC_PTP_HEADER_LEN);
104 buf[0] = (uint8_t)((h->transport_specific << 4) | (h->message_type & 0x0F));
105 buf[1] = (uint8_t)(h->version & 0x0F);
106 put_u16(buf + 2, (uint16_t)(PC_PTP_HEADER_LEN + body_len));
107 buf[4] = h->domain;
108 put_u16(buf + 6, h->flags);
109 put_u64(buf + 8, (uint64_t)h->correction);
110 memcpy(buf + 20, h->clock_identity, 8);
111 put_u16(buf + 28, h->port_number);
112 put_u16(buf + 30, h->sequence_id);
113 buf[32] = h->control;
114 buf[33] = (uint8_t)h->log_interval;
115 return PC_PTP_HEADER_LEN;
116}
117
118bool pc_ptp_parse_header(const uint8_t *s, size_t len, pc_ptp_header *h)
119{
120 if (!s || !h || len < PC_PTP_HEADER_LEN)
121 {
122 return false;
123 }
124 h->message_type = (uint8_t)(s[0] & 0x0F);
125 h->transport_specific = (uint8_t)(s[0] >> 4);
126 h->version = (uint8_t)(s[1] & 0x0F);
127 h->message_length = get_u16(s + 2);
128 h->domain = s[4];
129 h->flags = get_u16(s + 6);
130 h->correction = (int64_t)get_u64(s + 8);
131 memcpy(h->clock_identity, s + 20, 8);
132 h->port_number = get_u16(s + 28);
133 h->sequence_id = get_u16(s + 30);
134 h->control = s[32];
135 h->log_interval = (int8_t)s[33];
136 return true;
137}
138
139// -- messages --
140
141static size_t build_ts_msg(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *ts, uint8_t mtype,
142 uint8_t control)
143{
144 if (!buf || !h || !ts || cap < PC_PTP_HEADER_LEN + PC_PTP_TS_LEN)
145 {
146 return 0;
147 }
148 pc_ptp_header hh = *h;
149 hh.message_type = mtype;
150 hh.control = control;
151 if (hh.version == 0)
152 {
153 hh.version = 2;
154 }
155 pc_ptp_build_header(buf, cap, &hh, PC_PTP_TS_LEN);
156 pc_ptp_ts_write(buf + PC_PTP_HEADER_LEN, ts);
157 return PC_PTP_HEADER_LEN + PC_PTP_TS_LEN;
158}
159
160size_t pc_ptp_build_sync(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *origin)
161{
162 return build_ts_msg(buf, cap, h, origin, PC_PTP_SYNC, 0x00);
163}
164
165size_t pc_ptp_build_delay_req(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *origin)
166{
167 return build_ts_msg(buf, cap, h, origin, PC_PTP_DELAY_REQ, 0x01);
168}
169
170size_t pc_ptp_build_follow_up(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *precise)
171{
172 return build_ts_msg(buf, cap, h, precise, PC_PTP_FOLLOW_UP, 0x02);
173}
174
175size_t pc_ptp_build_delay_resp(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *recv,
176 const uint8_t *req_clock_id, uint16_t req_port)
177{
178 const size_t body = PC_PTP_TS_LEN + 10; // receiveTimestamp + requestingPortIdentity(10)
179 if (!buf || !h || !recv || !req_clock_id || cap < PC_PTP_HEADER_LEN + body)
180 {
181 return 0;
182 }
183 pc_ptp_header hh = *h;
184 hh.message_type = PC_PTP_DELAY_RESP;
185 hh.control = 0x03;
186 if (hh.version == 0)
187 {
188 hh.version = 2;
189 }
190 pc_ptp_build_header(buf, cap, &hh, (uint16_t)body);
191 uint8_t *p = buf + PC_PTP_HEADER_LEN;
192 pc_ptp_ts_write(p, recv);
193 p += PC_PTP_TS_LEN;
194 memcpy(p, req_clock_id, 8);
195 p += 8;
196 put_u16(p, req_port);
197 return PC_PTP_HEADER_LEN + body;
198}
199
200size_t pc_ptp_build_pdelay_req(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *origin)
201{
202 const size_t body = PC_PTP_TS_LEN + 10; // originTimestamp + 10-octet reserved (pads to Pdelay_Resp length)
203 if (!buf || !h || !origin || cap < PC_PTP_HEADER_LEN + body)
204 {
205 return 0;
206 }
207 pc_ptp_header hh = *h;
208 hh.message_type = PC_PTP_PDELAY_REQ;
209 hh.control = 0x05; // "all others" (IEEE 1588-2008 Table 23)
210 if (hh.version == 0)
211 {
212 hh.version = 2;
213 }
214 pc_ptp_build_header(buf, cap, &hh, (uint16_t)body);
215 uint8_t *p = buf + PC_PTP_HEADER_LEN;
216 pc_ptp_ts_write(p, origin);
217 memset(p + PC_PTP_TS_LEN, 0, 10); // reserved
218 return PC_PTP_HEADER_LEN + body;
219}
220
221// Pdelay_Resp and Pdelay_Resp_Follow_Up share a body layout (a timestamp + the requesting port identity);
222// only the messageType and which timestamp it carries differ.
223static size_t build_pdelay_resp_msg(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *ts,
224 const uint8_t *req_clock_id, uint16_t req_port, uint8_t mtype)
225{
226 const size_t body = PC_PTP_TS_LEN + 10; // timestamp + requestingPortIdentity(10)
227 if (!buf || !h || !ts || !req_clock_id || cap < PC_PTP_HEADER_LEN + body)
228 {
229 return 0;
230 }
231 pc_ptp_header hh = *h;
232 hh.message_type = mtype;
233 hh.control = 0x05;
234 if (hh.version == 0)
235 {
236 hh.version = 2;
237 }
238 pc_ptp_build_header(buf, cap, &hh, (uint16_t)body);
239 uint8_t *p = buf + PC_PTP_HEADER_LEN;
240 pc_ptp_ts_write(p, ts);
241 p += PC_PTP_TS_LEN;
242 memcpy(p, req_clock_id, 8);
243 p += 8;
244 put_u16(p, req_port);
245 return PC_PTP_HEADER_LEN + body;
246}
247
248size_t pc_ptp_build_pdelay_resp(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_timestamp *recv,
249 const uint8_t *req_clock_id, uint16_t req_port)
250{
251 return build_pdelay_resp_msg(buf, cap, h, recv, req_clock_id, req_port, PC_PTP_PDELAY_RESP);
252}
253
254size_t pc_ptp_build_pdelay_resp_follow_up(uint8_t *buf, size_t cap, const pc_ptp_header *h,
255 const pc_ptp_timestamp *origin, const uint8_t *req_clock_id,
256 uint16_t req_port)
257{
258 return build_pdelay_resp_msg(buf, cap, h, origin, req_clock_id, req_port, PC_PTP_PDELAY_RESP_FOLLOW_UP);
259}
260
261size_t pc_ptp_build_announce(uint8_t *buf, size_t cap, const pc_ptp_header *h, const pc_ptp_announce *a)
262{
263 const size_t body = 30; // originTimestamp(10)+utc(2)+rsv(1)+p1(1)+quality(4)+p2(1)+id(8)+steps(2)+src(1)
264 if (!buf || !h || !a || cap < PC_PTP_HEADER_LEN + body)
265 {
266 return 0;
267 }
268 pc_ptp_header hh = *h;
269 hh.message_type = PC_PTP_ANNOUNCE;
270 hh.control = 0x05;
271 if (hh.version == 0)
272 {
273 hh.version = 2;
274 }
275 pc_ptp_build_header(buf, cap, &hh, (uint16_t)body);
276 uint8_t *p = buf + PC_PTP_HEADER_LEN;
277 pc_ptp_ts_write(p, &a->origin);
278 p += PC_PTP_TS_LEN;
279 put_u16(p, (uint16_t)a->utc_offset);
280 p += 2;
281 *p++ = 0; // reserved
282 *p++ = a->gm_priority1;
283 *p++ = a->gm_clock_class;
284 *p++ = a->gm_clock_accuracy;
285 put_u16(p, a->gm_variance);
286 p += 2;
287 *p++ = a->gm_priority2;
288 memcpy(p, a->gm_identity, 8);
289 p += 8;
290 put_u16(p, a->steps_removed);
291 p += 2;
292 *p = a->time_source;
293 return PC_PTP_HEADER_LEN + body;
294}
295
296bool pc_ptp_parse_timestamp_msg(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_timestamp *ts)
297{
298 if (!ts || !pc_ptp_parse_header(s, len, h))
299 {
300 return false;
301 }
302 if (len < PC_PTP_HEADER_LEN + PC_PTP_TS_LEN)
303 {
304 return false;
305 }
306 if (h->message_type != PC_PTP_SYNC && h->message_type != PC_PTP_DELAY_REQ && h->message_type != PC_PTP_FOLLOW_UP)
307 {
308 return false;
309 }
310 pc_ptp_ts_read(s + PC_PTP_HEADER_LEN, ts);
311 return true;
312}
313
314bool pc_ptp_parse_delay_resp(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_delay_resp *out)
315{
316 if (!out || !pc_ptp_parse_header(s, len, h))
317 {
318 return false;
319 }
320 if (h->message_type != PC_PTP_DELAY_RESP)
321 {
322 return false;
323 }
324 if (len < PC_PTP_HEADER_LEN + PC_PTP_TS_LEN + 10)
325 {
326 return false;
327 }
328 const uint8_t *p = s + PC_PTP_HEADER_LEN;
329 pc_ptp_ts_read(p, &out->receive);
330 p += PC_PTP_TS_LEN;
331 memcpy(out->req_clock_id, p, 8);
332 p += 8;
333 out->req_port = get_u16(p);
334 return true;
335}
336
337bool pc_ptp_parse_pdelay_req(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_timestamp *ts)
338{
339 if (!ts || !pc_ptp_parse_header(s, len, h))
340 {
341 return false;
342 }
343 if (h->message_type != PC_PTP_PDELAY_REQ)
344 {
345 return false;
346 }
347 if (len < PC_PTP_HEADER_LEN + PC_PTP_TS_LEN) // the originTimestamp (the reserved tail is ignored)
348 {
349 return false;
350 }
351 pc_ptp_ts_read(s + PC_PTP_HEADER_LEN, ts);
352 return true;
353}
354
355// Pdelay_Resp and its Follow_Up share the body layout; only the messageType differs.
356static bool parse_pdelay_resp_msg(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_pdelay_resp *out,
357 uint8_t mtype)
358{
359 if (!out || !pc_ptp_parse_header(s, len, h))
360 {
361 return false;
362 }
363 if (h->message_type != mtype)
364 {
365 return false;
366 }
367 if (len < PC_PTP_HEADER_LEN + PC_PTP_TS_LEN + 10)
368 {
369 return false;
370 }
371 const uint8_t *p = s + PC_PTP_HEADER_LEN;
372 pc_ptp_ts_read(p, &out->timestamp);
373 p += PC_PTP_TS_LEN;
374 memcpy(out->req_clock_id, p, 8);
375 p += 8;
376 out->req_port = get_u16(p);
377 return true;
378}
379
380bool pc_ptp_parse_pdelay_resp(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_pdelay_resp *out)
381{
382 return parse_pdelay_resp_msg(s, len, h, out, PC_PTP_PDELAY_RESP);
383}
384
385bool pc_ptp_parse_pdelay_resp_follow_up(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_pdelay_resp *out)
386{
387 return parse_pdelay_resp_msg(s, len, h, out, PC_PTP_PDELAY_RESP_FOLLOW_UP);
388}
389
390bool pc_ptp_parse_announce(const uint8_t *s, size_t len, pc_ptp_header *h, pc_ptp_announce *out)
391{
392 if (!out || !pc_ptp_parse_header(s, len, h))
393 {
394 return false;
395 }
396 if (h->message_type != PC_PTP_ANNOUNCE)
397 {
398 return false;
399 }
400 if (len < PC_PTP_HEADER_LEN + 30) // originTimestamp(10)+utc(2)+rsv(1)+p1(1)+quality(4)+p2(1)+id(8)+steps(2)+src(1)
401 {
402 return false;
403 }
404 const uint8_t *p = s + PC_PTP_HEADER_LEN;
405 pc_ptp_ts_read(p, &out->origin);
406 p += PC_PTP_TS_LEN;
407 out->utc_offset = (int16_t)get_u16(p);
408 p += 2;
409 p += 1; // reserved
410 out->gm_priority1 = *p++;
411 out->gm_clock_class = *p++;
412 out->gm_clock_accuracy = *p++;
413 out->gm_variance = get_u16(p);
414 p += 2;
415 out->gm_priority2 = *p++;
416 memcpy(out->gm_identity, p, 8);
417 p += 8;
418 out->steps_removed = get_u16(p);
419 p += 2;
420 out->time_source = *p;
421 return true;
422}
423
424// -- slave clock math --
425
426void pc_ptp_compute(int64_t t1, int64_t t2, int64_t t3, int64_t t4, pc_ptp_sync *out)
427{
428 if (!out)
429 {
430 return;
431 }
432 int64_t ms = t2 - t1; // master -> slave transit (+ offset)
433 int64_t sm = t4 - t3; // slave -> master transit (- offset)
434 out->offset_ns = (ms - sm) / 2;
435 out->delay_ns = (ms + sm) / 2;
436}
437
438int64_t pc_ptp_compute_link_delay(int64_t t1, int64_t t2, int64_t t3, int64_t t4)
439{
440 // meanLinkDelay = ((t4 - t1) - (t3 - t2)) / 2: the round trip minus the peer's turnaround, halved.
441 return ((t4 - t1) - (t3 - t2)) / 2;
442}
443
444#endif // PC_ENABLE_PTP
ptp.h
PTP / IEEE 1588-2008 (PTPv2) message codec + slave clock math (PC_ENABLE_PTP).