milan-avb: gptp: rework management I/O as non-blocking with sequence-id matching

src/modules/module-avb/gptp.c

@@ -5,184 +5,277 @@
 
 #include "gptp.h"
 
-#include <sys/ioctl.h>
+#include <errno.h>
+#include <inttypes.h>
+#include <stddef.h>
+#include <stdio.h>
 #include <sys/socket.h>
 #include <sys/un.h>
 #include <time.h>
 
-#include <pipewire/log.h>
+#include <pipewire/pipewire.h>
-#include <pipewire/properties.h>
 #include <spa/utils/cleanup.h>
 #include <spa/utils/hook.h>
 
+#include "aecp-aem-descriptors.h"
+
 #define server_emit(s,m,v,...) spa_hook_list_call(&s->listener_list, struct server_events, m, v, ##__VA_ARGS__)
 #define server_emit_gm_changed(s, n, g)	server_emit(s, gm_changed, 0, n, g)
 
+#define PTP_REQUEST_TIMEOUT_NS	(SPA_NSEC_PER_SEC / 2)
+
 struct gptp {
 	struct server *server;
 
 	struct spa_hook server_listener;
+	struct spa_source *source;
 
 	char *ptp_mgmt_socket_path;
 	int ptp_fd;
-	uint32_t ptp_seq;
+	uint16_t ptp_seq;
 	uint8_t clock_id[8];
 	uint8_t gm_id[8];
+
+	bool req_in_flight;
+	uint16_t req_sequence_id;
+	uint16_t req_management_id;
+	uint64_t req_sent_ns;
+
+	uint32_t tick_count;
+	bool data_valid;
 };
 
