subsys/net/lib/ptp/port.c

/*
 * Copyright (c) 2024 BayLibre SAS
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(ptp_port, CONFIG_PTP_LOG_LEVEL);

#include <zephyr/kernel.h>
#include <zephyr/net/net_if.h>
#include <zephyr/net/ptp.h>
#include <zephyr/net/ptp_time.h>
#include <zephyr/random/random.h>

#include "btca.h"
#include "clock.h"
#include "port.h"
#include "msg.h"
#include "tlv.h"
#include "transport.h"

#define DEFAULT_LOG_MSG_INTERVAL (0x7F)

#define PORT_DELAY_REQ_CLEARE_TO (3 * NSEC_PER_SEC)

#define PORT_LINK_UP	     BIT(0)
#define PORT_LINK_DOWN	     BIT(1)
#define PORT_LINK_CHANGED    BIT(2)
#define PORT_LINK_EVENT_MASK (NET_EVENT_IF_DOWN | NET_EVENT_IF_UP)

static struct ptp_port ports[CONFIG_PTP_NUM_PORTS];
static struct k_mem_slab foreign_tts_slab;
#if CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_FEATURE
BUILD_ASSERT(CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_RECORD_SIZE >= 5 * CONFIG_PTP_NUM_PORTS,
	     "PTP_FOREIGN_TIME_TRANSMITTER_RECORD_SIZE is smaller than expected!");

K_MEM_SLAB_DEFINE_STATIC(foreign_tts_slab,
			 sizeof(struct ptp_foreign_tt_clock),
			 CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_RECORD_SIZE,
			 8);
#endif

char str_port_id[] = "FF:FF:FF:FF:FF:FF:FF:FF-FFFF";

const char *port_id_str(struct ptp_port_id *port_id)
{
	uint8_t *pid = port_id->clk_id.id;

	snprintk(str_port_id, sizeof(str_port_id), "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X-%04X",
		 pid[0],
		 pid[1],
		 pid[2],
		 pid[3],
		 pid[4],
		 pid[5],
		 pid[6],
		 pid[7],
		 port_id->port_number);

	return str_port_id;
}

static const char *port_state_str(enum ptp_port_state state)
{
	const static char * const states[] = {
		[PTP_PS_INITIALIZING]	      = "INITIALIZING",
		[PTP_PS_FAULTY]		      = "FAULTY",
		[PTP_PS_DISABLED]	      = "DISABLED",
		[PTP_PS_LISTENING]	      = "LISTENING",
		[PTP_PS_PRE_TIME_TRANSMITTER] = "PRE TIME TRANSMITTER",
		[PTP_PS_TIME_TRANSMITTER]     = "TIME TRANSMITTER",
		[PTP_PS_GRAND_MASTER]	      = "GRAND MASTER",
		[PTP_PS_PASSIVE]	      = "PASSIVE",
		[PTP_PS_UNCALIBRATED]	      = "UNCALIBRATED",
		[PTP_PS_TIME_RECEIVER]	      = "TIME RECEIVER",
	};

	return states[state];
}

static int port_msg_send(struct ptp_port *port, struct ptp_msg *msg, enum ptp_socket idx)
{
	ptp_msg_pre_send(msg);

	return ptp_transport_send(port, msg, idx);
}

static void port_timer_set_timeout(struct k_timer *timer, uint8_t factor, int8_t log_seconds)
{
	uint64_t timeout = log_seconds < 0 ?
		(uint64_t)((NSEC_PER_SEC * factor) >> (log_seconds * -1)) :
		(uint64_t)((NSEC_PER_SEC * factor) << log_seconds);

	k_timer_start(timer, K_NSEC(timeout), K_NO_WAIT);
}

static void port_timer_set_timeout_random(struct k_timer *timer,
					  int min_factor,
					  int span,
					  int log_seconds)
{
	uint64_t timeout, random_ns;

	if (log_seconds < 0) {
		timeout = (uint64_t)((NSEC_PER_SEC * min_factor) >> -log_seconds);
		random_ns = (uint64_t)(NSEC_PER_SEC >> -log_seconds);
	} else {
		timeout = (uint64_t)((NSEC_PER_SEC * min_factor) << log_seconds);
		random_ns = (uint64_t)((span * NSEC_PER_SEC) << log_seconds);
	}

	timeout = (uint64_t)(timeout + (random_ns * (sys_rand32_get() % (1 << 15) + 1) >> 15));
	k_timer_start(timer, K_NSEC(timeout), K_NO_WAIT);
}

static void port_synchronize(struct ptp_port *port,
			     struct net_ptp_time ingress_ts,
			     struct net_ptp_time origin_ts,
			     ptp_timeinterval correction1,
			     ptp_timeinterval correction2)
{
	uint64_t t1, t2, t1c;

	t1 = origin_ts.second * NSEC_PER_SEC + origin_ts.nanosecond;
	t2 = ingress_ts.second * NSEC_PER_SEC + ingress_ts.nanosecond;
	t1c = t1 + (correction1 >> 16) + (correction2 >> 16);

	ptp_clock_synchronize(t2, t1c);

	port_timer_set_timeout(&port->timers.sync,
			       port->port_ds.announce_receipt_timeout,
			       port->port_ds.log_sync_interval);
}

static void port_ds_init(struct ptp_port *port)
{
	struct ptp_port_ds *ds = &port->port_ds;
	const struct ptp_default_ds *dds = ptp_clock_default_ds();

	memcpy(&ds->id.clk_id, &dds->clk_id, sizeof(ptp_clk_id));
	ds->id.port_number = dds->n_ports + 1;

	ds->state			= PTP_PS_INITIALIZING;
	ds->log_min_delay_req_interval	= CONFIG_PTP_MIN_DELAY_REQ_LOG_INTERVAL;
	ds->log_announce_interval	= CONFIG_PTP_ANNOUNCE_LOG_INTERVAL;
	ds->announce_receipt_timeout	= CONFIG_PTP_ANNOUNCE_RECV_TIMEOUT;
	ds->log_sync_interval		= CONFIG_PTP_SYNC_LOG_INTERVAL;
	ds->delay_mechanism		= PTP_DM_E2E;
	ds->log_min_pdelay_req_interval = CONFIG_PTP_MIN_PDELAY_REQ_LOG_INTERVAL;
	ds->version			= PTP_VERSION;
	ds->delay_asymmetry		= 0;
}

static void port_delay_req_timestamp_cb(struct net_pkt *pkt)
{
	struct ptp_port *port = ptp_clock_port_from_iface(pkt->iface);
	struct ptp_msg *req, *msg = ptp_msg_from_pkt(pkt);
	sys_snode_t *iter, *last = NULL;

	if (!port || !msg) {
		return;
	}

	msg->header.src_port_id.port_number = ntohs(msg->header.src_port_id.port_number);

	if (!ptp_port_id_eq(&port->port_ds.id, &msg->header.src_port_id) ||
	    ptp_msg_type(msg) != PTP_MSG_DELAY_REQ) {
		return;
	}

	for (iter = sys_slist_peek_head(&port->delay_req_list);
	     iter;
	     iter = sys_slist_peek_next(iter), last = iter) {

		req =  CONTAINER_OF(iter, struct ptp_msg, node);

		if (req->header.sequence_id != msg->header.sequence_id) {
			continue;
		}

		if (pkt->timestamp.second == UINT64_MAX ||
		    (pkt->timestamp.second == 0 && pkt->timestamp.nanosecond == 0)) {
			net_if_unregister_timestamp_cb(&port->delay_req_ts_cb);
			sys_slist_remove(&port->delay_req_list, last, iter);
			ptp_msg_unref(req);
			return;
		}

		req->timestamp.host._sec.high = ntohs(pkt->timestamp._sec.high);
		req->timestamp.host._sec.low = ntohl(pkt->timestamp._sec.low);
		req->timestamp.host.nanosecond = ntohl(pkt->timestamp.nanosecond);

		LOG_DBG("Port %d registered timestamp for %d Delay_Req",
			port->port_ds.id.port_number,
			ntohs(msg->header.sequence_id));

		if (iter == sys_slist_peek_tail(&port->delay_req_list)) {
			net_if_unregister_timestamp_cb(&port->delay_req_ts_cb);
		}
	}
}

static void port_sync_timestamp_cb(struct net_pkt *pkt)
{
	struct ptp_port *port = ptp_clock_port_from_iface(pkt->iface);
	struct ptp_msg *msg = ptp_msg_from_pkt(pkt);

	if (!port || !msg) {
		return;
	}

	msg->header.src_port_id.port_number = ntohs(msg->header.src_port_id.port_number);

	if (ptp_port_id_eq(&port->port_ds.id, &msg->header.src_port_id) &&
	    ptp_msg_type(msg) == PTP_MSG_SYNC) {

		const struct ptp_default_ds *dds = ptp_clock_default_ds();
		const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
		struct ptp_msg *resp = ptp_msg_alloc();

		if (!resp) {
			return;
		}

		resp->header.type_major_sdo_id = PTP_MSG_FOLLOW_UP;
		resp->header.version	       = PTP_VERSION;
		resp->header.msg_length	       = sizeof(struct ptp_follow_up_msg);
		resp->header.domain_number     = dds->domain;
		resp->header.flags[1]	       = tpds->flags;
		resp->header.src_port_id       = port->port_ds.id;
		resp->header.sequence_id       = port->seq_id.sync++;
		resp->header.log_msg_interval  = port->port_ds.log_sync_interval;

		resp->follow_up.precise_origin_timestamp.seconds_high = pkt->timestamp._sec.high;
		resp->follow_up.precise_origin_timestamp.seconds_low = pkt->timestamp._sec.low;
		resp->follow_up.precise_origin_timestamp.nanoseconds = pkt->timestamp.nanosecond;

		net_if_unregister_timestamp_cb(&port->sync_ts_cb);

		port_msg_send(port, resp, PTP_SOCKET_GENERAL);
		ptp_msg_unref(resp);

		LOG_DBG("Port %d sends Follow_Up message", port->port_ds.id.port_number);
	}
}

static int port_announce_msg_transmit(struct ptp_port *port)
{
	const struct ptp_parent_ds *pds = ptp_clock_parent_ds();
	const struct ptp_current_ds *cds = ptp_clock_current_ds();
	const struct ptp_default_ds *dds = ptp_clock_default_ds();
	const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
	struct ptp_msg *msg = ptp_msg_alloc();
	int ret;

	if (!msg) {
		return -ENOMEM;
	}

	msg->header.type_major_sdo_id = PTP_MSG_ANNOUNCE;
	msg->header.version	      = PTP_VERSION;
	msg->header.msg_length	      = sizeof(struct ptp_announce_msg);
	msg->header.domain_number     = dds->domain;
	msg->header.flags[1]	      = tpds->flags;
	msg->header.src_port_id	      = port->port_ds.id;
	msg->header.sequence_id	      = port->seq_id.announce++;
	msg->header.log_msg_interval  = port->port_ds.log_sync_interval;

	msg->announce.current_utc_offset = tpds->current_utc_offset;
	msg->announce.gm_priority1	 = pds->gm_priority1;
	msg->announce.gm_clk_quality	 = pds->gm_clk_quality;
	msg->announce.gm_priority2	 = pds->gm_priority2;
	msg->announce.gm_id		 = pds->gm_id;
	msg->announce.steps_rm		 = cds->steps_rm;
	msg->announce.time_src		 = tpds->time_src;

	ret = port_msg_send(port, msg, PTP_SOCKET_GENERAL);
	ptp_msg_unref(msg);

	if (ret < 0) {
		return -EFAULT;
	}

	LOG_DBG("Port %d sends Announce message", port->port_ds.id.port_number);
	return 0;
}

static int port_delay_req_msg_transmit(struct ptp_port *port)
{
	const struct ptp_default_ds *dds = ptp_clock_default_ds();
	struct ptp_msg *msg = ptp_msg_alloc();
	int ret;

	if (!msg) {
		return -ENOMEM;
	}

	msg->header.type_major_sdo_id = PTP_MSG_DELAY_REQ;
	msg->header.version	      = PTP_VERSION;
	msg->header.msg_length	      = sizeof(struct ptp_delay_req_msg);
	msg->header.domain_number     = dds->domain;
	msg->header.src_port_id	      = port->port_ds.id;
	msg->header.sequence_id	      = port->seq_id.delay++;
	msg->header.log_msg_interval  = DEFAULT_LOG_MSG_INTERVAL;

	net_if_register_timestamp_cb(&port->delay_req_ts_cb,
				     NULL,
				     port->iface,
				     port_delay_req_timestamp_cb);

	ret = port_msg_send(port, msg, PTP_SOCKET_EVENT);
	if (ret < 0) {
		ptp_msg_unref(msg);
		return -EFAULT;
	}

	sys_slist_append(&port->delay_req_list, &msg->node);

	LOG_DBG("Port %d sends Delay_Req message", port->port_ds.id.port_number);
	return 0;
}

static int port_sync_msg_transmit(struct ptp_port *port)
{
	const struct ptp_default_ds *dds = ptp_clock_default_ds();
	const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
	struct ptp_msg *msg = ptp_msg_alloc();
	int ret;

	if (!msg) {
		return -ENOMEM;
	}

	msg->header.type_major_sdo_id = PTP_MSG_SYNC;
	msg->header.version	      = PTP_VERSION;
	msg->header.msg_length	      = sizeof(struct ptp_sync_msg);
	msg->header.domain_number     = dds->domain;
	msg->header.flags[0]	      = PTP_MSG_TWO_STEP_FLAG;
	msg->header.flags[1]	      = tpds->flags;
	msg->header.src_port_id	      = port->port_ds.id;
	msg->header.sequence_id	      = port->seq_id.sync;
	msg->header.log_msg_interval  = port->port_ds.log_sync_interval;

	net_if_register_timestamp_cb(&port->sync_ts_cb,
				     NULL,
				     port->iface,
				     port_sync_timestamp_cb);

	ret = port_msg_send(port, msg, PTP_SOCKET_EVENT);
	ptp_msg_unref(msg);

	if (ret < 0) {
		return -EFAULT;
	}
	LOG_DBG("Port %d sends Sync message", port->port_ds.id.port_number);
	return 0;
}

static void port_timer_init(struct k_timer *timer, k_timer_expiry_t timeout_fn, void *user_data)
{
	k_timer_init(timer, timeout_fn, NULL);
	k_timer_user_data_set(timer, user_data);
}

static void port_timer_to_handler(struct k_timer *timer)
{
	struct ptp_port *port = (struct ptp_port *)k_timer_user_data_get(timer);

	if (timer == &port->timers.announce) {
		atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO);
	} else if (timer == &port->timers.sync) {
		atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO);
	} else if (timer == &port->timers.delay) {
		atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_DELAY_TO);
	} else if (timer == &port->timers.qualification) {
		atomic_set_bit(&port->timeouts, PTP_PORT_TIMER_QUALIFICATION_TO);
	}

	ptp_clock_signal_timeout();
}

static void foreign_clock_cleanup(struct ptp_foreign_tt_clock *foreign)
{
	struct ptp_msg *msg;
	int64_t timestamp, timeout, current = k_uptime_get() * NSEC_PER_MSEC;

	while (foreign->messages_count > FOREIGN_TIME_TRANSMITTER_THRESHOLD) {
		msg = (struct ptp_msg *)k_fifo_get(&foreign->messages, K_NO_WAIT);
		ptp_msg_unref(msg);
		foreign->messages_count--;
	}

	/* Remove messages that don't arrived at
	 * FOREIGN_TIME_TRANSMITTER_TIME_WINDOW (4 * announce interval) - IEEE 1588-2019 9.3.2.4.5
	 */
	while (!k_fifo_is_empty(&foreign->messages)) {
		msg = (struct ptp_msg *)k_fifo_peek_head(&foreign->messages);

		if (msg->header.log_msg_interval <= -31) {
			timeout = 0;
		} else if (msg->header.log_msg_interval >= 31) {
			timeout = INT64_MAX;
		} else if (msg->header.log_msg_interval > 0) {
			timeout = FOREIGN_TIME_TRANSMITTER_TIME_WINDOW_MUL *
				  (1 << msg->header.log_msg_interval) * NSEC_PER_SEC;
		} else {
			timeout = FOREIGN_TIME_TRANSMITTER_TIME_WINDOW_MUL * NSEC_PER_SEC /
				  (1 << (-msg->header.log_msg_interval));
		}

		timestamp = msg->timestamp.host.second * NSEC_PER_SEC +
			    msg->timestamp.host.nanosecond;

		if (current - timestamp < timeout) {
			/* Remaining messages are within time window */
			break;
		}

		msg = (struct ptp_msg *)k_fifo_get(&foreign->messages, K_NO_WAIT);
		ptp_msg_unref(msg);
		foreign->messages_count--;
	}
}

