From 55bb0b6a6a0810828182a48f21b35dba9146c55a Mon Sep 17 00:00:00 2001 From: hackerman-kl Date: Wed, 29 Apr 2026 07:46:36 +0200 Subject: [PATCH] milan-avb: gptp: query CURRENT_DATA_SET for canonical is_grandmaster check --- src/modules/module-avb/gptp.c | 61 ++++++++++++++++++++++++++++++++--- src/modules/module-avb/gptp.h | 8 +++++ 2 files changed, 64 insertions(+), 5 deletions(-) diff --git a/src/modules/module-avb/gptp.c b/src/modules/module-avb/gptp.c index 56b9a8a2b..2aa4ac7d9 100644 --- a/src/modules/module-avb/gptp.c +++ b/src/modules/module-avb/gptp.c @@ -66,6 +66,10 @@ struct gptp { uint32_t tick_count; bool data_valid; + + uint16_t steps_removed; + int64_t offset_from_master_scaled_ns; + bool data_valid_current; }; static int make_bind_path(char *out, size_t out_size, uint64_t entity_id) @@ -368,6 +372,41 @@ static void handle_port_data_set(struct gptp *gptp, update_avb_interface_port(gptp, pds); } +static void handle_current_data_set(struct gptp *gptp, + const struct ptp_management_msg *res, + const uint8_t *payload, size_t payload_len) +{ + const struct ptp_current_data_set *cds; + uint16_t data_len; + uint16_t steps_removed; + int64_t offset_from_master; + + data_len = ntohs(res->management_message_length_be) - 2; + if (data_len != sizeof(struct ptp_current_data_set) || + payload_len < sizeof(struct ptp_current_data_set)) { + pw_log_warn("Unexpected PTP GET CURRENT_DATA_SET response length: " + "tlv=%u payload=%zu expected=%zu", + data_len, payload_len, + sizeof(struct ptp_current_data_set)); + return; + } + + cds = (const struct ptp_current_data_set *)payload; + steps_removed = ntohs(cds->steps_removed_be); + offset_from_master = (int64_t)be64toh((uint64_t)cds->offset_from_master_be); + + if (!gptp->data_valid_current || + gptp->steps_removed != steps_removed) { + pw_log_info("PTP currentDS: steps_removed=%u offset_from_master=%" + PRId64 " (scaled ns)", + steps_removed, offset_from_master); + } + + gptp->steps_removed = steps_removed; + gptp->offset_from_master_scaled_ns = offset_from_master; + gptp->data_valid_current = true; +} + static void on_ptp_mgmt_data(void *data, int fd, uint32_t mask) { struct gptp *gptp = data; @@ -457,6 +496,11 @@ static void on_ptp_mgmt_data(void *data, int fd, uint32_t mask) buf + sizeof(struct ptp_management_msg), (size_t)ret - sizeof(struct ptp_management_msg)); break; + case PTP_MGMT_ID_CURRENT_DATA_SET: + handle_current_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; @@ -467,10 +511,11 @@ static void on_ptp_mgmt_data(void *data, int fd, uint32_t mask) 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; + switch (tick_count % 4) { + case 0: return PTP_MGMT_ID_PARENT_DATA_SET; + case 1: return PTP_MGMT_ID_CURRENT_DATA_SET; + case 2: return PTP_MGMT_ID_DEFAULT_DATA_SET; + default: return PTP_MGMT_ID_PORT_DATA_SET; } } @@ -587,7 +632,13 @@ bool avb_gptp_get_grandmaster_id(const struct avb_gptp *agptp, uint64_t *gm_id_b bool avb_gptp_is_grandmaster(const struct avb_gptp *agptp) { const struct gptp *gptp = (const struct gptp *)agptp; - if (gptp == NULL || !gptp->data_valid) { + if (gptp == NULL) { + return false; + } + if (gptp->data_valid_current) { + return gptp->steps_removed == 0; + } + if (!gptp->data_valid) { return false; } return memcmp(gptp->clock_id, gptp->gm_id, 8) == 0; diff --git a/src/modules/module-avb/gptp.h b/src/modules/module-avb/gptp.h index 2e66f6b4f..5c65f9000 100644 --- a/src/modules/module-avb/gptp.h +++ b/src/modules/module-avb/gptp.h @@ -25,6 +25,7 @@ extern "C" { #define PTP_MGMT_ACTION_RESPONSE 2 #define PTP_TLV_TYPE_MGMT 0x0001 #define PTP_MGMT_ID_DEFAULT_DATA_SET 0x2000 +#define PTP_MGMT_ID_CURRENT_DATA_SET 0x2001 #define PTP_MGMT_ID_PARENT_DATA_SET 0x2002 #define PTP_MGMT_ID_PORT_DATA_SET 0x2004 @@ -91,6 +92,13 @@ struct ptp_default_data_set { uint8_t reserved2; } __attribute__((packed)); +/* IEEE 1588-2008 Section 15.5.1.2 currentDS */ +struct ptp_current_data_set { + uint16_t steps_removed_be; + int64_t offset_from_master_be; + int64_t mean_path_delay_be; +} __attribute__((packed)); + /* IEEE 1588-2008 Section 15.5.1.4 portDS */ struct ptp_port_data_set { uint8_t port_clock_identity[8];