-static int make_unix_ptp_mgmt_socket(const char *path)
+static int make_bind_path(char *out, size_t out_size, uint64_t entity_id)
+{
+	int len = snprintf(out, out_size,
+			"/tmp/pipewire-avb-gptp-%016" PRIx64, entity_id);
+	if (len < 0 || (size_t)len >= out_size) {
+		return -1;
+	}
+	return 0;
+}
+
+static int make_unix_ptp_mgmt_socket(const char *path, uint64_t entity_id)
 {
 	struct sockaddr_un addr;
+	char bind_path[64];
+	int val = 1;
 
-	spa_autoclose int fd = socket(AF_UNIX, SOCK_DGRAM | SOCK_CLOEXEC, 0);
+	if (make_bind_path(bind_path, sizeof(bind_path), entity_id) < 0) {
+		pw_log_warn("Failed to format PTP management bind path");
+		return -1;
+	}
+
+	spa_autoclose int fd = socket(AF_UNIX,
+			SOCK_DGRAM | SOCK_CLOEXEC | SOCK_NONBLOCK, 0);
 	if (fd < 0) {
 		pw_log_warn("Failed to create PTP management socket: %m");
 		return -1;
 	}
 
-	int val = 1;
 	if (setsockopt(fd, SOL_SOCKET, SO_PASSCRED, &val, sizeof(val)) < 0) {
 		pw_log_warn("Failed to set SO_PASSCRED on PTP management socket: %m");
 		return -1;
 	}
 
+	unlink(bind_path);
+	spa_zero(addr);
+	addr.sun_family = AF_UNIX;
+	strncpy(addr.sun_path, bind_path, sizeof(addr.sun_path) - 1);
+	if (bind(fd, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
+		pw_log_warn("Failed to bind PTP management socket to '%s': %m",
+				bind_path);
+		return -1;
+	}
+	pw_log_info("PTP management socket bound to '%s'", bind_path);
+
 	spa_zero(addr);
 	addr.sun_family = AF_UNIX;
 	strncpy(addr.sun_path, path, sizeof(addr.sun_path) - 1);
-
 	if (connect(fd, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
 		pw_log_warn("Failed to connect PTP management socket: %m");
+		unlink(bind_path);
 		return -1;
 	}
+	pw_log_info("PTP management socket connected to '%s'", path);
 
 	return spa_steal_fd(fd);
 }
 
-static bool update_ts_refclk(struct gptp *gptp)
+static void on_ptp_mgmt_data(void *data, int fd, uint32_t mask);
+
+static void gptp_close_socket(struct gptp *gptp)
+{
+	char bind_path[64];
+
+	if (gptp->source) {
+		pw_loop_destroy_source(gptp->server->impl->loop, gptp->source);
+		gptp->source = NULL;
+	}
+	if (gptp->ptp_fd >= 0) {
+		close(gptp->ptp_fd);
+		gptp->ptp_fd = -1;
+		if (make_bind_path(bind_path, sizeof(bind_path),
+				gptp->server->entity_id) == 0) {
+			unlink(bind_path);
+		}
+	}
+	gptp->req_in_flight = false;
+}
+
+static int gptp_open_socket(struct gptp *gptp)
+{
+	struct impl *impl = gptp->server->impl;
+	int fd;
+
+	fd = make_unix_ptp_mgmt_socket(gptp->ptp_mgmt_socket_path,
+			gptp->server->entity_id);
+	if (fd < 0) {
+		return -1;
+	}
+
+	gptp->source = pw_loop_add_io(impl->loop, fd, SPA_IO_IN, false,
+			on_ptp_mgmt_data, gptp);
+	if (gptp->source == NULL) {
+		pw_log_warn("Failed to add PTP management IO source: %m");
+		close(fd);
+		return -1;
+	}
+	gptp->ptp_fd = fd;
+	return 0;
+}
+
+static struct avb_aem_desc_avb_interface *get_avb_interface(struct gptp *gptp)
+{
+	struct descriptor *d = server_find_descriptor(gptp->server,
+			AVB_AEM_DESC_AVB_INTERFACE, 0);
+	return d ? descriptor_body(d) : NULL;
+}
+
+static void update_avb_interface_clock_identity(struct gptp *gptp,
+		const uint8_t cid[8])
+{
+	struct avb_aem_desc_avb_interface *iface = get_avb_interface(gptp);
+	if (iface == NULL) {
+		return;
+	}
+	memcpy(&iface->clock_identity, cid, sizeof(iface->clock_identity));
+}
+
+static void update_avb_interface_default(struct gptp *gptp,
+		const struct ptp_default_data_set *dds)
+{
+	struct avb_aem_desc_avb_interface *iface = get_avb_interface(gptp);
+	if (iface == NULL) {
+		return;
+	}
+	memcpy(&iface->clock_identity, dds->clock_identity,
+			sizeof(iface->clock_identity));
+	iface->priority1 = dds->priority1;
+	iface->clock_class = dds->clock_class;
+	iface->clock_accuracy = dds->clock_accuracy;
+	iface->offset_scaled_log_variance = dds->offset_scaled_log_variance_be;
+	iface->priority2 = dds->priority2;
+	iface->domain_number = dds->domain_number;
+}
+
+static void update_avb_interface_port(struct gptp *gptp,
+		const struct ptp_port_data_set *pds)
+{
+	struct avb_aem_desc_avb_interface *iface = get_avb_interface(gptp);
+	if (iface == NULL) {
+		return;
+	}
+	iface->log_sync_interval = pds->log_sync_interval;
+	iface->log_announce_interval = pds->log_announce_interval;
+	iface->log_pdelay_interval = pds->log_min_pdelay_req_interval;
+	iface->port_number = pds->port_number_be;
+}
+
+static int send_management_request(struct gptp *gptp, uint16_t management_id,
+		uint64_t now_ns)
 {
 	struct ptp_management_msg req;
-	struct ptp_management_msg res;
+	ssize_t ret;
-	struct ptp_parent_data_set parent;
+	uint16_t seq;
-	struct timespec now;
-	uint8_t buf[sizeof(struct ptp_management_msg) + sizeof(struct ptp_parent_data_set)];
-	uint8_t *cid;
-	uint8_t *gmid;
-	uint8_t tmp;
-	ssize_t ret = 0;
-	uint16_t data_len;
-	int avail;
-	bool gmid_changed = false;
-
-	if (!gptp->ptp_mgmt_socket_path)
-		return false;
-	if (gptp->ptp_fd < 0) {
-		gptp->ptp_fd = make_unix_ptp_mgmt_socket(gptp->ptp_mgmt_socket_path);
-		if (gptp->ptp_fd < 0)
-			return false;
-	}
-
-	/* Read if something is left in the socket */
-	if (ioctl(gptp->ptp_fd, FIONREAD, &avail) == -1) {
-		pw_log_warn("Failed to get number of byes in ptp_fd input buffer: %m");
-		return false;
-	}
-	pw_log_debug("Clearing stale data: %u bytes", avail);
-	while (avail-- && (ret = read(gptp->ptp_fd, &tmp, 1)) > 0);
-	if (ret == -1) {
-		pw_log_warn("Failed to clear ptp_fd input buffer: %m");
-		return false;
-	}
 
 	spa_zero(req);
 
+	seq = gptp->ptp_seq++;
 	req.major_sdo_id_message_type = PTP_MESSAGE_TYPE_MANAGEMENT;
 	req.ver = PTP_VERSION_1588_2008_2_1;
 	req.message_length_be = htons(sizeof(struct ptp_management_msg));
 	spa_zero(req.clock_identity);
 	req.source_port_id_be = htons((uint16_t)gptp->server->entity_id);
 	req.log_message_interval = PTP_DEFAULT_LOG_MESSAGE_INTERVAL;
-	req.sequence_id_be = htons(gptp->ptp_seq++);
+	req.sequence_id_be = htons(seq);
 	memset(req.target_port_identity, 0xff, 8);
 	req.target_port_id_be = htons(0xffff);
 	req.starting_boundary_hops = 1;
 	req.boundary_hops = 1;
 	req.action = PTP_MGMT_ACTION_GET;
 	req.tlv_type_be = htons(PTP_TLV_TYPE_MGMT);
-	/* sent empty TLV, only sending management_id */
 	req.management_message_length_be = htons(2);
-	req.management_id_be = htons(PTP_MGMT_ID_PARENT_DATA_SET);
+	req.management_id_be = htons(management_id);
 
 	ret = write(gptp->ptp_fd, &req, sizeof(req));
 	if (ret == -1) {
+		if (errno == EAGAIN || errno == EWOULDBLOCK) {
+			pw_log_debug("PTP management write would block, skipping tick");
+			return -EAGAIN;
+		}
 		pw_log_warn("Failed to send PTP management request: %m");
-		if (errno != ENOTCONN)
+		return -errno;
-			return false;
-		close(gptp->ptp_fd);
-		gptp->ptp_fd = make_unix_ptp_mgmt_socket(gptp->ptp_mgmt_socket_path);
-		if (gptp->ptp_fd > -1)
-			pw_log_info("Reopened PTP management socket");
-		return false;
 	}
-	if (ret != sizeof(req)) {
+	if (ret != (ssize_t)sizeof(req)) {
 		pw_log_warn("Incomplete PTP management request: %zd of %zu bytes",
 				ret, sizeof(req));
-		return false;
+		return -EIO;
 	}
 
-	ret = read(gptp->ptp_fd, &buf, sizeof(buf));
+	gptp->req_in_flight = true;
-	if (ret == -1) {
+	gptp->req_sequence_id = seq;
-		pw_log_warn("Failed to receive PTP management response: %m");
+	gptp->req_management_id = management_id;
-		return false;
+	gptp->req_sent_ns = now_ns;
+	pw_log_info("PTP management request sent: id=%04x seq=%u",
+			management_id, seq);
+	return 0;
+}
+
+static void handle_parent_data_set(struct gptp *gptp,
+		const struct ptp_management_msg *res,
+		const uint8_t *payload, size_t payload_len)
+{
+	const struct ptp_parent_data_set *parent;
+	struct timespec ts;
+	uint16_t data_len;
+	const uint8_t *cid, *gmid;
+	bool gmid_changed = false;
+	bool cid_changed = false;
+
+	data_len = ntohs(res->management_message_length_be) - 2;
+	if (data_len != sizeof(struct ptp_parent_data_set) ||
+			payload_len < sizeof(struct ptp_parent_data_set)) {
+		pw_log_warn("Unexpected PTP GET PARENT_DATA_SET response length: "
+				"tlv=%u payload=%zu expected=%zu",
+				data_len, payload_len,
+				sizeof(struct ptp_parent_data_set));
+		return;
 	}
 
-	if (ret != sizeof(buf)) {
+	parent = (const struct ptp_parent_data_set *)payload;
-		pw_log_warn("Received incomplete PTP management response: %zd of %zu bytes",
-				ret, sizeof(buf));
-		return false;
-	}
 
-	res = *(struct ptp_management_msg *)buf;
+	cid = res->clock_identity;
-	parent = *(struct ptp_parent_data_set *)(buf + sizeof(struct ptp_management_msg));
+	if (memcmp(cid, gptp->clock_id, 8) != 0) {
-
-	if ((res.ver & 0x0f) != 2) {
-		pw_log_warn("PTP major version is %d, expected 2", res.ver);
-		return false;
-	}
-
-	if ((res.major_sdo_id_message_type & 0x0f) != PTP_MESSAGE_TYPE_MANAGEMENT) {
-		pw_log_warn("PTP management returned type %x, expected management", res.major_sdo_id_message_type);
-		return false;
-	}
-
-	if (res.action != PTP_MGMT_ACTION_RESPONSE) {
-		pw_log_warn("PTP management returned action %d, expected response", res.action);
-		return false;
-	}
-
-	if (ntohs(res.tlv_type_be) != PTP_TLV_TYPE_MGMT) {
-		pw_log_warn("PTP management returned tlv type %d, expected management", ntohs(res.tlv_type_be));
-		return false;
-	}
-
-	if (ntohs(res.management_id_be) != PTP_MGMT_ID_PARENT_DATA_SET) {
-		pw_log_warn("PTP management returned ID %d, expected PARENT_DATA_SET", ntohs(res.management_id_be));
-		return false;
-	}
-
-	data_len = ntohs(res.management_message_length_be) - 2;
-	if (data_len != sizeof(struct ptp_parent_data_set))
-		pw_log_warn("Unexpected PTP GET PARENT_DATA_SET response length %u, expected %zu",
-				data_len, sizeof(struct ptp_parent_data_set));
-
-	cid = res.clock_identity;
-	if (memcmp(cid, gptp->clock_id, 8) != 0)
 		pw_log_info("Local clock ID: IEEE1588-2008:"
 				"%02X-%02X-%02X-%02X-%02X-%02X-%02X-%02X:%d",
 				cid[0], cid[1], cid[2], cid[3],
 				cid[4], cid[5], cid[6], cid[7],
 				0 /* domain */);
+		cid_changed = true;
+	}
 
-	gmid = parent.gm_clock_id;
+	gmid = parent->gm_clock_id;
 	if (memcmp(gmid, gptp->gm_id, 8) != 0) {
 		pw_log_info("GM ID: IEEE1588-2008:"
 				"%02X-%02X-%02X-%02X-%02X-%02X-%02X-%02X:%d",
@@ -192,24 +285,202 @@ static bool update_ts_refclk(struct gptp *gptp)
 		gmid_changed = true;
 	}
 
-	/* When GM is not equal to own clock we are clocked by external master */
+	pw_log_debug("Synced to GM: %s",
-	pw_log_debug("Synced to GM: %s", (memcmp(cid, gmid, 8) != 0) ? "true" : "false");
+			(memcmp(cid, gmid, 8) != 0) ? "true" : "false");
 
 	memcpy(gptp->clock_id, cid, 8);
 	memcpy(gptp->gm_id, gmid, 8);
+	gptp->data_valid = true;
 
-	if (gmid_changed) {
+	/* IEEE 1722.1-2021 Section 7.2.8: AVB_INTERFACE.clock_identity is
-		clock_gettime(CLOCK_REALTIME, &now);
+	 * the local gPTP clock. */
-		server_emit_gm_changed(gptp->server, SPA_TIMESPEC_TO_NSEC(&now), gmid);
+	if (cid_changed) {
+		update_avb_interface_clock_identity(gptp, cid);
 	}
 
-	return gmid_changed;
+	if (gmid_changed) {
+		clock_gettime(CLOCK_REALTIME, &ts);
+		server_emit_gm_changed(gptp->server,
+				SPA_TIMESPEC_TO_NSEC(&ts), (uint8_t *)gmid);
+	}
+}
+
+static void handle_default_data_set(struct gptp *gptp,
+		const struct ptp_management_msg *res,
+		const uint8_t *payload, size_t payload_len)
+{
+	const struct ptp_default_data_set *dds;
+	uint16_t data_len;
+
+	data_len = ntohs(res->management_message_length_be) - 2;
+	if (data_len != sizeof(struct ptp_default_data_set) ||
+			payload_len < sizeof(struct ptp_default_data_set)) {
+		pw_log_warn("Unexpected PTP GET DEFAULT_DATA_SET response length: "
+				"tlv=%u payload=%zu expected=%zu",
+				data_len, payload_len,
+				sizeof(struct ptp_default_data_set));
+		return;
+	}
+	dds = (const struct ptp_default_data_set *)payload;
+	update_avb_interface_default(gptp, dds);
+}
+
+static void handle_port_data_set(struct gptp *gptp,
+		const struct ptp_management_msg *res,
+		const uint8_t *payload, size_t payload_len)
+{
+	const struct ptp_port_data_set *pds;
+	uint16_t data_len;
+
+	data_len = ntohs(res->management_message_length_be) - 2;
+	if (data_len != sizeof(struct ptp_port_data_set) ||
+			payload_len < sizeof(struct ptp_port_data_set)) {
+		pw_log_warn("Unexpected PTP GET PORT_DATA_SET response length: "
+				"tlv=%u payload=%zu expected=%zu",
+				data_len, payload_len,
+				sizeof(struct ptp_port_data_set));
+		return;
+	}
+	pds = (const struct ptp_port_data_set *)payload;
+	update_avb_interface_port(gptp, pds);
+}
+
+static void on_ptp_mgmt_data(void *data, int fd, uint32_t mask)
+{
+	struct gptp *gptp = data;
+	uint8_t buf[sizeof(struct ptp_management_msg) + 256];
+	struct ptp_management_msg res;
+	ssize_t ret;
+	uint16_t seq;
+	uint16_t mgmt_id;
+
+	if (!(mask & SPA_IO_IN)) {
+		return;
+	}
+
+	pw_log_info("PTP management socket has data (mask=%#x)", mask);
+
+	for (;;) {
+		ret = recv(fd, buf, sizeof(buf), 0);
+		if (ret == -1) {
+			if (errno == EAGAIN || errno == EWOULDBLOCK) {
+				return;
+			}
+			pw_log_warn("Failed to receive PTP management response: %m");
+			return;
+		}
+		pw_log_info("PTP management received %zd bytes", ret);
+		if (ret < (ssize_t)sizeof(struct ptp_management_msg)) {
+			pw_log_warn("Received undersized PTP management response: %zd bytes",
+					ret);
+			continue;
+		}
+
+		memcpy(&res, buf, sizeof(res));
+
+		if ((res.ver & 0x0f) != 2) {
+			pw_log_warn("PTP major version is %d, expected 2", res.ver);
+			continue;
+		}
+
+		if ((res.major_sdo_id_message_type & 0x0f) != PTP_MESSAGE_TYPE_MANAGEMENT) {
+			pw_log_warn("PTP management returned type %x, expected management",
+					res.major_sdo_id_message_type);
+			continue;
+		}
+
+		if (res.action != PTP_MGMT_ACTION_RESPONSE) {
+			pw_log_debug("PTP management returned action %d, expected response",
+					res.action);
+			continue;
+		}
+
+		seq = ntohs(res.sequence_id_be);
+		mgmt_id = ntohs(res.management_id_be);
+
+		if (!gptp->req_in_flight || seq != gptp->req_sequence_id) {
+			pw_log_debug("Ignoring unsolicited PTP response (seq=%u, id=%04x)",
+					seq, mgmt_id);
+			continue;
+		}
+
+		if (ntohs(res.tlv_type_be) != PTP_TLV_TYPE_MGMT) {
+			pw_log_warn("PTP management returned tlv type %d, expected management",
+					ntohs(res.tlv_type_be));
+			gptp->req_in_flight = false;
+			continue;
+		}
+
+		if (mgmt_id != gptp->req_management_id) {
+			pw_log_warn("PTP management returned ID %04x, expected %04x",
+					mgmt_id, gptp->req_management_id);
+			gptp->req_in_flight = false;
+			continue;
+		}
+
+		switch (mgmt_id) {
+		case PTP_MGMT_ID_PARENT_DATA_SET:
+			handle_parent_data_set(gptp, &res,
+					buf + sizeof(struct ptp_management_msg),
+					(size_t)ret - sizeof(struct ptp_management_msg));
+			break;
+		case PTP_MGMT_ID_DEFAULT_DATA_SET:
+			handle_default_data_set(gptp, &res,
+					buf + sizeof(struct ptp_management_msg),
+					(size_t)ret - sizeof(struct ptp_management_msg));
+			break;
+		case PTP_MGMT_ID_PORT_DATA_SET:
+			handle_port_data_set(gptp, &res,
+					buf + sizeof(struct ptp_management_msg),
+					(size_t)ret - sizeof(struct ptp_management_msg));
+			break;
+		default:
+			pw_log_debug("Unhandled PTP management ID: %04x", mgmt_id);
+			break;
+		}
+		gptp->req_in_flight = false;
+	}
+}
+
+static uint16_t next_management_id(uint32_t tick_count)
+{
+	switch (tick_count % 10) {
+	case 0:  return PTP_MGMT_ID_DEFAULT_DATA_SET;
+	case 5:  return PTP_MGMT_ID_PORT_DATA_SET;
+	default: return PTP_MGMT_ID_PARENT_DATA_SET;
+	}
 }
 
 static void gptp_periodic(void *data, uint64_t now)
 {
 	struct gptp *gptp = data;
-	update_ts_refclk(gptp);
+	int err;
+
+	if (!gptp->ptp_mgmt_socket_path) {
+		return;
+	}
+
+	if (gptp->ptp_fd < 0 && gptp_open_socket(gptp) < 0) {
+		return;
+	}
+
+	if (gptp->req_in_flight && (now - gptp->req_sent_ns) > PTP_REQUEST_TIMEOUT_NS) {
+		pw_log_debug("PTP management request seq=%u timed out",
+				gptp->req_sequence_id);
+		gptp->req_in_flight = false;
+	}
+
+	if (gptp->req_in_flight) {
+		return;
+	}
+
+	err = send_management_request(gptp, next_management_id(gptp->tick_count), now);
+	if (err == 0) {
+		gptp->tick_count++;
+	} else if (err == -ENOTCONN) {
+		pw_log_info("PTP management socket disconnected, will reopen");
+		gptp_close_socket(gptp);
+	}
 }
 
 static void gptp_destroy(void *data)
@@ -217,8 +488,7 @@ static void gptp_destroy(void *data)
 	struct gptp *gptp = data;
 	spa_hook_remove(&gptp->server_listener);
 
-	if (gptp->ptp_fd != -1)
+	gptp_close_socket(gptp);
-		close(gptp->ptp_fd);
 
 	free(gptp->ptp_mgmt_socket_path);
 	free(gptp);
@@ -235,11 +505,11 @@ struct avb_gptp *avb_gptp_new(struct server *server)
 	struct impl *impl;
 	struct gptp *gptp;
 	const char *str;
-	int ret;
 
 	gptp = calloc(1, sizeof(*gptp));
-	if (gptp == NULL)
+	if (gptp == NULL) {
 		return NULL;
+	}
 
 	gptp->server = server;
 	gptp->ptp_fd = -1;
@@ -250,20 +520,47 @@ struct avb_gptp *avb_gptp_new(struct server *server)
 	gptp->ptp_mgmt_socket_path = str ? strdup(str) : NULL;
 
 	if (gptp->ptp_mgmt_socket_path) {
-		ret = make_unix_ptp_mgmt_socket(gptp->ptp_mgmt_socket_path);
+		if (gptp_open_socket(gptp) < 0) {
-		if (ret == -1)
 			pw_log_warn("server %p: PTP management socket unavailable, "
 					"continuing without GM tracking; will retry on '%s'",
 					impl, gptp->ptp_mgmt_socket_path);
-		else
+		}
-			gptp->ptp_fd = ret;
 	} else {
 		pw_log_warn("server %p: ptp.management-socket not set, "
 				"continuing without GM tracking", impl);
 	}
 
-	avdecc_server_add_listener(server, &gptp->server_listener, &server_events, gptp);
+	avdecc_server_add_listener(server, &gptp->server_listener,
+			&server_events, gptp);
 
 	return (struct avb_gptp*)gptp;
 }
 
+bool avb_gptp_get_clock_id(const struct avb_gptp *agptp, uint64_t *clock_id_be)
+{
+	const struct gptp *gptp = (const struct gptp *)agptp;
+	if (gptp == NULL || !gptp->data_valid) {
+		return false;
+	}
+	memcpy(clock_id_be, gptp->clock_id, sizeof(*clock_id_be));
+	return true;
+}
+
+bool avb_gptp_get_grandmaster_id(const struct avb_gptp *agptp, uint64_t *gm_id_be)
+{
+	const struct gptp *gptp = (const struct gptp *)agptp;
+	if (gptp == NULL || !gptp->data_valid) {
+		return false;
+	}
+	memcpy(gm_id_be, gptp->gm_id, sizeof(*gm_id_be));
+	return true;
+}
+
+bool avb_gptp_is_grandmaster(const struct avb_gptp *agptp)
+{
+	const struct gptp *gptp = (const struct gptp *)agptp;
+	if (gptp == NULL || !gptp->data_valid) {
+		return false;
+	}
+	return memcmp(gptp->clock_id, gptp->gm_id, 8) == 0;
+}

src/modules/module-avb/gptp.h

@@ -5,6 +5,7 @@
 #ifndef AVB_GPTP_H
 #define AVB_GPTP_H
 
+#include <stdbool.h>
 #include <stdint.h>
 #include "internal.h"
 
@@ -18,7 +19,9 @@ extern "C" {
 #define PTP_MGMT_ACTION_GET 0
 #define PTP_MGMT_ACTION_RESPONSE 2
 #define PTP_TLV_TYPE_MGMT 0x0001
+#define PTP_MGMT_ID_DEFAULT_DATA_SET 0x2000
 #define PTP_MGMT_ID_PARENT_DATA_SET 0x2002
+#define PTP_MGMT_ID_PORT_DATA_SET 0x2004
 
 /**************************************************************************************/
 /* IEEE 1588-2019, Sec. 15.4.1 PTP management message format - Common Fields */
@@ -68,8 +71,42 @@ struct ptp_parent_data_set {
 	uint8_t  gm_clock_id[8];
 } __attribute__((packed));
 
+/* IEEE 1588-2008 Section 15.5.1.1 defaultDS */
+struct ptp_default_data_set {
+	uint8_t  flags;
+	uint8_t  reserved1;
+	uint16_t number_ports_be;
+	uint8_t  priority1;
+	uint8_t  clock_class;
+	uint8_t  clock_accuracy;
+	uint16_t offset_scaled_log_variance_be;
+	uint8_t  priority2;
+	uint8_t  clock_identity[8];
+	uint8_t  domain_number;
+	uint8_t  reserved2;
+} __attribute__((packed));
+
+/* IEEE 1588-2008 Section 15.5.1.4 portDS */
+struct ptp_port_data_set {
+	uint8_t  port_clock_identity[8];
+	uint16_t port_number_be;
+	uint8_t  port_state;
+	int8_t   log_min_delay_req_interval;
+	uint8_t  peer_mean_path_delay[8];
+	int8_t   log_announce_interval;
+	uint8_t  announce_receipt_timeout;
+	int8_t   log_sync_interval;
+	uint8_t  delay_mechanism;
+	int8_t   log_min_pdelay_req_interval;
+	uint8_t  version_number;
+} __attribute__((packed));
+
 struct avb_gptp *avb_gptp_new(struct server *server);
 
+bool avb_gptp_get_clock_id(const struct avb_gptp *gptp, uint64_t *clock_id_be);
+bool avb_gptp_get_grandmaster_id(const struct avb_gptp *gptp, uint64_t *gm_id_be);
+bool avb_gptp_is_grandmaster(const struct avb_gptp *gptp);
+
 #ifdef __cplusplus
 }
 #endif