static void port_clear_foreign_clock_records(struct ptp_foreign_tt_clock *foreign)
{
	struct ptp_msg *msg;

	while (!k_fifo_is_empty(&foreign->messages)) {
		msg = (struct ptp_msg *)k_fifo_get(&foreign->messages, K_NO_WAIT);
		ptp_msg_unref(msg);
		foreign->messages_count--;
	}
}

static void port_delay_req_cleanup(struct ptp_port *port)
{
	sys_snode_t *prev = NULL;
	struct ptp_msg *msg;
	int64_t timestamp, current = k_uptime_get() * NSEC_PER_MSEC;

	SYS_SLIST_FOR_EACH_CONTAINER(&port->delay_req_list, msg, node) {
		timestamp = msg->timestamp.host.second * NSEC_PER_SEC +
			    msg->timestamp.host.nanosecond;

		if (current - timestamp < PORT_DELAY_REQ_CLEARE_TO) {
			break;
		}

		ptp_msg_unref(msg);
		sys_slist_remove(&port->delay_req_list, prev, &msg->node);
		prev = &msg->node;
	}
}

static void port_clear_delay_req(struct ptp_port *port)
{
	sys_snode_t *prev = NULL;
	struct ptp_msg *msg;

	SYS_SLIST_FOR_EACH_CONTAINER(&port->delay_req_list, msg, node) {
		ptp_msg_unref(msg);
		sys_slist_remove(&port->delay_req_list, prev, &msg->node);
		prev = &msg->node;
	}
}

