From 54a9495715c5797016ad21568319f41a659e69a3 Mon Sep 17 00:00:00 2001 From: Nils Tonnaett Date: Tue, 13 Jan 2026 17:40:13 -0800 Subject: [PATCH] module-avb: check ptp management socket periodically --- src/daemon/pipewire-avb.conf.in | 1 + src/modules/meson.build | 1 + src/modules/module-avb/avdecc.c | 4 + src/modules/module-avb/gptp.c | 251 ++++++++++++++++++++++++++++++ src/modules/module-avb/gptp.h | 73 +++++++++ src/modules/module-avb/internal.h | 2 + 6 files changed, 332 insertions(+) create mode 100644 src/modules/module-avb/gptp.c create mode 100644 src/modules/module-avb/gptp.h diff --git a/src/daemon/pipewire-avb.conf.in b/src/daemon/pipewire-avb.conf.in index e43015780..1a9f48499 100644 --- a/src/daemon/pipewire-avb.conf.in +++ b/src/daemon/pipewire-avb.conf.in @@ -71,6 +71,7 @@ avb.properties = { #ifname = "eth0.2" ifname = "enp3s0" milan = false + ptp.management-socket = "/var/run/ptp4lro" } avb.properties.rules = [ diff --git a/src/modules/meson.build b/src/modules/meson.build index 242b9fad3..439a37693 100644 --- a/src/modules/meson.build +++ b/src/modules/meson.build @@ -821,6 +821,7 @@ if build_module_avb 'module-avb/es-builder.c', 'module-avb/avdecc.c', 'module-avb/descriptors.c', + 'module-avb/gptp.c', 'module-avb/maap.c', 'module-avb/mmrp.c', 'module-avb/mrp.c', diff --git a/src/modules/module-avb/avdecc.c b/src/modules/module-avb/avdecc.c index 737244ccb..dae9dbed7 100644 --- a/src/modules/module-avb/avdecc.c +++ b/src/modules/module-avb/avdecc.c @@ -21,6 +21,7 @@ #include "avb.h" #include "packets.h" #include "internal.h" +#include "gptp.h" #include "stream.h" #include "acmp.h" #include "adp.h" @@ -405,6 +406,9 @@ struct server *avdecc_server_new(struct impl *impl, struct spa_dict *props) if ((res = server->transport->setup(server)) < 0) goto error_free; + server->gptp = avb_gptp_new(server); + + init_descriptors(server); server->mrp = avb_mrp_new(server); if (server->mrp == NULL) diff --git a/src/modules/module-avb/gptp.c b/src/modules/module-avb/gptp.c new file mode 100644 index 000000000..0779f817b --- /dev/null +++ b/src/modules/module-avb/gptp.c @@ -0,0 +1,251 @@ +/* PipeWire */ +/* SPDX-FileCopyrightText: Copyright © 2024 Dmitry Sharshakov */ +/* SPDX-FileCopyrightText: Copyright © 2025 Nils Tonnaett */ +/* SPDX-License-Identifier: MIT */ + +#include "gptp.h" +#include "module-avb/aecp-aem.h" +#include "pipewire/properties.h" +#include "pipewire/protocol.h" + +#include /* Definition of constants */ +#include +#include +#include +#include +#include +#include +#include +#include + + +struct gptp { + struct server *server; + + struct pw_loop *loop; + struct pw_timer_queue *timer_queue; + + struct spa_hook server_listener; + + struct spa_hook_list listener_list; + + struct spa_list attributes; + + char *ptp_mgmt_socket_path; + int ptp_fd; + uint32_t ptp_seq; + uint8_t clock_id[8]; + uint8_t gm_id[8]; +}; + +static int make_unix_ptp_mgmt_socket(const char *path) { + struct sockaddr_un addr; + + spa_autoclose int fd = socket(AF_UNIX, SOCK_DGRAM | SOCK_CLOEXEC, 0); + if (fd < 0) { + pw_log_warn("Failed to create PTP management socket"); + return -1; + } + + int val = 1; + if (setsockopt(fd, SOL_SOCKET, SO_PASSCRED, &val, sizeof(val)) < 0) { + pw_log_warn("Failed to bind PTP management socket"); + return -1; + } + + 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"); + return -1; + } + + return spa_steal_fd(fd); +} + +static bool update_ts_refclk(struct gptp *gptp) { + 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 + int avail; + uint8_t tmp; + + ioctl(gptp->ptp_fd, FIONREAD, &avail); + pw_log_debug("Flushing stale data: %u bytes", avail); + while (avail-- && read(gptp->ptp_fd, &tmp, 1)); + + struct ptp_management_msg req; + spa_zero(req); + + req.major_sdo_id_message_type = PTP_MESSAGE_TYPE_MANAGEMENT; + req.ver = PTP_VERSION_1588_2008_2_1; + req.message_length_be = htobe16(sizeof(struct ptp_management_msg)); + spa_zero(req.clock_identity); + req.source_port_id_be = htobe16(getpid()); + req.log_message_interval = 127; + req.sequence_id_be = htobe16(gptp->ptp_seq++); + memset(req.target_port_identity, 0xff, 8); + req.target_port_id_be = htobe16(0xffff); + req.starting_boundary_hops = 1; + req.boundary_hops = 1; + req.action = PTP_MGMT_ACTION_GET; + req.tlv_type_be = htobe16(PTP_TLV_TYPE_MGMT); + // sent empty TLV, only sending management_id + req.management_message_length_be = htobe16(2); + req.management_id_be = htobe16(PTP_MGMT_ID_PARENT_DATA_SET); + + if (write(gptp->ptp_fd, &req, sizeof(req)) == -1) { + pw_log_warn("Failed to send PTP management request: %m"); + if (errno != ENOTCONN) + 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; + } + + uint8_t buf[sizeof(struct ptp_management_msg) + sizeof(struct ptp_parent_data_set)]; + if (read(gptp->ptp_fd, &buf, sizeof(buf)) == -1) { + pw_log_warn("Failed to receive PTP management response: %m"); + return false; + } + + struct ptp_management_msg res = *(struct ptp_management_msg *)buf; + struct ptp_parent_data_set parent = + *(struct ptp_parent_data_set *)(buf + sizeof(struct ptp_management_msg)); + + 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 (be16toh(res.tlv_type_be) != PTP_TLV_TYPE_MGMT) { + pw_log_warn("PTP management returned tlv type %d, expected management", be16toh(res.tlv_type_be)); + return false; + } + + if (be16toh(res.management_id_be) != PTP_MGMT_ID_PARENT_DATA_SET) { + pw_log_warn("PTP management returned ID %d, expected PARENT_DATA_SET", be16toh(res.management_id_be)); + return false; + } + + uint16_t data_len = be16toh(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)); + + uint8_t *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 */ + ); + + uint8_t *gmid = parent.gm_clock_id; + bool gmid_changed = false; + if (memcmp(gmid, gptp->gm_id, 8) != 0) { + pw_log_info( + "GM ID: IEEE1588-2008:%02X-%02X-%02X-%02X-%02X-%02X-%02X-%02X:%d", + gmid[0], + gmid[1], + gmid[2], + gmid[3], + gmid[4], + gmid[5], + gmid[6], + gmid[7], + 0 /* domain */ + ); + gmid_changed = true; + } + + // When GM is not equal to own clock we are clocked by external master + pw_log_debug("Synced to GM: %s", (memcmp(cid, gmid, 8) != 0) ? "true" : "false"); + + memcpy(gptp->clock_id, cid, 8); + memcpy(gptp->gm_id, gmid, 8); + return gmid_changed; +} + +static void gptp_periodic(void *data, uint64_t now) +{ + struct gptp *gptp = data; + update_ts_refclk(gptp); +} + +static void gptp_destroy(void *data) +{ + struct gptp *gptp = data; + spa_hook_remove(&gptp->server_listener); + free(gptp); +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = gptp_destroy, + .periodic = gptp_periodic, +}; + +void avb_gptp_destroy(struct avb_gptp *gptp) +{ + gptp_destroy(gptp); +} + +struct avb_gptp *avb_gptp_new(struct server *server) +{ + struct impl *impl; + struct gptp *gptp; + const char *str; + + gptp = calloc(1, sizeof(*gptp)); + if (gptp == NULL) + return NULL; + + gptp->server = server; + gptp->ptp_fd = -1; + + impl = server->impl; + + gptp->loop = pw_context_get_main_loop(impl->context); + gptp->timer_queue = pw_context_get_timer_queue(impl->context); + + str = pw_properties_get(impl->props, "ptp.management-socket"); + gptp->ptp_mgmt_socket_path = str ? strdup(str) : NULL; + + if(gptp->ptp_mgmt_socket_path) + gptp->ptp_fd = make_unix_ptp_mgmt_socket(gptp->ptp_mgmt_socket_path); + + spa_list_init(&gptp->attributes); + spa_hook_list_init(&gptp->listener_list); + + avdecc_server_add_listener(server, &gptp->server_listener, &server_events, gptp); + + return (struct avb_gptp*)gptp; +} + diff --git a/src/modules/module-avb/gptp.h b/src/modules/module-avb/gptp.h new file mode 100644 index 000000000..73cd9e7f0 --- /dev/null +++ b/src/modules/module-avb/gptp.h @@ -0,0 +1,73 @@ +/* PipeWire */ +/* SPDX-FileCopyrightText: Copyright © 2024 Dmitry Sharshakov */ +/* SPDX-License-Identifier: MIT */ + +#ifndef AVB_GPTP_H +#define AVB_GPTP_H + +#include +#include "internal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define PTP_MESSAGE_TYPE_MANAGEMENT 0x0d +#define PTP_VERSION_1588_2008_2_1 0x12 +#define PTP_DEFAULT_LOG_MESSAGE_INTERVAL 127 +#define PTP_MGMT_ACTION_GET 0 +#define PTP_MGMT_ACTION_RESPONSE 2 +#define PTP_TLV_TYPE_MGMT 0x0001 +#define PTP_MGMT_ID_PARENT_DATA_SET 0x2002 + + +struct ptp_management_msg { + // 4 for major_sdo, 4 for msg_type + uint8_t major_sdo_id_message_type; + // 4 for minor, 4 for major + uint8_t ver; + uint16_t message_length_be; + uint8_t domain_number; + uint8_t minor_sdo_id; + uint16_t flags_be; + uint8_t correction_field[8]; + uint32_t message_type_specific; + uint8_t clock_identity[8]; + uint16_t source_port_id_be; + uint16_t sequence_id_be; + uint8_t control_field; + uint8_t log_message_interval; + uint8_t target_port_identity[8]; + uint16_t target_port_id_be; + uint8_t starting_boundary_hops; + uint8_t boundary_hops; + uint8_t action; + uint8_t reserved; + uint16_t tlv_type_be; + // length of data after this + 2 for management_id + uint16_t management_message_length_be; + uint16_t management_id_be; +} __attribute__((packed)); + +struct ptp_parent_data_set { + uint8_t parent_clock_id[8]; + uint16_t parent_port_id_be; + uint8_t parent_stats; + uint8_t reserved; + uint16_t log_variance_be; + int32_t phase_change_rate_be; + uint8_t gm_prio1; + uint8_t gm_clock_class; + uint8_t gm_clock_accuracy; + uint16_t gm_clock_variance_be; + uint8_t gm_prio2; + uint8_t gm_clock_id[8]; +} __attribute__((packed)); + +struct avb_gptp *avb_gptp_new(struct server *server); + +#ifdef __cplusplus +} +#endif + +#endif /* AVB_GPTP_H */ diff --git a/src/modules/module-avb/internal.h b/src/modules/module-avb/internal.h index e7698fa52..a9a38d0c6 100644 --- a/src/modules/module-avb/internal.h +++ b/src/modules/module-avb/internal.h @@ -14,6 +14,7 @@ extern "C" { #endif struct server; +struct avb_gptp; struct avb_mrp; #define AVB_TSN_ETH 0x22f0 @@ -114,6 +115,7 @@ struct server { unsigned debug_messages:1; + struct avb_gptp *gptp; struct avb_mrp *mrp; struct avb_mmrp *mmrp; struct avb_mvrp *mvrp;