From 7abb896aef03388427d3ae5c764ae94312438c66 Mon Sep 17 00:00:00 2001 From: Dmitry Sharshakov Date: Thu, 18 Jan 2024 12:50:41 +0300 Subject: [PATCH] module-rtp-sap: support PTP management protocol Request current clock sync status to know the grandmaster identity for SAP announcements --- src/daemon/pipewire-aes67.conf.in | 3 +- src/modules/module-rtp-sap.c | 182 +++++++++++++++++++++++++++++- src/modules/module-rtp/ptp.h | 61 ++++++++++ 3 files changed, 239 insertions(+), 7 deletions(-) create mode 100644 src/modules/module-rtp/ptp.h diff --git a/src/daemon/pipewire-aes67.conf.in b/src/daemon/pipewire-aes67.conf.in index bb61e6d80..8d2b9eb79 100644 --- a/src/daemon/pipewire-aes67.conf.in +++ b/src/daemon/pipewire-aes67.conf.in @@ -68,6 +68,7 @@ context.modules = [ sap.port = 9875 net.ttl = 32 net.loop = false + ptp.management-socket = "/var/run/ptp4lro" stream.rules = [ { @@ -116,7 +117,6 @@ context.modules = [ # please change this, especially for multiple streams sess.name = "PipeWire RTP stream" sess.media = "audio" - ### please select the PTP grandmaster ID here sess.ts-refclk = "ptp=traceable" sess.ts-offset = 0 sess.latency.msec = 3 @@ -135,6 +135,7 @@ context.modules = [ node.always-process = true node.group = pipewire.ptp0 rtp.ntp = 0 + rtp.fetch-ts-refclk = true } } }, diff --git a/src/modules/module-rtp-sap.c b/src/modules/module-rtp-sap.c index dbcadbbc0..a875911ec 100644 --- a/src/modules/module-rtp-sap.c +++ b/src/modules/module-rtp-sap.c @@ -3,6 +3,7 @@ /* SPDX-License-Identifier: MIT */ #include "config.h" +#include "pipewire/properties.h" #include #include @@ -24,6 +25,7 @@ #include #include +#include #ifdef __FreeBSD__ #define ifr_ifindex ifr_index @@ -164,6 +166,13 @@ static const struct spa_dict_item module_info[] = { { PW_KEY_MODULE_VERSION, PACKAGE_VERSION }, }; +#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_TLV_TYPE_MGMT 0x0001 +#define PTP_MGMT_ID_PARENT_DATA_SET 0x2002 + struct sdp_info { uint16_t hash; uint32_t ntp; @@ -198,6 +207,7 @@ struct session { bool announce; uint64_t timestamp; + bool ts_refclk_ptp; struct impl *impl; struct node *node; @@ -262,6 +272,13 @@ struct impl { char *extra_attrs_preamble; char *extra_attrs_end; + + char *ptp_mgmt_socket; + char ptp_client_path[64]; + int ptp_fd; + uint32_t ptp_seq; + uint8_t clock_id[8]; + uint8_t gm_id[8]; }; struct format_info { @@ -367,6 +384,37 @@ static bool is_multicast(struct sockaddr *sa, socklen_t salen) return false; } +static int make_unix_socket(char *path, char *client_path) { + struct sockaddr_un client_addr, server_addr; + int fd; + + fd = socket(AF_UNIX, SOCK_DGRAM, 0); + if (fd == -1) { + pw_log_warn("Failed to create PTP management socket"); + return 0; + } + + spa_zero(client_addr); + client_addr.sun_family = AF_UNIX; + strncpy(client_addr.sun_path, client_path, strlen(client_path)); + + if (bind(fd, (struct sockaddr *)&client_addr, sizeof(client_addr)) == -1) { + pw_log_warn("Failed to bind PTP management socket"); + return 0; + } + + spa_zero(server_addr); + server_addr.sun_family = AF_UNIX; + strncpy(server_addr.sun_path, path, sizeof(server_addr.sun_path) - 1); + + if (connect(fd, (struct sockaddr*)&server_addr, sizeof(server_addr)) == -1) { + pw_log_warn("Failed to connect PTP management socket"); + return 0; + } + + return fd; +} + static int make_send_socket( struct sockaddr_storage *src, socklen_t src_len, struct sockaddr_storage *sa, socklen_t salen, @@ -490,6 +538,96 @@ static int get_ip(const struct sockaddr_storage *sa, char *ip, size_t len, bool return 0; } +static void update_ts_refclk(struct impl *impl) +{ + if (!impl->ptp_mgmt_socket || !impl->ptp_fd) + return; + + // Read if something is left in the socket + int avail; + ioctl(impl->ptp_fd, FIONREAD, &avail); + if (avail) { + void *tmp = malloc(avail); + read(impl->ptp_fd, tmp, avail); + free(tmp); + } + + 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(impl->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(impl->ptp_fd, &req, sizeof(req)) == -1) { + pw_log_warn("Failed to send PTP management request: %m"); + return; + } + + uint8_t buf[sizeof(struct ptp_management_msg) + sizeof(struct ptp_parent_data_set)]; + if (read(impl->ptp_fd, &buf, sizeof(buf)) == -1) { + pw_log_warn("Failed to send PTP management request: %m"); + return; + } + + 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)); + + 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 32", data_len); + + uint8_t *cid = res.clock_identity; + if (memcmp(cid, impl->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; + if (memcmp(gmid, impl->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 */ + ); + + // 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(impl->clock_id, cid, 8); + memcpy(impl->gm_id, gmid, 8); +} + static int send_sap(struct impl *impl, struct session *sess, bool bye) { char buffer[2048], src_addr[64], dst_addr[64], dst_ttl[8]; @@ -596,12 +734,24 @@ static int send_sap(struct impl *impl, struct session *sess, bool bye) spa_strbuf_append(&buf, "a=framecount:%u\n", sdp->framecount); - if (sdp->ts_refclk != NULL) { - spa_strbuf_append(&buf, - "a=ts-refclk:%s\n" - "a=mediaclk:direct=%u\n", - sdp->ts_refclk, - sdp->ts_offset); + if (sdp->ts_refclk != NULL || sess->ts_refclk_ptp) { + // Only broadcast the GM ID when we are synced to external time source + if (sess->ts_refclk_ptp && memcmp(impl->clock_id, impl->gm_id, 8) != 0) { + spa_strbuf_append(&buf, + "a=ts-refclk:ptp=IEEE1588-2008:%02X-%02X-%02X-%02X-%02X-%02X-%02X-%02X:%d\n", + impl->gm_id[0], + impl->gm_id[1], + impl->gm_id[2], + impl->gm_id[3], + impl->gm_id[4], + impl->gm_id[5], + impl->gm_id[6], + impl->gm_id[7], + 0/* domain */); + } else { + spa_strbuf_append(&buf, "a=ts-refclk:%s\n", sdp->ts_refclk); + } + spa_strbuf_append(&buf, "a=mediaclk:direct=%u\n", sdp->ts_offset); } else { spa_strbuf_append(&buf, "a=mediaclk:sender\n"); } @@ -646,6 +796,7 @@ static void on_timer_event(void *data, uint64_t expirations) clock_gettime(CLOCK_MONOTONIC, &ts); timestamp = SPA_TIMESPEC_TO_NSEC(&ts); interval = impl->cleanup_interval * SPA_NSEC_PER_SEC; + update_ts_refclk(impl); spa_list_for_each_safe(sess, tmp, &impl->sessions, link) { if (sess->announce) { @@ -739,6 +890,7 @@ static struct session *session_new_announce(struct impl *impl, struct node *node sdp->ts_offset = atoi(str); if ((str = pw_properties_get(props, "rtp.ts-refclk")) != NULL) sdp->ts_refclk = strdup(str); + sess->ts_refclk_ptp = pw_properties_get_bool(props, "rtp.fetch-ts-refclk", false); if ((str = pw_properties_get(props, PW_KEY_NODE_CHANNELNAMES)) != NULL) { struct spa_strbuf buf; spa_strbuf_init(&buf, sdp->channelmap, sizeof(sdp->channelmap)); @@ -1464,12 +1616,16 @@ static void impl_destroy(struct impl *impl) if (impl->sap_fd != -1) close(impl->sap_fd); + if (impl->ptp_fd != -1) + close(impl->ptp_fd); pw_properties_free(impl->props); free(impl->extra_attrs_preamble); free(impl->extra_attrs_end); + free(impl->ptp_mgmt_socket); + unlink(impl->ptp_client_path); free(impl->ifname); free(impl); } @@ -1539,6 +1695,20 @@ int pipewire__module_init(struct pw_impl_module *module, const char *args) str = pw_properties_get(props, "local.ifname"); impl->ifname = str ? strdup(str) : NULL; + str = pw_properties_get(props, "ptp.management-socket"); + impl->ptp_mgmt_socket = str ? strdup(str) : NULL; + + if (impl->ptp_mgmt_socket) { + char *client_dir = getenv("PIPEWIRE_RUNTIME_DIR"); + if (!client_dir) client_dir = getenv("XDG_RUNTIME_DIR"); + if (!client_dir) client_dir = "/tmp"; + + snprintf(impl->ptp_client_path, 63, "%s/pipewire-ptp-mgmt.%d", client_dir, getpid()); + + // TODO: support UDP management access as well + impl->ptp_fd = make_unix_socket(impl->ptp_mgmt_socket, impl->ptp_client_path); + } + if ((str = pw_properties_get(props, "sap.ip")) == NULL) str = DEFAULT_SAP_IP; port = pw_properties_get_uint32(props, "sap.port", DEFAULT_SAP_PORT); diff --git a/src/modules/module-rtp/ptp.h b/src/modules/module-rtp/ptp.h new file mode 100644 index 000000000..64a5931a7 --- /dev/null +++ b/src/modules/module-rtp/ptp.h @@ -0,0 +1,61 @@ +/* PipeWire */ +/* SPDX-FileCopyrightText: Copyright © 2024 Dmitry Sharshakov */ +/* SPDX-License-Identifier: MIT */ + +#ifndef PIPEWIRE_PTP_H +#define PIPEWIRE_PTP_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +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)); + +#ifdef __cplusplus +} +#endif + +#endif /* PIPEWIRE_PTP_H */