static void port_sync_fup_ooo_handle(struct ptp_port *port, struct ptp_msg *msg)
{
	struct ptp_msg *last = port->last_sync_fup;

	if (ptp_msg_type(msg) != PTP_MSG_FOLLOW_UP &&
	    ptp_msg_type(msg) != PTP_MSG_SYNC) {
		return;
	}

	if (!last) {
		port->last_sync_fup = msg;
		ptp_msg_ref(msg);
		return;
	}

	if (ptp_msg_type(last) == PTP_MSG_SYNC &&
	    ptp_msg_type(msg) == PTP_MSG_FOLLOW_UP &&
	    msg->header.sequence_id == last->header.sequence_id) {

		port_synchronize(port,
				 last->timestamp.host,
				 msg->timestamp.protocol,
				 last->header.correction,
				 msg->header.correction);

		ptp_msg_unref(port->last_sync_fup);
		port->last_sync_fup = NULL;
	} else if (ptp_msg_type(last) == PTP_MSG_FOLLOW_UP &&
		   ptp_msg_type(msg) == PTP_MSG_SYNC &&
		   msg->header.sequence_id == last->header.sequence_id) {

		port_synchronize(port,
				 msg->timestamp.host,
				 last->timestamp.protocol,
				 msg->header.correction,
				 last->header.correction);

		ptp_msg_unref(port->last_sync_fup);
		port->last_sync_fup = NULL;
	} else {
		ptp_msg_unref(port->last_sync_fup);
		port->last_sync_fup = msg;
		ptp_msg_ref(msg);
	}
}

static int port_announce_msg_process(struct ptp_port *port, struct ptp_msg *msg)
{
	int ret = 0;
	const struct ptp_default_ds *dds = ptp_clock_default_ds();

	if (msg->announce.steps_rm >= dds->max_steps_rm) {
		return ret;
	}

	switch (ptp_port_state(port)) {
	case PTP_PS_INITIALIZING:
		__fallthrough;
	case PTP_PS_DISABLED:
		__fallthrough;
	case PTP_PS_FAULTY:
		break;
	case PTP_PS_LISTENING:
		__fallthrough;
	case PTP_PS_PRE_TIME_TRANSMITTER:
		__fallthrough;
	case PTP_PS_TIME_TRANSMITTER:
		__fallthrough;
	case PTP_PS_GRAND_MASTER:
#if CONFIG_PTP_FOREIGN_TIME_TRANSMITTER_FEATURE
		ret = ptp_port_add_foreign_tt(port, msg);
		break;
#else
		__fallthrough;
#endif
	case PTP_PS_TIME_RECEIVER:
		__fallthrough;
	case PTP_PS_UNCALIBRATED:
		__fallthrough;
	case PTP_PS_PASSIVE:
		ret = ptp_port_update_current_time_transmitter(port, msg);
		break;
	default:
		break;
	}

	return ret;
}

