4  * SPDX-License-Identifier: Apache-2.0
54 	msg->timestamp.protocol._sec.high = ntohs(ts->seconds_high);  in msg_timestamp_post_recv()
55 	msg->timestamp.protocol._sec.low = ntohl(ts->seconds_low);  in msg_timestamp_post_recv()
56 	msg->timestamp.protocol.nanosecond = ntohl(ts->nanoseconds);  in msg_timestamp_post_recv()
61 	ts->seconds_high = htons(ts->seconds_high);  in msg_timestamp_pre_send()
62 	ts->seconds_low = htonl(ts->seconds_low);  in msg_timestamp_pre_send()
63 	ts->nanoseconds = htonl(ts->nanoseconds);  in msg_timestamp_pre_send()
68 	port_id->port_number = ntohs(port_id->port_number);  in msg_port_id_post_recv()
73 	port_id->port_number = htons(port_id->port_number);  in msg_port_id_pre_send()
76 static int msg_header_post_recv(struct ptp_header *header)  in msg_header_post_recv()  argument
78 	if ((header->version & 0xF) != PTP_MAJOR_VERSION) {  in msg_header_post_recv()
80 		return -1;  in msg_header_post_recv()
83 	header->msg_length = ntohs(header->msg_length);  in msg_header_post_recv()
84 	header->correction = ntohll(header->correction);  in msg_header_post_recv()
85 	header->sequence_id = ntohs(header->sequence_id);  in msg_header_post_recv()
87 	msg_port_id_post_recv(&header->src_port_id);  in msg_header_post_recv()
92 static void msg_header_pre_send(struct ptp_header *header)  in msg_header_pre_send()  argument
94 	header->msg_length = htons(header->msg_length);  in msg_header_pre_send()
95 	header->correction = htonll(header->correction);  in msg_header_pre_send()
96 	header->sequence_id = htons(header->sequence_id);  in msg_header_pre_send()
98 	msg_port_id_pre_send(&header->src_port_id);  in msg_header_pre_send()
107 		suffix = msg->sync.suffix;  in msg_suffix()
110 		suffix = msg->delay_req.suffix;  in msg_suffix()
113 		suffix = msg->pdelay_req.suffix;  in msg_suffix()
116 		suffix = msg->pdelay_resp.suffix;  in msg_suffix()
119 		suffix = msg->follow_up.suffix;  in msg_suffix()
122 		suffix = msg->delay_resp.suffix;  in msg_suffix()
125 		suffix = msg->pdelay_resp_follow_up.suffix;  in msg_suffix()
128 		suffix = msg->announce.suffix;  in msg_suffix()
131 		suffix = msg->signaling.suffix;  in msg_suffix()
134 		suffix = msg->management.suffix;  in msg_suffix()
155 			return -ENOMEM;  in msg_tlv_post_recv()
158 		tlv_container->tlv = (struct ptp_tlv *)suffix;  in msg_tlv_post_recv()
159 		tlv_container->tlv->type = ntohs(tlv_container->tlv->type);  in msg_tlv_post_recv()
160 		tlv_container->tlv->length = ntohs(tlv_container->tlv->length);  in msg_tlv_post_recv()
162 		if (tlv_container->tlv->length % 2) {  in msg_tlv_post_recv()
163 			/* IEEE 1588-2019 Section 5.3.8 - length is an even number */  in msg_tlv_post_recv()
166 			return -EBADMSG;  in msg_tlv_post_recv()
169 		length -= sizeof(struct ptp_tlv);  in msg_tlv_post_recv()
173 		if (tlv_container->tlv->length > length) {  in msg_tlv_post_recv()
176 			return -EBADMSG;  in msg_tlv_post_recv()
179 		length -= tlv_container->tlv->length;  in msg_tlv_post_recv()
180 		suffix += tlv_container->tlv->length;  in msg_tlv_post_recv()
181 		suffix_len += tlv_container->tlv->length;  in msg_tlv_post_recv()
183 		ret = ptp_tlv_post_recv(tlv_container->tlv);  in msg_tlv_post_recv()
189 		sys_slist_append(&msg->tlvs, &tlv_container->node);  in msg_tlv_post_recv()
199 	for (sys_snode_t *iter = sys_slist_get(&msg->tlvs);  in msg_tlv_free()
201 	     iter = sys_slist_get(&msg->tlvs)) {  in msg_tlv_free()
211 	SYS_SLIST_FOR_EACH_CONTAINER(&msg->tlvs, tlv_container, node) {  in msg_tlv_pre_send()
212 		ptp_tlv_pre_send(tlv_container->tlv);  in msg_tlv_pre_send()
230 	sys_slist_init(&msg->tlvs);  in ptp_msg_alloc()
231 	atomic_inc(&msg->ref);  in ptp_msg_alloc()
240 	atomic_t val = atomic_dec(&msg->ref);  in ptp_msg_unref()
254 	atomic_inc(&msg->ref);  in ptp_msg_ref()
259 	return (enum ptp_msg_type)(msg->header.type_major_sdo_id & 0xF);  in ptp_msg_type()
269 	int port, payload;  in ptp_msg_from_pkt()  local
271 	if (pkt->buffer->len == eth_hdr_len) {  in ptp_msg_from_pkt()
272 		/* Packet contain Ethernet header at the beginning. */  in ptp_msg_from_pkt()
276 		buf = pkt->buffer;  in ptp_msg_from_pkt()
277 		pkt->buffer = buf->frags;  in ptp_msg_from_pkt()
288 		LOG_ERR("Couldn't retrieve UDP header from the net packet");  in ptp_msg_from_pkt()
292 	payload = ntohs(hdr->len) - NET_UDPH_LEN;  in ptp_msg_from_pkt()
293 	port = ntohs(hdr->dst_port);  in ptp_msg_from_pkt()
302 	if (payload == ntohs(msg->header.msg_length)) {  in ptp_msg_from_pkt()
313 	msg_header_pre_send(&msg->header);  in ptp_msg_pre_send()
321 		msg->timestamp.host.second = (uint64_t)(current / MSEC_PER_SEC);  in ptp_msg_pre_send()
322 		msg->timestamp.host.nanosecond = (current % MSEC_PER_SEC) * NSEC_PER_MSEC;  in ptp_msg_pre_send()
327 		msg_timestamp_pre_send(&msg->pdelay_resp.req_receipt_timestamp);  in ptp_msg_pre_send()
328 		msg_port_id_pre_send(&msg->pdelay_resp.req_port_id);  in ptp_msg_pre_send()
331 		msg_timestamp_pre_send(&msg->follow_up.precise_origin_timestamp);  in ptp_msg_pre_send()
334 		msg_timestamp_pre_send(&msg->delay_resp.receive_timestamp);  in ptp_msg_pre_send()
335 		msg_port_id_pre_send(&msg->delay_resp.req_port_id);  in ptp_msg_pre_send()
338 		msg_timestamp_pre_send(&msg->pdelay_resp_follow_up.resp_origin_timestamp);  in ptp_msg_pre_send()
339 		msg_port_id_pre_send(&msg->pdelay_resp_follow_up.req_port_id);  in ptp_msg_pre_send()
342 		msg->announce.current_utc_offset = htons(msg->announce.current_utc_offset);  in ptp_msg_pre_send()
343 		msg->announce.gm_clk_quality.offset_scaled_log_variance =  in ptp_msg_pre_send()
344 			htons(msg->announce.gm_clk_quality.offset_scaled_log_variance);  in ptp_msg_pre_send()
345 		msg->announce.steps_rm = htons(msg->announce.steps_rm);  in ptp_msg_pre_send()
348 		msg_port_id_pre_send(&msg->signaling.target_port_id);  in ptp_msg_pre_send()
351 		msg_port_id_pre_send(&msg->management.target_port_id);  in ptp_msg_pre_send()
378 		return -EBADMSG;  in ptp_msg_post_recv()
381 	if (msg_header_post_recv(&msg->header)) {  in ptp_msg_post_recv()
383 		return -EBADMSG;  in ptp_msg_post_recv()
386 	LOG_DBG("Port %d received %s message", port->port_ds.id.port_number, msg_type_str(msg));  in ptp_msg_post_recv()
390 		msg_timestamp_post_recv(msg, &msg->sync.origin_timestamp);  in ptp_msg_post_recv()
397 		msg_timestamp_post_recv(msg, &msg->pdelay_resp.req_receipt_timestamp);  in ptp_msg_post_recv()
398 		msg_port_id_post_recv(&msg->pdelay_resp.req_port_id);  in ptp_msg_post_recv()
401 		msg_timestamp_post_recv(msg, &msg->follow_up.precise_origin_timestamp);  in ptp_msg_post_recv()
404 		msg_timestamp_post_recv(msg, &msg->delay_resp.receive_timestamp);  in ptp_msg_post_recv()
405 		msg_port_id_post_recv(&msg->delay_resp.req_port_id);  in ptp_msg_post_recv()
408 		msg_timestamp_post_recv(msg, &msg->pdelay_resp_follow_up.resp_origin_timestamp);  in ptp_msg_post_recv()
409 		msg_port_id_post_recv(&msg->pdelay_resp_follow_up.req_port_id);  in ptp_msg_post_recv()
414 		msg->timestamp.host.second = (uint64_t)(current / MSEC_PER_SEC);  in ptp_msg_post_recv()
415 		msg->timestamp.host.nanosecond = (current % MSEC_PER_SEC) * NSEC_PER_MSEC;  in ptp_msg_post_recv()
416 		msg_timestamp_post_recv(msg, &msg->announce.origin_timestamp);  in ptp_msg_post_recv()
417 		msg->announce.current_utc_offset = ntohs(msg->announce.current_utc_offset);  in ptp_msg_post_recv()
418 		msg->announce.gm_clk_quality.offset_scaled_log_variance =  in ptp_msg_post_recv()
419 			ntohs(msg->announce.gm_clk_quality.offset_scaled_log_variance);  in ptp_msg_post_recv()
420 		msg->announce.steps_rm = ntohs(msg->announce.steps_rm);  in ptp_msg_post_recv()
423 		msg_port_id_post_recv(&msg->signaling.target_port_id);  in ptp_msg_post_recv()
426 		msg_port_id_post_recv(&msg->management.target_port_id);  in ptp_msg_post_recv()
430 	tlv_len = msg_tlv_post_recv(msg, cnt - msg_size[type]);  in ptp_msg_post_recv()
433 		return -EBADMSG;  in ptp_msg_post_recv()
436 	if (msg_size[type] + tlv_len != msg->header.msg_length) {  in ptp_msg_post_recv()
438 		return -EMSGSIZE;  in ptp_msg_post_recv()
453 	tlv_container = (struct ptp_tlv_container *)sys_slist_peek_tail(&msg->tlvs);  in ptp_msg_add_tlv()
455 		suffix = (uint8_t *)tlv_container->tlv;  in ptp_msg_add_tlv()
456 		suffix += sizeof(*tlv_container->tlv);  in ptp_msg_add_tlv()
457 		suffix += tlv_container->tlv->length;  in ptp_msg_add_tlv()
460 	if ((intptr_t)(suffix + length) >= (intptr_t)&msg->ref) {  in ptp_msg_add_tlv()
467 		tlv_container->tlv = (struct ptp_tlv *)suffix;  in ptp_msg_add_tlv()
468 		msg->header.msg_length += length;  in ptp_msg_add_tlv()
471 	return tlv_container ? tlv_container->tlv : NULL;  in ptp_msg_add_tlv()
476 	int len = sizeof(m1->gm_priority1) + sizeof(m1->gm_clk_quality) +  in ptp_msg_announce_cmp()
477 		  sizeof(m1->gm_priority1) + sizeof(m1->gm_id) +  in ptp_msg_announce_cmp()
478 		  sizeof(m1->steps_rm);  in ptp_msg_announce_cmp()
480 	return memcmp(&m1->gm_priority1, &m2->gm_priority1, len);  in ptp_msg_announce_cmp()
487 	return ptp_port_id_eq(&pds->port_id, &msg->header.src_port_id);  in ptp_msg_current_parent()