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_tlv, CONFIG_PTP_LOG_LEVEL);
9
10 #include <zephyr/kernel.h>
11 #include <zephyr/net/net_ip.h>
12
13 #include "port.h"
14 #include "tlv.h"
15
16 #define TLV_MANUFACTURER_ID_LEN (3)
17 #define TLV_PROFILE_ID_LEN (6)
18 #define TLV_ADDR_LEN_MAX (16)
19
20 K_MEM_SLAB_DEFINE_STATIC(tlv_slab,
21 sizeof(struct ptp_tlv_container),
22 2 * CONFIG_PTP_MSG_POLL_SIZE,
23 8);
24
tlv_ntohs(void * ptr)25 static inline void tlv_ntohs(void *ptr)
26 {
27 uint16_t val = *(uint16_t *)ptr;
28
29 val = ntohs(val);
30 memcpy(ptr, &val, sizeof(val));
31 }
32
tlv_htons(void * ptr)33 static inline void tlv_htons(void *ptr)
34 {
35 uint16_t val = *(uint16_t *)ptr;
36
37 val = htons(val);
38 memcpy(ptr, &val, sizeof(val));
39 }
40
tlv_mgmt_post_recv(struct ptp_tlv_mgmt * mgmt_tlv,uint16_t length)41 static int tlv_mgmt_post_recv(struct ptp_tlv_mgmt *mgmt_tlv, uint16_t length)
42 {
43 struct ptp_tlv_mgmt_clock_desc *clock_desc;
44 struct ptp_tlv_time_prop_ds *time_prop_ds;
45 struct ptp_tlv_default_ds *default_ds;
46 struct ptp_tlv_current_ds *current_ds;
47 struct ptp_tlv_parent_ds *parent_ds;
48 struct ptp_tlv_port_ds *port_ds;
49 struct ptp_timestamp ts;
50
51 enum ptp_mgmt_id id = (enum ptp_mgmt_id)mgmt_tlv->id;
52 struct ptp_tlv_container *container;
53 uint8_t *data;
54 int32_t data_length;
55
56 switch (id) {
57 case PTP_MGMT_NULL_PTP_MANAGEMENT:
58 __fallthrough;
59 case PTP_MGMT_SAVE_IN_NON_VOLATILE_STORAGE:
60 __fallthrough;
61 case PTP_MGMT_RESET_NON_VOLATILE_STORAGE:
62 __fallthrough;
63 case PTP_MGMT_FAULT_LOG_RESET:
64 __fallthrough;
65 case PTP_MGMT_ENABLE_PORT:
66 __fallthrough;
67 case PTP_MGMT_DISABLE_PORT:
68 if (length != 0) {
69 return -EBADMSG;
70 }
71 break;
72 case PTP_MGMT_CLOCK_DESCRIPTION:
73 container = CONTAINER_OF((void *)mgmt_tlv, struct ptp_tlv_container, tlv);
74
75 clock_desc = &container->clock_desc;
76 data = mgmt_tlv->data;
77 data_length = length;
78
79 clock_desc->type = (uint16_t *)data;
80 data += sizeof(*clock_desc->type);
81 data_length -= sizeof(*clock_desc->type);
82 if (data_length < 0) {
83 return -EBADMSG;
84 }
85 tlv_ntohs(&clock_desc->type);
86
87 clock_desc->phy_protocol = (struct ptp_text *)data;
88 data += sizeof(*clock_desc->phy_protocol);
89 data_length -= sizeof(*clock_desc->phy_protocol);
90 if (data_length < 0) {
91 return -EBADMSG;
92 }
93 data += clock_desc->phy_protocol->length;
94 data_length -= clock_desc->phy_protocol->length;
95 if (data_length < 0) {
96 return -EBADMSG;
97 }
98
99 clock_desc->phy_addr_len = (uint16_t *)data;
100 data += sizeof(*clock_desc->phy_addr_len);
101 data_length -= sizeof(*clock_desc->phy_addr_len);
102 if (data_length < 0) {
103 return -EBADMSG;
104 }
105 tlv_ntohs(&clock_desc->phy_addr_len);
106 if (*clock_desc->phy_addr_len > TLV_ADDR_LEN_MAX) {
107 return -EBADMSG;
108 }
109
110 clock_desc->phy_addr = data;
111 data += *clock_desc->phy_addr_len;
112 data_length -= *clock_desc->phy_addr_len;
113 if (data_length < 0) {
114 return -EBADMSG;
115 }
116
117 clock_desc->protocol_addr = (struct ptp_port_addr *)data;
118 data += sizeof(*clock_desc->protocol_addr);
119 data_length -= sizeof(*clock_desc->protocol_addr);
120 if (data_length < 0) {
121 return -EBADMSG;
122 }
123 tlv_ntohs(&clock_desc->protocol_addr->protocol);
124 tlv_ntohs(&clock_desc->protocol_addr->addr_len);
125 if (clock_desc->protocol_addr->addr_len > TLV_ADDR_LEN_MAX) {
126 return -EBADMSG;
127 }
128
129 data += clock_desc->protocol_addr->addr_len;
130 data_length -= clock_desc->protocol_addr->addr_len;
131 if (data_length < 0) {
132 return -EBADMSG;
133 }
134
135 clock_desc->manufacturer_id = data;
136 /* extra byte for reserved field - see IEEE 1588-2019 15.5.3.1.2 */
137 data += TLV_MANUFACTURER_ID_LEN + 1;
138 data_length -= TLV_MANUFACTURER_ID_LEN + 1;
139 if (data_length < 0) {
140 return -EBADMSG;
141 }
142
143 clock_desc->product_desc = (struct ptp_text *)data;
144 data += sizeof(*clock_desc->product_desc);
145 data_length -= sizeof(*clock_desc->product_desc);
146 if (data_length < 0) {
147 return -EBADMSG;
148 }
149 data += clock_desc->product_desc->length;
150 data_length -= clock_desc->product_desc->length;
151 if (data_length < 0) {
152 return -EBADMSG;
153 }
154
155 clock_desc->revision_data = (struct ptp_text *)data;
156 data += sizeof(*clock_desc->revision_data);
157 data_length -= sizeof(*clock_desc->revision_data);
158 if (data_length < 0) {
159 return -EBADMSG;
160 }
161 data += clock_desc->revision_data->length;
162 data_length -= clock_desc->revision_data->length;
163 if (data_length < 0) {
164 return -EBADMSG;
165 }
166
167 clock_desc->user_desc = (struct ptp_text *)data;
168 data += sizeof(*clock_desc->user_desc);
169 data_length -= sizeof(*clock_desc->user_desc);
170 if (data_length < 0) {
171 return -EBADMSG;
172 }
173 data += clock_desc->user_desc->length;
174 data_length -= clock_desc->user_desc->length;
175 if (data_length < 0) {
176 return -EBADMSG;
177 }
178
179 clock_desc->profile_id = data;
180 data += TLV_PROFILE_ID_LEN;
181 data_length -= TLV_PROFILE_ID_LEN;
182
183 break;
184 case PTP_MGMT_USER_DESCRIPTION:
185 container = CONTAINER_OF((void *)mgmt_tlv, struct ptp_tlv_container, tlv);
186
187 if (length < sizeof(struct ptp_text)) {
188 return -EBADMSG;
189 }
190 container->clock_desc.user_desc = (struct ptp_text *)mgmt_tlv->data;
191 break;
192 case PTP_MGMT_DEFAULT_DATA_SET:
193 if (length != sizeof(struct ptp_tlv_default_ds)) {
194 return -EBADMSG;
195 }
196 default_ds = (struct ptp_tlv_default_ds *)mgmt_tlv->data;
197
198 default_ds->n_ports = ntohs(default_ds->n_ports);
199 default_ds->clk_quality.offset_scaled_log_variance =
200 ntohs(default_ds->clk_quality.offset_scaled_log_variance);
201 break;
202 case PTP_MGMT_CURRENT_DATA_SET:
203 if (length != sizeof(struct ptp_tlv_current_ds)) {
204 return -EBADMSG;
205 }
206 current_ds = (struct ptp_tlv_current_ds *)mgmt_tlv->data;
207
208 current_ds->steps_rm = ntohs(current_ds->steps_rm);
209 current_ds->offset_from_tt = ntohll(current_ds->offset_from_tt);
210 current_ds->mean_delay = ntohll(current_ds->mean_delay);
211 break;
212 case PTP_MGMT_PARENT_DATA_SET:
213 if (length != sizeof(struct ptp_tlv_parent_ds)) {
214 return -EBADMSG;
215 }
216 parent_ds = (struct ptp_tlv_parent_ds *)mgmt_tlv->data;
217
218 parent_ds->port_id.port_number = ntohs(parent_ds->port_id.port_number);
219 parent_ds->obsreved_parent_offset_scaled_log_variance =
220 ntohs(parent_ds->obsreved_parent_offset_scaled_log_variance);
221 parent_ds->obsreved_parent_clk_phase_change_rate =
222 ntohl(parent_ds->obsreved_parent_clk_phase_change_rate);
223 parent_ds->gm_clk_quality.offset_scaled_log_variance =
224 ntohs(parent_ds->gm_clk_quality.offset_scaled_log_variance);
225 break;
226 case PTP_MGMT_TIME_PROPERTIES_DATA_SET:
227 if (length != sizeof(struct ptp_tlv_time_prop_ds)) {
228 return -EBADMSG;
229 }
230 time_prop_ds = (struct ptp_tlv_time_prop_ds *)mgmt_tlv->data;
231
232 time_prop_ds->current_utc_offset = ntohs(time_prop_ds->current_utc_offset);
233 break;
234 case PTP_MGMT_PORT_DATA_SET:
235 if (length != sizeof(struct ptp_tlv_port_ds)) {
236 return -EBADMSG;
237 }
238 port_ds = (struct ptp_tlv_port_ds *)mgmt_tlv->data;
239
240 port_ds->id.port_number = ntohs(port_ds->id.port_number);
241 port_ds->mean_link_delay = ntohll(port_ds->mean_link_delay);
242 break;
243 case PTP_MGMT_TIME:
244 ts = *(struct ptp_timestamp *)mgmt_tlv->data;
245
246 ts.seconds_high = ntohs(ts.seconds_high);
247 ts.seconds_low = ntohl(ts.seconds_low);
248 ts.nanoseconds = ntohl(ts.nanoseconds);
249
250 memcpy(mgmt_tlv->data, &ts, sizeof(ts));
251 break;
252 default:
253 break;
254 }
255
256 return 0;
257 }
258
tlv_mgmt_pre_send(struct ptp_tlv_mgmt * mgmt_tlv)259 static void tlv_mgmt_pre_send(struct ptp_tlv_mgmt *mgmt_tlv)
260 {
261 enum ptp_mgmt_id id = (enum ptp_mgmt_id)mgmt_tlv->id;
262 struct ptp_tlv_mgmt_clock_desc *clock_desc;
263 struct ptp_tlv_time_prop_ds *time_prop_ds;
264 struct ptp_tlv_default_ds *default_ds;
265 struct ptp_tlv_current_ds *current_ds;
266 struct ptp_tlv_container *container;
267 struct ptp_tlv_parent_ds *parent_ds;
268 struct ptp_tlv_port_ds *port_ds;
269 struct ptp_timestamp ts;
270
271 switch (id) {
272 case PTP_MGMT_CLOCK_DESCRIPTION:
273 container = CONTAINER_OF((void *)mgmt_tlv, struct ptp_tlv_container, tlv);
274 clock_desc = &container->clock_desc;
275
276 tlv_htons(&clock_desc->type);
277 tlv_htons(&clock_desc->phy_addr_len);
278 tlv_htons(&clock_desc->protocol_addr->protocol);
279 tlv_htons(&clock_desc->protocol_addr->addr_len);
280 break;
281 case PTP_MGMT_DEFAULT_DATA_SET:
282 default_ds = (struct ptp_tlv_default_ds *)mgmt_tlv->data;
283
284 default_ds->n_ports = htons(default_ds->n_ports);
285 default_ds->clk_quality.offset_scaled_log_variance =
286 htons(default_ds->clk_quality.offset_scaled_log_variance);
287 break;
288 case PTP_MGMT_CURRENT_DATA_SET:
289 current_ds = (struct ptp_tlv_current_ds *)mgmt_tlv->data;
290
291 current_ds->steps_rm = htons(current_ds->steps_rm);
292 current_ds->offset_from_tt = htonll(current_ds->offset_from_tt);
293 current_ds->mean_delay = htonll(current_ds->mean_delay);
294 break;
295 case PTP_MGMT_PARENT_DATA_SET:
296 parent_ds = (struct ptp_tlv_parent_ds *)mgmt_tlv->data;
297
298 parent_ds->port_id.port_number = htons(parent_ds->port_id.port_number);
299 parent_ds->obsreved_parent_offset_scaled_log_variance =
300 htons(parent_ds->obsreved_parent_offset_scaled_log_variance);
301 parent_ds->obsreved_parent_clk_phase_change_rate =
302 htons(parent_ds->obsreved_parent_clk_phase_change_rate);
303 parent_ds->gm_clk_quality.offset_scaled_log_variance =
304 htons(parent_ds->gm_clk_quality.offset_scaled_log_variance);
305 break;
306 case PTP_MGMT_TIME_PROPERTIES_DATA_SET:
307 time_prop_ds = (struct ptp_tlv_time_prop_ds *)mgmt_tlv->data;
308
309 time_prop_ds->current_utc_offset = htons(time_prop_ds->current_utc_offset);
310 break;
311 case PTP_MGMT_PORT_DATA_SET:
312 port_ds = (struct ptp_tlv_port_ds *)mgmt_tlv->data;
313
314 port_ds->id.port_number = htons(port_ds->id.port_number);
315 port_ds->mean_link_delay = htonll(port_ds->mean_link_delay);
316 break;
317 case PTP_MGMT_TIME:
318 ts = *(struct ptp_timestamp *)mgmt_tlv->data;
319
320 ts.seconds_high = htons(ts.seconds_high);
321 ts.seconds_low = htonl(ts.seconds_low);
322 ts.nanoseconds = htonl(ts.nanoseconds);
323
324 memcpy(mgmt_tlv->data, &ts, sizeof(ts));
325 break;
326 default:
327 break;
328 }
329 }
330
ptp_tlv_alloc(void)331 struct ptp_tlv_container *ptp_tlv_alloc(void)
332 {
333 struct ptp_tlv_container *tlv_container = NULL;
334 int ret = k_mem_slab_alloc(&tlv_slab, (void **)&tlv_container, K_FOREVER);
335
336 if (ret) {
337 LOG_ERR("Couldn't allocate memory for the TLV");
338 return NULL;
339 }
340
341 memset(tlv_container, 0, sizeof(*tlv_container));
342 return tlv_container;
343 }
344
ptp_tlv_free(struct ptp_tlv_container * tlv_container)345 void ptp_tlv_free(struct ptp_tlv_container *tlv_container)
346 {
347 k_mem_slab_free(&tlv_slab, (void *)tlv_container);
348 }
349
ptp_mgmt_action(struct ptp_msg * msg)350 enum ptp_mgmt_op ptp_mgmt_action(struct ptp_msg *msg)
351 {
352 return (enum ptp_mgmt_op)msg->management.action;
353 }
354
ptp_tlv_type(struct ptp_tlv * tlv)355 enum ptp_tlv_type ptp_tlv_type(struct ptp_tlv *tlv)
356 {
357 return (enum ptp_tlv_type)tlv->type;
358 }
359
ptp_tlv_post_recv(struct ptp_tlv * tlv)360 int ptp_tlv_post_recv(struct ptp_tlv *tlv)
361 {
362 struct ptp_tlv_mgmt_err *mgmt_err;
363 struct ptp_tlv_mgmt *mgmt;
364 int ret = 0;
365
366 switch (ptp_tlv_type(tlv)) {
367 case PTP_TLV_TYPE_MANAGEMENT:
368 if (tlv->length < (sizeof(struct ptp_tlv_mgmt) - sizeof(struct ptp_tlv))) {
369 return -EBADMSG;
370 }
371 mgmt = (struct ptp_tlv_mgmt *)tlv;
372 mgmt->id = ntohs(mgmt->id);
373
374 /* Value of length is 2 + N, where N is length of data field
375 * based on IEEE 1588-2019 Section 15.5.2.2.
376 */
377 if (tlv->length > sizeof(mgmt->id)) {
378 ret = tlv_mgmt_post_recv(mgmt, tlv->length - 2);
379 }
380 break;
381 case PTP_TLV_TYPE_MANAGEMENT_ERROR_STATUS:
382 if (tlv->length < (sizeof(struct ptp_tlv_mgmt_err) - sizeof(struct ptp_tlv))) {
383 return -EBADMSG;
384 }
385 mgmt_err = (struct ptp_tlv_mgmt_err *)tlv;
386 mgmt_err->err_id = ntohs(mgmt_err->err_id);
387 mgmt_err->id = ntohs(mgmt_err->id);
388 break;
389 default:
390 break;
391 }
392
393 return ret;
394 }
395
ptp_tlv_pre_send(struct ptp_tlv * tlv)396 void ptp_tlv_pre_send(struct ptp_tlv *tlv)
397 {
398 struct ptp_tlv_mgmt_err *mgmt_err;
399 struct ptp_tlv_mgmt *mgmt;
400
401 switch (ptp_tlv_type(tlv)) {
402 case PTP_TLV_TYPE_MANAGEMENT:
403 mgmt = (struct ptp_tlv_mgmt *)tlv;
404
405 /* Check if management TLV contains data */
406 if (tlv->length > sizeof(mgmt->id)) {
407 tlv_mgmt_pre_send(mgmt);
408 }
409 mgmt->id = htons(mgmt->id);
410 break;
411 case PTP_TLV_TYPE_MANAGEMENT_ERROR_STATUS:
412 mgmt_err = (struct ptp_tlv_mgmt_err *)tlv;
413
414 mgmt_err->err_id = htons(mgmt_err->err_id);
415 mgmt_err->id = htons(mgmt_err->id);
416 break;
417 default:
418 break;
419 }
420
421 tlv->length = htons(tlv->length);
422 tlv->type = htons(tlv->type);
423 }
424