static void port_sync_msg_process(struct ptp_port *port, struct ptp_msg *msg)
{
	enum ptp_port_state state = ptp_port_state(port);

	if (state != PTP_PS_TIME_RECEIVER && state != PTP_PS_UNCALIBRATED) {
		return;
	}

	if (!ptp_msg_current_parent(msg)) {
		return;
	}

	if (port->port_ds.log_sync_interval != msg->header.log_msg_interval) {
		port->port_ds.log_sync_interval = msg->header.log_msg_interval;
	}

	msg->header.correction += port->port_ds.delay_asymmetry;

	if (!(msg->header.flags[0] & PTP_MSG_TWO_STEP_FLAG)) {
		port_synchronize(port,
				 msg->timestamp.host,
				 msg->timestamp.protocol,
				 msg->header.correction,
				 0);

		if (port->last_sync_fup) {
			ptp_msg_unref(port->last_sync_fup);
			port->last_sync_fup = NULL;
		}

		return;
	}

	port_sync_fup_ooo_handle(port, msg);
}

static void port_follow_up_msg_process(struct ptp_port *port, struct ptp_msg *msg)
{
	enum ptp_port_state state = ptp_port_state(port);

	if (state != PTP_PS_TIME_RECEIVER && state != PTP_PS_UNCALIBRATED) {
		return;
	}

	if (!ptp_msg_current_parent(msg)) {
		return;
	}

	port_sync_fup_ooo_handle(port, msg);
}

static int port_delay_req_msg_process(struct ptp_port *port, struct ptp_msg *msg)
{
	int ret;
	struct ptp_msg *resp;
	enum ptp_port_state state = ptp_port_state(port);
	const struct ptp_default_ds *dds = ptp_clock_default_ds();

	if (state != PTP_PS_TIME_TRANSMITTER && state != PTP_PS_GRAND_MASTER) {
		return 0;
	}

	resp = ptp_msg_alloc();
	if (!resp) {
		return -ENOMEM;
	}

	resp->header.type_major_sdo_id = PTP_MSG_DELAY_RESP;
	resp->header.version	       = PTP_VERSION;
	resp->header.msg_length	       = sizeof(struct ptp_delay_resp_msg);
	resp->header.domain_number     = dds->domain;
	resp->header.correction	       = msg->header.correction;
	resp->header.src_port_id       = port->port_ds.id;
	resp->header.sequence_id       = msg->header.sequence_id;
	resp->header.log_msg_interval  = port->port_ds.log_min_delay_req_interval;

	resp->delay_resp.receive_timestamp.seconds_high = msg->timestamp.host._sec.high;
	resp->delay_resp.receive_timestamp.seconds_low = msg->timestamp.host._sec.low;
	resp->delay_resp.receive_timestamp.nanoseconds = msg->timestamp.host.nanosecond;
	resp->delay_resp.req_port_id = msg->header.src_port_id;

	if (msg->header.flags[0] & PTP_MSG_UNICAST_FLAG) {
		/* TODO handle unicast messages */
		resp->header.flags[0] |= PTP_MSG_UNICAST_FLAG;
	}

	ret = port_msg_send(port, resp, PTP_SOCKET_EVENT);
	ptp_msg_unref(resp);

	if (ret < 0) {
		return -EFAULT;
	}

	LOG_DBG("Port %d responds to Delay_Req message", port->port_ds.id.port_number);
	return 0;
}

static void port_delay_resp_msg_process(struct ptp_port *port, struct ptp_msg *msg)
{
	uint64_t t3, t4, t4c;
	sys_snode_t *prev = NULL;
	struct ptp_msg *req;
	enum ptp_port_state state = ptp_port_state(port);

	if (state != PTP_PS_TIME_RECEIVER && state != PTP_PS_UNCALIBRATED) {
		return;
	}

	if (!ptp_port_id_eq(&msg->delay_resp.req_port_id, &port->port_ds.id)) {
		/* Message is not meant for this PTP Port */
		return;
	}

	SYS_SLIST_FOR_EACH_CONTAINER(&port->delay_req_list, req, node) {
		if (msg->header.sequence_id == ntohs(req->header.sequence_id)) {
			break;
		}
		prev = &req->node;
	}

	if (!req) {
		return;
	}

	t3 = req->timestamp.host.second * NSEC_PER_SEC + req->timestamp.host.nanosecond;
	t4 = msg->timestamp.protocol.second * NSEC_PER_SEC + msg->timestamp.protocol.nanosecond;
	t4c = t4 - (msg->header.correction >> 16);

	ptp_clock_delay(t3, t4c);

	sys_slist_remove(&port->delay_req_list, prev, &req->node);
	ptp_msg_unref(req);

	port->port_ds.log_min_delay_req_interval = msg->header.log_msg_interval;
}

static struct ptp_msg *port_management_resp_prepare(struct ptp_port *port, struct ptp_msg *req)
{
	struct ptp_msg *resp = ptp_msg_alloc();
	const struct ptp_default_ds *dds = ptp_clock_default_ds();

	if (!resp) {
		return NULL;
	}

	resp->header.type_major_sdo_id = PTP_MSG_MANAGEMENT;
	resp->header.version	       = PTP_VERSION;
	resp->header.msg_length	       = sizeof(struct ptp_management_msg);
	resp->header.domain_number     = dds->domain;
	resp->header.src_port_id       = port->port_ds.id;
	resp->header.sequence_id       = req->header.sequence_id;
	resp->header.log_msg_interval  = port->port_ds.log_min_delay_req_interval;

	if (req->management.action == PTP_MGMT_GET ||
	    req->management.action == PTP_MGMT_SET) {
		resp->management.action = PTP_MGMT_RESP;
	} else if (req->management.action == PTP_MGMT_CMD) {
		resp->management.action = PTP_MGMT_ACK;
	}

	memcpy(&resp->management.target_port_id,
	       &req->header.src_port_id,
	       sizeof(struct ptp_port_id));

	resp->management.starting_boundary_hops = req->management.starting_boundary_hops -
						 req->management.boundary_hops;
	resp->management.boundary_hops = resp->management.starting_boundary_hops;

	return resp;
}

static int port_management_resp_tlv_fill(struct ptp_port *port,
					 struct ptp_msg *req,
					 struct ptp_msg *resp,
					 struct ptp_tlv_mgmt *req_mgmt)
{
	int length = 0;
	struct ptp_tlv_mgmt *mgmt;
	struct ptp_tlv_default_ds *tlv_dds;
	struct ptp_tlv_parent_ds *tlv_pds;
	const struct ptp_parent_ds *pds = ptp_clock_parent_ds();
	const struct ptp_current_ds *cds = ptp_clock_current_ds();
	const struct ptp_default_ds *dds = ptp_clock_default_ds();
	const struct ptp_time_prop_ds *tpds = ptp_clock_time_prop_ds();
	struct ptp_tlv_container *container  = ptp_tlv_alloc();

	if (!container) {
		return -ENOMEM;
	}

	container->tlv = (struct ptp_tlv *)resp->management.suffix;
	mgmt = (struct ptp_tlv_mgmt *)container->tlv;
	mgmt->type = PTP_TLV_TYPE_MANAGEMENT;
	mgmt->id = req_mgmt->id;

	switch (mgmt->id) {
	case PTP_MGMT_DEFAULT_DATA_SET:
		tlv_dds = (struct ptp_tlv_default_ds *)mgmt->data;

		length = sizeof(struct ptp_tlv_default_ds);
		tlv_dds->flags = 0x1 | (dds->time_receiver_only << 1);
		tlv_dds->n_ports = dds->n_ports;
		tlv_dds->priority1 = dds->priority1;
		tlv_dds->priority2 = dds->priority2;
		tlv_dds->domain = dds->domain;
		memcpy(&tlv_dds->clk_id, &dds->clk_id, sizeof(tlv_dds->clk_id));
		memcpy(&tlv_dds->clk_quality, &dds->clk_quality, sizeof(tlv_dds->clk_quality));
		break;
	case PTP_MGMT_CURRENT_DATA_SET:
		length = sizeof(struct ptp_tlv_current_ds);
		memcpy(mgmt->data, cds, sizeof(struct ptp_tlv_current_ds));
		break;
	case PTP_MGMT_PARENT_DATA_SET:
		tlv_pds = (struct ptp_tlv_parent_ds *)mgmt->data;

		length = sizeof(struct ptp_tlv_parent_ds);

		tlv_pds->obsreved_parent_offset_scaled_log_variance =
			pds->obsreved_parent_offset_scaled_log_variance;
		tlv_pds->obsreved_parent_clk_phase_change_rate =
			pds->obsreved_parent_clk_phase_change_rate;
		tlv_pds->gm_priority1 = pds->gm_priority1;
		tlv_pds->gm_priority2 = pds->gm_priority2;
		memcpy(&tlv_pds->port_id, &pds->port_id, sizeof(tlv_pds->port_id));
		memcpy(&tlv_pds->gm_id, &pds->gm_id, sizeof(tlv_pds->gm_id));
		memcpy(&tlv_pds->gm_clk_quality,
		       &pds->gm_clk_quality,
		       sizeof(tlv_pds->gm_clk_quality));

		break;
	case PTP_MGMT_TIME_PROPERTIES_DATA_SET:
		length = sizeof(struct ptp_tlv_time_prop_ds);
		memcpy(mgmt->data, tpds, sizeof(struct ptp_tlv_time_prop_ds));
		break;
	case PTP_MGMT_PORT_DATA_SET:
		length = sizeof(struct ptp_tlv_port_ds);
		memcpy(mgmt->data, &port->port_ds, sizeof(struct ptp_tlv_port_ds));
		break;
	case PTP_MGMT_PRIORITY1:
		length = sizeof(dds->priority1);
		*mgmt->data = dds->priority1;
		break;
	case PTP_MGMT_PRIORITY2:
		length = sizeof(dds->priority2);
		*mgmt->data = dds->priority2;
		break;
	case PTP_MGMT_DOMAIN:
		length = sizeof(dds->domain);
		*mgmt->data = dds->domain;
		break;
	case PTP_MGMT_TIME_RECEIVER_ONLY:
		length = sizeof(dds->time_receiver_only);
		*mgmt->data = dds->time_receiver_only;
		break;
	case PTP_MGMT_LOG_ANNOUNCE_INTERVAL:
		length = sizeof(port->port_ds.log_announce_interval);
		*mgmt->data = port->port_ds.log_announce_interval;
		break;
	case PTP_MGMT_LOG_SYNC_INTERVAL:
		length = sizeof(port->port_ds.log_sync_interval);
		*mgmt->data = port->port_ds.log_sync_interval;
		break;
	case PTP_MGMT_VERSION_NUMBER:
		length = sizeof(port->port_ds.version);
		*mgmt->data = port->port_ds.version;
		break;
	case PTP_MGMT_CLOCK_ACCURACY:
		length = sizeof(dds->clk_quality.accuracy);
		*mgmt->data = dds->clk_quality.accuracy;
		break;
	case PTP_MGMT_DELAY_MECHANISM:
		length = sizeof(port->port_ds.delay_mechanism);
		*(uint16_t *)mgmt->data = port->port_ds.delay_mechanism;
		break;
	default:
		ptp_tlv_free(container);
		return -EINVAL;
	}

	/* Management TLV length shall be an even number */
	if (length % 2) {
		mgmt->data[length] = 0;
		length++;
	}

	container->tlv->length = sizeof(mgmt->id) + length;
	resp->header.msg_length += sizeof(*container->tlv) + container->tlv->length;
	sys_slist_append(&resp->tlvs, &container->node);

	return 0;
}

static int port_management_set(struct ptp_port *port,
			       struct ptp_msg *req,
			       struct ptp_tlv_mgmt *tlv)
{
	bool send_resp = false;

	switch (tlv->id) {
	case PTP_MGMT_LOG_ANNOUNCE_INTERVAL:
		port->port_ds.log_announce_interval = *tlv->data;
		send_resp = true;
		break;
	case PTP_MGMT_LOG_SYNC_INTERVAL:
		port->port_ds.log_sync_interval = *tlv->data;
		send_resp = true;
		break;
	case PTP_MGMT_UNICAST_NEGOTIATION_ENABLE:
		/* TODO unicast */
		break;
	default:
		break;
	}

	return send_resp ? ptp_port_management_resp(port, req, tlv) : 0;
}

static int port_enable(struct ptp_port *port)
{
	while (!net_if_is_up(port->iface)) {
		return -1;
	}

	port->link_status = PORT_LINK_UP;

	if (ptp_transport_open(port)) {
		LOG_ERR("Couldn't open socket on Port %d.", port->port_ds.id.port_number);
		return -1;
	}

	port->port_ds.enable = true;

	ptp_clock_pollfd_invalidate();
	LOG_DBG("Port %d opened", port->port_ds.id.port_number);
	return 0;
}

static bool port_is_enabled(struct ptp_port *port)
{
	enum ptp_port_state state = ptp_port_state(port);

	if (state == PTP_PS_FAULTY ||
	    state == PTP_PS_DISABLED ||
	    state == PTP_PS_INITIALIZING) {
		return false;
	}
	return true;
}

static void port_disable(struct ptp_port *port)
{
	k_timer_stop(&port->timers.announce);
	k_timer_stop(&port->timers.delay);
	k_timer_stop(&port->timers.sync);
	k_timer_stop(&port->timers.qualification);

	atomic_clear(&port->timeouts);

	ptp_transport_close(port);
	ptp_port_free_foreign_tts(port);
	port->best = NULL;

	net_if_unregister_timestamp_cb(&port->sync_ts_cb);
	net_if_unregister_timestamp_cb(&port->delay_req_ts_cb);

	ptp_clock_pollfd_invalidate();
	port->port_ds.enable = false;
	LOG_DBG("Port %d disabled", port->port_ds.id.port_number);
}

int port_state_update(struct ptp_port *port, enum ptp_port_event event, bool tt_diff)
{
	enum ptp_port_state next_state = port->state_machine(ptp_port_state(port),
							     event,
							     tt_diff);

	if (next_state == PTP_PS_FAULTY) {
		/* clear fault if interface is UP */
		if (net_if_oper_state(port->iface) == NET_IF_OPER_UP) {
			next_state = port->state_machine(next_state, PTP_EVT_FAULT_CLEARED, false);
		}
	}

	if (next_state == PTP_PS_INITIALIZING) {
		if (port_is_enabled(port)) {
			port_disable(port);
		}
		if (port_enable(port)) {
			event = PTP_EVT_FAULT_DETECTED;
		} else {
			event = PTP_EVT_INIT_COMPLETE;
		}
		next_state = port->state_machine(next_state, event, false);
	}

	if (next_state != ptp_port_state(port)) {
		LOG_DBG("Port %d changed state from %s to %s",
			port->port_ds.id.port_number,
			port_state_str(ptp_port_state(port)),
			port_state_str(next_state));

		port->port_ds.state = next_state;
		return 1;
	}

	return 0;
}

static void port_link_monitor(struct net_mgmt_event_callback *cb,
			      uint32_t mgmt_event,
			      struct net_if *iface)
{
	ARG_UNUSED(cb);

	enum ptp_port_event event = PTP_EVT_NONE;
	struct ptp_port *port = ptp_clock_port_from_iface(iface);
	uint8_t iface_state = mgmt_event == NET_EVENT_IF_UP ? PORT_LINK_UP : PORT_LINK_DOWN;

	if (!port) {
		return;
	}

	if (iface_state & port->link_status) {
		port->link_status = iface_state;
	} else {
		port->link_status = iface_state | PORT_LINK_CHANGED;
		LOG_DBG("Port %d link %s",
			port->port_ds.id.port_number,
			port->link_status & PORT_LINK_UP ? "up" : "down");
	}

	if (port->link_status & PORT_LINK_CHANGED) {
		event = iface_state == PORT_LINK_UP ?
			PTP_EVT_FAULT_CLEARED : PTP_EVT_FAULT_DETECTED;
		port->link_status ^= PORT_LINK_CHANGED;
	}

	if (port->link_status & PORT_LINK_DOWN) {
		ptp_clock_state_decision_req();
	}

	ptp_port_event_handle(port, event, false);
}

void ptp_port_init(struct net_if *iface, void *user_data)
{
	struct ptp_port *port;
	const struct ptp_default_ds *dds = ptp_clock_default_ds();

	ARG_UNUSED(user_data);

	if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
		return;
	}

	if (dds->n_ports > CONFIG_PTP_NUM_PORTS) {
		LOG_WRN("Exceeded number of PTP Ports.");
		return;
	}

	port = &ports[dds->n_ports];

	port->iface = iface;
	port->best = NULL;
	port->socket[PTP_SOCKET_EVENT] = -1;
	port->socket[PTP_SOCKET_GENERAL] = -1;

	port->state_machine = dds->time_receiver_only ? ptp_tr_state_machine : ptp_state_machine;
	port->last_sync_fup = NULL;

	port_ds_init(port);
	sys_slist_init(&port->foreign_list);
	sys_slist_init(&port->delay_req_list);

	port_timer_init(&port->timers.delay, port_timer_to_handler, port);
	port_timer_init(&port->timers.announce, port_timer_to_handler, port);
	port_timer_init(&port->timers.sync, port_timer_to_handler, port);
	port_timer_init(&port->timers.qualification, port_timer_to_handler, port);

	ptp_clock_pollfd_invalidate();
	ptp_clock_port_add(port);

	net_mgmt_init_event_callback(&port->link_cb, port_link_monitor, PORT_LINK_EVENT_MASK);
	net_mgmt_add_event_callback(&port->link_cb);

	LOG_DBG("Port %d initialized", port->port_ds.id.port_number);
}

enum ptp_port_event ptp_port_event_gen(struct ptp_port *port, int idx)
{
	enum ptp_port_event event = PTP_EVT_NONE;
	struct ptp_msg *msg;
	int ret, cnt;

	if (idx < 0) {
		return event;
	}

	msg = ptp_msg_alloc();
	if (!msg) {
		return PTP_EVT_FAULT_DETECTED;
	}

	cnt = ptp_transport_recv(port, msg, idx);
	if (cnt <= 0) {
		LOG_ERR("Error during message reception");
		ptp_msg_unref(msg);
		return PTP_EVT_FAULT_DETECTED;
	}

	ret = ptp_msg_post_recv(port, msg, cnt);
	if (ret) {
		ptp_msg_unref(msg);
		return PTP_EVT_FAULT_DETECTED;
	}

	if (ptp_port_id_eq(&msg->header.src_port_id, &port->port_ds.id)) {
		ptp_msg_unref(msg);
		return PTP_EVT_NONE;
	}

	switch (ptp_msg_type(msg)) {
	case PTP_MSG_SYNC:
		port_sync_msg_process(port, msg);
		break;
	case PTP_MSG_DELAY_REQ:
		if (port_delay_req_msg_process(port, msg)) {
			event = PTP_EVT_FAULT_DETECTED;
		}
		break;
	case PTP_MSG_PDELAY_REQ:
		__fallthrough;
	case PTP_MSG_PDELAY_RESP:
		__fallthrough;
	case PTP_MSG_PDELAY_RESP_FOLLOW_UP:
		/* P2P delay machanism not supported */
		break;
	case PTP_MSG_FOLLOW_UP:
		port_follow_up_msg_process(port, msg);
		break;
	case PTP_MSG_DELAY_RESP:
		port_delay_resp_msg_process(port, msg);
		break;
	case PTP_MSG_ANNOUNCE:
		if (port_announce_msg_process(port, msg)) {
			event = PTP_EVT_STATE_DECISION;
		}
		break;
	case PTP_MSG_SIGNALING:
		/* Signalling messages not supported */
		break;
	case PTP_MSG_MANAGEMENT:
		if (ptp_clock_management_msg_process(port, msg)) {
			event = PTP_EVT_STATE_DECISION;
		}
		break;
	default:
		break;
	}

	ptp_msg_unref(msg);
	return event;
}

void ptp_port_event_handle(struct ptp_port *port, enum ptp_port_event event, bool tt_diff)
{
	const struct ptp_current_ds *cds = ptp_clock_current_ds();

	if (event == PTP_EVT_NONE) {
		return;
	}

	if (!port_state_update(port, event, tt_diff)) {
		/* No PTP Port state change */
		return;
	}

	k_timer_stop(&port->timers.announce);
	k_timer_stop(&port->timers.delay);
	k_timer_stop(&port->timers.sync);
	k_timer_stop(&port->timers.qualification);

	switch (port->port_ds.state) {
	case PTP_PS_INITIALIZING:
		break;
	case PTP_PS_FAULTY:
		__fallthrough;
	case PTP_PS_DISABLED:
		port_disable(port);
		break;
	case PTP_PS_LISTENING:
		port_timer_set_timeout_random(&port->timers.announce,
					      port->port_ds.announce_receipt_timeout,
					      1,
					      port->port_ds.log_announce_interval);
		break;
	case PTP_PS_PRE_TIME_TRANSMITTER:
		port_timer_set_timeout(&port->timers.qualification,
				       1 + cds->steps_rm,
				       port->port_ds.log_announce_interval);
		break;
	case PTP_PS_GRAND_MASTER:
		__fallthrough;
	case PTP_PS_TIME_TRANSMITTER:
		port_timer_set_timeout(&port->timers.announce,
				       1,
				       port->port_ds.log_announce_interval);
		port_timer_set_timeout(&port->timers.sync, 1, port->port_ds.log_sync_interval);
		break;
	case PTP_PS_PASSIVE:
		port_timer_set_timeout_random(&port->timers.announce,
					      port->port_ds.announce_receipt_timeout,
					      1,
					      port->port_ds.log_announce_interval);
		break;
	case PTP_PS_UNCALIBRATED:
		if (port->last_sync_fup) {
			ptp_msg_unref(port->last_sync_fup);
			port->last_sync_fup = NULL;
		}
		port_clear_delay_req(port);
		__fallthrough;
	case PTP_PS_TIME_RECEIVER:
		port_timer_set_timeout_random(&port->timers.announce,
					      port->port_ds.announce_receipt_timeout,
					      1,
					      port->port_ds.log_announce_interval);
		port_timer_set_timeout_random(&port->timers.delay,
					      0,
					      2,
					      port->port_ds.log_min_delay_req_interval);
		break;
	};
}

enum ptp_port_state ptp_port_state(struct ptp_port *port)
{
	return (enum ptp_port_state)port->port_ds.state;
}

enum ptp_port_event ptp_port_timer_event_gen(struct ptp_port *port, struct k_timer *timer)
{
	enum ptp_port_event event = PTP_EVT_NONE;
	enum ptp_port_state state = ptp_port_state(port);

	switch (state) {
	case PTP_PS_PRE_TIME_TRANSMITTER:
		if (timer == &port->timers.qualification &&
		    atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_QUALIFICATION_TO)) {
			LOG_DBG("Port %d Qualification timeout", port->port_ds.id.port_number);
			atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_QUALIFICATION_TO);

			return PTP_EVT_QUALIFICATION_TIMEOUT_EXPIRES;
		}
		break;
	case PTP_PS_GRAND_MASTER:
		__fallthrough;
	case PTP_PS_TIME_TRANSMITTER:
		if (timer == &port->timers.sync &&
		    atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO)) {
			LOG_DBG("Port %d TX Sync timeout", port->port_ds.id.port_number);
			port_timer_set_timeout(&port->timers.sync,
					       1,
					       port->port_ds.log_sync_interval);
			atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO);

			return port_sync_msg_transmit(port) == 0 ? PTP_EVT_NONE :
								   PTP_EVT_FAULT_DETECTED;
		}

		if (timer == &port->timers.announce &&
		    atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO)) {
			LOG_DBG("Port %d TimeTransmitter Announce timeout",
				port->port_ds.id.port_number);
			port_timer_set_timeout(&port->timers.announce,
					       1,
					       port->port_ds.log_announce_interval);
			atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO);

			return port_announce_msg_transmit(port) == 0 ? PTP_EVT_NONE :
								       PTP_EVT_FAULT_DETECTED;
		}
		break;
	case PTP_PS_TIME_RECEIVER:
		if (timer == &port->timers.delay &&
		    atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_DELAY_TO)) {

			atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_DELAY_TO);
			port_delay_req_cleanup(port);
			port_timer_set_timeout(&port->timers.delay,
					       1,
					       port->port_ds.log_announce_interval);

			if (port_delay_req_msg_transmit(port) < 0) {
				return PTP_EVT_FAULT_DETECTED;
			}
		}
		__fallthrough;
	case PTP_PS_PASSIVE:
		__fallthrough;
	case PTP_PS_UNCALIBRATED:
		__fallthrough;
	case PTP_PS_LISTENING:
		if ((timer == &port->timers.announce || timer == &port->timers.sync) &&
		    (atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO) ||
		     atomic_test_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO))) {

			LOG_DBG("Port %d %s timeout",
				port->port_ds.id.port_number,
				timer == &port->timers.announce ? "Announce" : "RX Sync");

			atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_ANNOUNCE_TO);
			atomic_clear_bit(&port->timeouts, PTP_PORT_TIMER_SYNC_TO);

			if (port->best) {
				port_clear_foreign_clock_records(port->best);
			}

			port_delay_req_cleanup(port);
			port_timer_set_timeout_random(&port->timers.announce,
						      port->port_ds.announce_receipt_timeout,
						      1,
						      port->port_ds.log_announce_interval);

			return PTP_EVT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES;
		}
		break;
	default:
		break;
	}

	return event;
}

bool ptp_port_id_eq(const struct ptp_port_id *p1, const struct ptp_port_id *p2)
{
	return memcmp(p1, p2, sizeof(struct ptp_port_id)) == 0;
}

struct ptp_dataset *ptp_port_best_foreign_ds(struct ptp_port *port)
{
	return port->best ? &port->best->dataset : NULL;
}

struct ptp_foreign_tt_clock *ptp_port_best_foreign(struct ptp_port *port)
{
	struct ptp_foreign_tt_clock *foreign;
	struct ptp_announce_msg *last;

	port->best = NULL;

	if (port->port_ds.time_transmitter_only) {
		return NULL;
	}

	SYS_SLIST_FOR_EACH_CONTAINER(&port->foreign_list, foreign, node) {
		if (!foreign->messages_count) {
			continue;
		}

		foreign_clock_cleanup(foreign);

		if (foreign->messages_count < FOREIGN_TIME_TRANSMITTER_THRESHOLD) {
			continue;
		}

		last = (struct ptp_announce_msg *)k_fifo_peek_head(&foreign->messages);

		foreign->dataset.priority1 = last->gm_priority1;
		foreign->dataset.priority2 = last->gm_priority2;
		foreign->dataset.steps_rm = last->steps_rm;

		memcpy(&foreign->dataset.clk_quality,
		       &last->gm_clk_quality,
		       sizeof(last->gm_clk_quality));
		memcpy(&foreign->dataset.clk_id, &last->gm_id, sizeof(last->gm_id));
		memcpy(&foreign->dataset.receiver, &port->port_ds.id, sizeof(port->port_ds.id));

		if (!port->best) {
			port->best = foreign;
		} else if (ptp_btca_ds_cmp(&foreign->dataset, &port->best->dataset)) {
			port->best = foreign;
		} else {
			port_clear_foreign_clock_records(foreign);
		}
	}
	return port->best;
}

int ptp_port_add_foreign_tt(struct ptp_port *port, struct ptp_msg *msg)
{
	struct ptp_foreign_tt_clock *foreign;
	struct ptp_msg *last;
	int diff = 0;

	SYS_SLIST_FOR_EACH_CONTAINER(&port->foreign_list, foreign, node) {
		if (ptp_port_id_eq(&msg->header.src_port_id, &foreign->dataset.sender)) {
			break;
		}
	}

	if (!foreign) {
		LOG_DBG("Port %d has a new foreign timeTransmitter %s",
			port->port_ds.id.port_number,
			port_id_str(&msg->header.src_port_id));

		int ret = k_mem_slab_alloc(&foreign_tts_slab, (void **)&foreign, K_NO_WAIT);

		if (ret) {
			LOG_ERR("Couldn't allocate memory for new foreign timeTransmitter");
			return 0;
		}

		memset(foreign, 0, sizeof(*foreign));
		memcpy(&foreign->dataset.sender,
		       &msg->header.src_port_id,
		       sizeof(foreign->dataset.sender));
		k_fifo_init(&foreign->messages);
		foreign->port = port;

		sys_slist_append(&port->foreign_list, &foreign->node);

		/* First message is not added to records. */
		return 0;
	}

	foreign_clock_cleanup(foreign);
	ptp_msg_ref(msg);

	foreign->messages_count++;
	k_fifo_put(&foreign->messages, (void *)msg);

	if (foreign->messages_count > 1) {
		last = (struct ptp_msg *)k_fifo_peek_tail(&foreign->messages);
		diff = ptp_msg_announce_cmp(&msg->announce, &last->announce);
	}

	return (foreign->messages_count == FOREIGN_TIME_TRANSMITTER_THRESHOLD ? 1 : 0) || diff;
}

void ptp_port_free_foreign_tts(struct ptp_port *port)
{
	sys_snode_t *iter;
	struct ptp_foreign_tt_clock *foreign;

	while (!sys_slist_is_empty(&port->foreign_list)) {
		iter = sys_slist_get(&port->foreign_list);
		foreign = CONTAINER_OF(iter, struct ptp_foreign_tt_clock, node);

		while (foreign->messages_count > FOREIGN_TIME_TRANSMITTER_THRESHOLD) {
			struct ptp_msg *msg = (struct ptp_msg *)k_fifo_get(&foreign->messages,
									   K_NO_WAIT);
			foreign->messages_count--;
			ptp_msg_unref(msg);
		}

		k_mem_slab_free(&foreign_tts_slab, (void *)foreign);
	}
}

int ptp_port_update_current_time_transmitter(struct ptp_port *port, struct ptp_msg *msg)
{
	struct ptp_foreign_tt_clock *foreign = port->best;

	if (!foreign ||
	    !ptp_port_id_eq(&msg->header.src_port_id, &foreign->dataset.sender)) {
		return ptp_port_add_foreign_tt(port, msg);
	}

	foreign_clock_cleanup(foreign);
	ptp_msg_ref(msg);

	k_fifo_put(&foreign->messages, (void *)msg);
	foreign->messages_count++;

	port_timer_set_timeout_random(&port->timers.announce,
				      port->port_ds.announce_receipt_timeout,
				      1,
				      port->port_ds.log_announce_interval);

	if (foreign->messages_count > 1) {
		struct ptp_msg *last = (struct ptp_msg *)k_fifo_peek_tail(&foreign->messages);

		return ptp_msg_announce_cmp(&msg->announce, &last->announce);
	}

	return 0;
}

int ptp_port_management_msg_process(struct ptp_port *port,
				    struct ptp_port *ingress,
				    struct ptp_msg *msg,
				    struct ptp_tlv_mgmt *tlv)
{
	int ret = 0;
	uint16_t target_port = msg->management.target_port_id.port_number;

	if (target_port != port->port_ds.id.port_number && target_port != 0xFFFF) {
		return ret;
	}

	if (ptp_mgmt_action(msg) == PTP_MGMT_SET) {
		ret = port_management_set(port, msg, tlv);
	} else {
		ret = ptp_port_management_resp(port, msg, tlv);
	}

	return ret;
}

int ptp_port_management_error(struct ptp_port *port, struct ptp_msg *msg, enum ptp_mgmt_err err)
{
	int ret;
	struct ptp_tlv *tlv;
	struct ptp_tlv_mgmt_err *mgmt_err;
	struct ptp_tlv_mgmt *mgmt = (struct ptp_tlv_mgmt *)msg->management.suffix;

	struct ptp_msg *resp = port_management_resp_prepare(port, msg);

	if (!resp) {
		return -ENOMEM;
	}

	tlv = ptp_msg_add_tlv(resp, sizeof(struct ptp_tlv_mgmt_err));
	if (!tlv) {
		ptp_msg_unref(resp);
		return -ENOMEM;
	}

	mgmt_err = (struct ptp_tlv_mgmt_err *)tlv;

	mgmt_err->type = PTP_TLV_TYPE_MANAGEMENT_ERROR_STATUS;
	mgmt_err->length = 8;
	mgmt_err->err_id = err;
	mgmt_err->id = mgmt->id;

	ret = port_msg_send(port, resp, PTP_SOCKET_GENERAL);
	ptp_msg_unref(resp);

	if (ret) {
		return -EFAULT;
	}

	LOG_DBG("Port %d sends Menagement Error message", port->port_ds.id.port_number);
	return 0;
}

int ptp_port_management_resp(struct ptp_port *port, struct ptp_msg *req, struct ptp_tlv_mgmt *tlv)
{
	int ret;
	struct ptp_msg *resp = port_management_resp_prepare(port, req);

	if (!resp) {
		return -ENOMEM;
	}

	ret = port_management_resp_tlv_fill(port, req, resp, tlv);
	if (ret) {
		return ret;
	}

	ret = port_msg_send(port, resp, PTP_SOCKET_GENERAL);
	ptp_msg_unref(resp);

	if (ret < 0) {
		return -EFAULT;
	}

	LOG_DBG("Port %d sends Menagement message response", port->port_ds.id.port_number);
	return 0;
}