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/adp.c b/src/modules/module-avb/adp.c index 6cc0cfbad..1126ec7b4 100644 --- a/src/modules/module-avb/adp.c +++ b/src/modules/module-avb/adp.c @@ -9,6 +9,7 @@ #include "adp.h" #include "acmp.h" #include "aecp-aem-descriptors.h" +#include "gptp.h" #include "internal.h" #include "utils.h" @@ -46,11 +47,32 @@ static void entity_free(struct entity *e) free(e); } +static void refresh_gptp_fields(struct server *server, struct avb_packet_adp *p) +{ + const struct descriptor *d; + struct avb_aem_desc_avb_interface *avb_interface; + uint64_t gm_id_be; + + d = server_find_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, 0); + avb_interface = d ? descriptor_body(d) : NULL; + if (avb_interface == NULL) { + return; + } + + if (avb_gptp_get_grandmaster_id(server->gptp, &gm_id_be)) { + p->gptp_grandmaster_id = gm_id_be; + } else { + p->gptp_grandmaster_id = avb_interface->clock_identity; + } + p->gptp_domain_number = avb_interface->domain_number; +} + static int send_departing(struct adp *adp, uint64_t now, struct entity *e) { struct avb_ethernet_header *h = (void*)e->buf; struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void); + refresh_gptp_fields(adp->server, p); AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_DEPARTING); p->available_index = htonl(adp->available_index++); avb_server_send_packet(adp->server, mac, AVB_TSN_ETH, e->buf, e->len); @@ -63,6 +85,7 @@ static int send_advertise(struct adp *adp, uint64_t now, struct entity *e) struct avb_ethernet_header *h = (void*)e->buf; struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void); + refresh_gptp_fields(adp->server, p); AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE); p->available_index = htonl(adp->available_index++); avb_server_send_packet(adp->server, mac, AVB_TSN_ETH, e->buf, e->len); @@ -269,7 +292,6 @@ static int check_advertise(struct adp *adp, uint64_t now) struct server *server = adp->server; const struct descriptor *d; struct avb_aem_desc_entity *entity; - struct avb_aem_desc_avb_interface *avb_interface; struct entity *e; uint64_t entity_id; struct avb_ethernet_header *h; @@ -289,9 +311,6 @@ static int check_advertise(struct adp *adp, uint64_t now) return 0; } - d = server_find_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, 0); - avb_interface = d ? descriptor_body(d) : NULL; - pw_log_info("entity %s advertise", avb_utils_format_id(buf, sizeof(buf), entity_id)); @@ -321,10 +340,6 @@ static int check_advertise(struct adp *adp, uint64_t now) p->listener_capabilities = entity->listener_capabilities; p->controller_capabilities = entity->controller_capabilities; p->available_index = entity->available_index; - if (avb_interface) { - p->gptp_grandmaster_id = avb_interface->clock_identity; - p->gptp_domain_number = avb_interface->domain_number; - } p->identify_control_index = 0; p->interface_index = 0; p->association_id = entity->association_id; diff --git a/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.c b/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.c index 0bf21ea63..6b946b467 100644 --- a/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.c +++ b/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.c @@ -5,40 +5,92 @@ #include "../aecp.h" #include "../aecp-aem.h" +#include "../aecp-aem-descriptors.h" +#include "../aecp-aem-state.h" +#include "../gptp.h" #include "../internal.h" #include "cmd-get-as-path.h" #include "cmd-resp-helpers.h" +#include "reply-unsol-helpers.h" -/* IEEE 1722.1-2021 Section 7.4.41 GET_AS_PATH placeholder: single-entry path - * pointing at the entity itself, until gPTP is wired up. */ +/* IEEE 1722.1-2021 Section 7.4.41.2 path_sequence */ +static uint16_t fill_get_as_path_body(struct server *server, + struct avb_packet_aecp_aem_get_as_path *body) +{ + uint64_t *path = (uint64_t *)(body + 1); + uint64_t clock_id_be = 0; + uint64_t gm_id_be = 0; + bool have_clock = avb_gptp_get_clock_id(server->gptp, &clock_id_be); + bool have_gm = avb_gptp_get_grandmaster_id(server->gptp, &gm_id_be); + bool is_gm = avb_gptp_is_grandmaster(server->gptp); + uint16_t count = 0; + + if (!have_clock) { + body->reserved = htons(0); + return 0; + } + + if (is_gm) { + path[0] = clock_id_be; + count = 1; + } else { + count = avb_gptp_get_path_trace(server->gptp, path, + PTP_AS_PATH_MAX_ENTRIES - 1); + + if (count == 0 && have_gm) { + path[0] = gm_id_be; + count = 1; + } + + if (count < PTP_AS_PATH_MAX_ENTRIES) { + path[count++] = clock_id_be; + } + } + + body->reserved = htons(count); + return count; +} + +/* IEEE 1722.1-2021 Section 7.4.41 GET_AS_PATH. */ int handle_cmd_get_as_path_milan_v12(struct aecp *aecp, int64_t now, const void *m, int len) { + struct server *server = aecp->server; + int total; + uint16_t count; uint8_t buf[2048]; struct avb_ethernet_header *h_reply; struct avb_packet_aecp_aem *p_reply; struct avb_packet_aecp_aem_get_as_path *body; - uint64_t *path; - int total = (int)(sizeof(struct avb_ethernet_header) + - sizeof(struct avb_packet_aecp_aem) + - sizeof(struct avb_packet_aecp_aem_get_as_path) + - sizeof(uint64_t)); (void)now; - if (total > (int)sizeof(buf)) - return reply_no_resources(aecp, m, len); if (len < (int)(sizeof(*h_reply) + sizeof(*p_reply) + - sizeof(*body))) + sizeof(*body))) { return reply_bad_arguments(aecp, m, len); + } memcpy(buf, m, len); - if (len < total) - memset(buf + len, 0, total - len); h_reply = (struct avb_ethernet_header *)buf; p_reply = SPA_PTROFF(h_reply, sizeof(*h_reply), void); + body = (struct avb_packet_aecp_aem_get_as_path *)p_reply->payload; + + memset(body, 0, sizeof(*body)); + count = fill_get_as_path_body(server, body); + + total = (int)(sizeof(struct avb_ethernet_header) + + sizeof(struct avb_packet_aecp_aem) + + sizeof(struct avb_packet_aecp_aem_get_as_path) + + count * sizeof(uint64_t)); + + if (total > (int)sizeof(buf)) { + return reply_no_resources(aecp, m, len); + } + if (len < total) { + memset(buf + len, 0, total - len); + } /* IEEE 1722.1-2021 Section 9.2.1.1.7: CDL excludes the 12-octet AVTPDU common. */ AVB_PACKET_SET_LENGTH(&p_reply->aecp.hdr, @@ -46,11 +98,47 @@ int handle_cmd_get_as_path_milan_v12(struct aecp *aecp, int64_t now, sizeof(struct avb_packet_header) - sizeof(uint64_t))); - body = (struct avb_packet_aecp_aem_get_as_path *)p_reply->payload; - body->reserved = htons(1); - - path = (uint64_t *)(body + 1); - *path = htobe64(aecp->server->entity_id); - return reply_success(aecp, buf, total); } + +void cmd_get_as_path_emit_unsol_milan_v12(struct aecp *aecp, uint16_t desc_index) +{ + struct server *server = aecp->server; + struct descriptor *desc; + uint8_t buf[256]; + struct avb_ethernet_header *h; + struct avb_packet_aecp_aem *p; + struct avb_packet_aecp_aem_get_as_path *body; + struct aecp_aem_base_info b_state = { 0 }; + uint16_t count; + int total; + + desc = server_find_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, desc_index); + if (desc == NULL) { + return; + } + + memset(buf, 0, sizeof(buf)); + h = (struct avb_ethernet_header *)buf; + p = SPA_PTROFF(h, sizeof(*h), void); + body = (struct avb_packet_aecp_aem_get_as_path *)p->payload; + + AVB_PACKET_AECP_SET_MESSAGE_TYPE(&p->aecp, AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(&p->aecp, AVB_AECP_AEM_STATUS_SUCCESS); + p->cmd1 = 0; + p->cmd2 = AVB_AECP_AEM_CMD_GET_AS_PATH; + + body->descriptor_index = htons(desc_index); + count = fill_get_as_path_body(server, body); + + total = (int)(sizeof(struct avb_ethernet_header) + + sizeof(struct avb_packet_aecp_aem) + + sizeof(*body) + count * sizeof(uint64_t)); + + if (total > (int)sizeof(buf)) { + return; + } + + (void)reply_unsolicited_notifications(aecp, &b_state, buf, + total, true); +} diff --git a/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.h b/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.h index fa25c89aa..d23543976 100644 --- a/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.h +++ b/src/modules/module-avb/aecp-aem-cmds-resps/cmd-get-as-path.h @@ -9,4 +9,6 @@ int handle_cmd_get_as_path_milan_v12(struct aecp *aecp, int64_t now, const void *m, int len); +void cmd_get_as_path_emit_unsol_milan_v12(struct aecp *aecp, uint16_t desc_index); + #endif /* __AVB_AECP_AEM_CMD_GET_AS_PATH_H__ */ diff --git a/src/modules/module-avb/aecp-aem-state.h b/src/modules/module-avb/aecp-aem-state.h index 21c5743c8..df3f65c4c 100644 --- a/src/modules/module-avb/aecp-aem-state.h +++ b/src/modules/module-avb/aecp-aem-state.h @@ -104,6 +104,9 @@ struct aecp_aem_avb_interface_state { * has elapsed, then clears dirty and updates last_counters_emit_ns. */ bool counters_dirty; int64_t last_counters_emit_ns; + + bool gptp_info_dirty; + bool as_path_dirty; }; diff --git a/src/modules/module-avb/aecp-aem.c b/src/modules/module-avb/aecp-aem.c index d3b204c76..f3fd2a531 100644 --- a/src/modules/module-avb/aecp-aem.c +++ b/src/modules/module-avb/aecp-aem.c @@ -7,6 +7,8 @@ #include "aecp-aem-descriptors.h" #include "aecp-aem-state.h" #include "aecp-aem-cmds-resps/cmd-resp-helpers.h" +#include "aecp-aem-cmds-resps/reply-unsol-helpers.h" +#include "gptp.h" #include "utils.h" /* The headers including the command and response of the system */ @@ -130,6 +132,25 @@ static int handle_read_descriptor_common(struct aecp *aecp, int64_t now, const v return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, size); } +static void fill_get_avb_info_body(struct server *server, + struct avb_aem_desc_avb_interface *avb_interface, + struct avb_packet_aecp_aem_get_avb_info *i) +{ + uint64_t gm_id_be; + uint8_t flags = AVB_AEM_AVB_INFO_FLAG_SRP_ENABLED; + + if (avb_gptp_get_grandmaster_id(server->gptp, &gm_id_be)) { + i->gptp_grandmaster_id = gm_id_be; + flags |= AVB_AEM_AVB_INFO_FLAG_GPTP_ENABLED; + } else { + i->gptp_grandmaster_id = avb_interface->clock_identity; + } + i->flags = flags; + i->propagation_delay = htonl(0); + i->gptp_domain_number = avb_interface->domain_number; + i->msrp_mappings_count = htons(0); +} + /* GET_AVB_INFO */ static int handle_get_avb_info_common(struct aecp *aecp, int64_t now, const void *m, int len) @@ -174,18 +195,49 @@ static int handle_get_avb_info_common(struct aecp *aecp, int64_t now, AVB_PACKET_SET_LENGTH(&reply->aecp.hdr, psize + 12); i = (struct avb_packet_aecp_aem_get_avb_info*)reply->payload; - i->gptp_grandmaster_id = avb_interface->clock_identity; - i->propagation_delay = htonl(0); - i->gptp_domain_number = avb_interface->domain_number; - /* IEEE 1722.1-2021 Section 7.4.40: GPTP_ENABLED / GPTP_GRANDMASTER_SUPPORTED - * stay 0 until the gPTP interface lands. SRP_ENABLED is on because - * MSRP is running on every AVB interface this module manages. */ - i->flags = AVB_AEM_AVB_INFO_FLAG_SRP_ENABLED; - i->msrp_mappings_count = htons(0); + fill_get_avb_info_body(server, avb_interface, i); return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, size); } +#define GET_AVB_INFO_PACKET_LEN \ + (int)(sizeof(struct avb_ethernet_header) + \ + sizeof(struct avb_packet_aecp_aem) + \ + sizeof(struct avb_packet_aecp_aem_get_avb_info)) + +static void emit_unsol_get_avb_info(struct aecp *aecp, uint16_t desc_index) +{ + struct server *server = aecp->server; + uint8_t buf[GET_AVB_INFO_PACKET_LEN]; + struct avb_ethernet_header *h; + struct avb_packet_aecp_aem *p; + struct avb_packet_aecp_aem_get_avb_info *body; + struct descriptor *desc; + struct aecp_aem_base_info b_state = { 0 }; + + desc = server_find_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, desc_index); + if (desc == NULL) { + return; + } + + memset(buf, 0, sizeof(buf)); + h = (struct avb_ethernet_header *)buf; + p = SPA_PTROFF(h, sizeof(*h), void); + body = (struct avb_packet_aecp_aem_get_avb_info *)p->payload; + + AVB_PACKET_AECP_SET_MESSAGE_TYPE(&p->aecp, AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(&p->aecp, AVB_AECP_AEM_STATUS_SUCCESS); + p->cmd1 = 0; + p->cmd2 = AVB_AECP_AEM_CMD_GET_AVB_INFO; + + body->descriptor_type = htons(AVB_AEM_DESC_AVB_INTERFACE); + body->descriptor_id = htons(desc_index); + fill_get_avb_info_body(server, descriptor_body(desc), body); + + (void)reply_unsolicited_notifications(aecp, &b_state, buf, + GET_AVB_INFO_PACKET_LEN, true); +} + /* AEM_COMMAND */ /* TODO in the case the AVB mode allows you to modifiy a Milan readonly descriptor, then create a array of is_readonly depending on the mode used */ @@ -579,6 +631,22 @@ void avb_aecp_aem_periodic(struct aecp *aecp, int64_t now) so->stream_info_dirty = false; } } + for (i = 0; i < UINT16_MAX; i++) { + struct descriptor *d = server_find_descriptor(srv, + AVB_AEM_DESC_AVB_INTERFACE, i); + struct aecp_aem_avb_interface_state *ifs; + if (d == NULL) + break; + ifs = d->ptr; + if (ifs->gptp_info_dirty) { + emit_unsol_get_avb_info(aecp, i); + ifs->gptp_info_dirty = false; + } + if (ifs->as_path_dirty) { + cmd_get_as_path_emit_unsol_milan_v12(aecp, i); + ifs->as_path_dirty = false; + } + } } } diff --git a/src/modules/module-avb/avdecc.c b/src/modules/module-avb/avdecc.c index 737244ccb..f6983c125 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); + if (server->gptp == NULL) + goto error_free; server->mrp = avb_mrp_new(server); if (server->mrp == NULL) diff --git a/src/modules/module-avb/descriptors.c b/src/modules/module-avb/descriptors.c index f983cb191..b496d713d 100644 --- a/src/modules/module-avb/descriptors.c +++ b/src/modules/module-avb/descriptors.c @@ -748,7 +748,7 @@ static void init_descriptor_milan_v12(struct server *server) struct avb_aem_desc_avb_interface avb_interface = { .localized_description = htons(DSC_AVB_INTERFACE_LOCALIZED_DESCRIPTION), .interface_flags = htons(DSC_AVB_INTERFACE_INTERFACE_FLAGS), - .clock_identity = htobe64(DSC_AVB_INTERFACE_CLOCK_IDENTITY), + .clock_identity = htobe64(server->entity_id), .priority1 = DSC_AVB_INTERFACE_PRIORITY1, .clock_class = DSC_AVB_INTERFACE_CLOCK_CLASS, .offset_scaled_log_variance = htons(DSC_AVB_INTERFACE_OFFSET_SCALED_LOG_VARIANCE), diff --git a/src/modules/module-avb/entity-model.h b/src/modules/module-avb/entity-model.h new file mode 100644 index 000000000..b3ae52286 --- /dev/null +++ b/src/modules/module-avb/entity-model.h @@ -0,0 +1,32 @@ +/* PipeWire */ +/* SPDX-FileCopyrightText: Copyright © 2025 Kebag-Logic */ +/* SPDX-FileCopyrightText: Copyright © 2025 Simon Gapp */ +/* SPDX-License-Identifier: MIT */ + +#ifndef _ENTITY_MODEL_MILAN_H_ +#define _ENTITY_MODEL_MILAN_H_ + +#include "aecp-aem-descriptors.h" + +/**************************************************************************************/ +/* IEEE 1722.1-2021, Sec. 7.2.8 AVB Interface Descriptor */ +/* Milan v1.2, Sec. 5.3.3.5 */ + +#define DSC_AVB_INTERFACE_LOCALIZED_DESCRIPTION AVB_AEM_DESC_INVALID +#define DSC_AVB_INTERFACE_INTERFACE_FLAGS (AVB_AEM_DESC_AVB_INTERFACE_FLAG_GPTP_GRANDMASTER_SUPPORTED | \ + AVB_AEM_DESC_AVB_INTERFACE_FLAG_GPTP_SUPPORTED | \ + AVB_AEM_DESC_AVB_INTERFACE_FLAG_SRP_SUPPORTED) +// TODO: This is a dynamic parameter +#define DSC_AVB_INTERFACE_CLOCK_IDENTITY 0x3cc0c6FFFE0002CB +#define DSC_AVB_INTERFACE_PRIORITY1 0xF8 +#define DSC_AVB_INTERFACE_CLOCK_CLASS 0xF8 +#define DSC_AVB_INTERFACE_OFFSET_SCALED_LOG_VARIANCE 0x436A +#define DSC_AVB_INTERFACE_CLOCK_ACCURACY 0x21 +#define DSC_AVB_INTERFACE_PRIORITY2 0xF8 +#define DSC_AVB_INTERFACE_DOMAIN_NUMBER 0 +#define DSC_AVB_INTERFACE_LOG_SYNC_INTERVAL 0 +#define DSC_AVB_INTERFACE_LOG_ANNOUNCE_INTERVAL 0 +#define DSC_AVB_INTERFACE_PDELAY_INTERVAL 0 +#define DSC_AVB_INTERFACE_PORT_NUMBER 0 + +#endif diff --git a/src/modules/module-avb/gptp.c b/src/modules/module-avb/gptp.c new file mode 100644 index 000000000..8f78d3cc1 --- /dev/null +++ b/src/modules/module-avb/gptp.c @@ -0,0 +1,815 @@ +/* PipeWire */ +/* SPDX-FileCopyrightText: Copyright © 2024 Dmitry Sharshakov */ +/* SPDX-FileCopyrightText: Copyright © 2025 Nils Tonnaett */ +/* SPDX-FileCopyrightText: Copyright © 2026 Alexandre Malki */ +/* SPDX-License-Identifier: MIT */ + +/* + * Companion ptp4l invocation (gPTP profile, UDS management enabled, + * Announce path trace recorded): + * + * ptp4l -i -f /etc/linuxptp/gPTP.cfg \ + * --uds_address=/var/run/ptp4l \ + * --path_trace_enabled=1 \ + * -m + * + * pipewire-avb.conf must point ptp.management-socket at the same path: + * + * ptp.management-socket = "/var/run/ptp4l" + * + * Equivalent pmc(8) probe to confirm ptp4l accepts our management + * messages (-t 1 forces transportSpecific = 1 / TS_IEEE_8021AS, + * matching PTP_GPTP_MANAGEMENT_TYPE in this file): + * + * pmc -u -b 0 -t 1 -s /var/run/ptp4l "GET PARENT_DATA_SET" + * pmc -u -b 0 -t 1 -s /var/run/ptp4l "GET PATH_TRACE_LIST" + */ + +#include "gptp.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "aecp-aem-descriptors.h" +#include "aecp-aem-state.h" +#include "entity-model-milan-v12.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_INTERVAL_NS (375 * SPA_NSEC_PER_MSEC) +#define PTP_REQUEST_TIMEOUT_NS PTP_REQUEST_INTERVAL_NS +#define PTP_LOST_TIMEOUT_THRESHOLD 8 + +struct gptp { + struct server *server; + + struct spa_hook server_listener; + struct spa_source *source; + + char *ptp_mgmt_socket_path; + int ptp_fd; + 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; + + uint32_t consecutive_timeouts; + + uint16_t steps_removed; + int64_t offset_from_master_scaled_ns; + bool data_valid_current; + + uint64_t path_trace[PTP_AS_PATH_MAX_ENTRIES]; + uint16_t path_trace_count; + bool path_trace_valid; +}; + +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; + + 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; + } + + 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 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 mark_gptp_info_dirty(struct gptp *gptp) +{ + struct descriptor *d = server_find_descriptor(gptp->server, + AVB_AEM_DESC_AVB_INTERFACE, 0); + struct aecp_aem_avb_interface_state *ifs; + + if (d == NULL) { + return; + } + ifs = d->ptr; + ifs->gptp_info_dirty = true; +} + +static void mark_as_path_dirty(struct gptp *gptp) +{ + struct descriptor *d = server_find_descriptor(gptp->server, + AVB_AEM_DESC_AVB_INTERFACE, 0); + struct aecp_aem_avb_interface_state *ifs; + + if (d == NULL) { + return; + } + ifs = d->ptr; + ifs->as_path_dirty = true; +} + +static void gptp_invalidate_state(struct gptp *gptp) +{ + bool had_data = gptp->data_valid || gptp->data_valid_current || + gptp->path_trace_valid; + struct avb_aem_desc_avb_interface *iface; + + gptp->data_valid = false; + gptp->data_valid_current = false; + gptp->path_trace_valid = false; + gptp->path_trace_count = 0; + gptp->steps_removed = 0; + gptp->offset_from_master_scaled_ns = 0; + memset(gptp->clock_id, 0, sizeof(gptp->clock_id)); + memset(gptp->gm_id, 0, sizeof(gptp->gm_id)); + memset(gptp->path_trace, 0, sizeof(gptp->path_trace)); + + iface = get_avb_interface(gptp); + if (iface != NULL) { + iface->clock_identity = htobe64(gptp->server->entity_id); + iface->priority1 = DSC_AVB_INTERFACE_PRIORITY1; + iface->clock_class = DSC_AVB_INTERFACE_CLOCK_CLASS; + iface->clock_accuracy = DSC_AVB_INTERFACE_CLOCK_ACCURACY; + iface->offset_scaled_log_variance = + htons(DSC_AVB_INTERFACE_OFFSET_SCALED_LOG_VARIANCE); + iface->priority2 = DSC_AVB_INTERFACE_PRIORITY2; + iface->domain_number = DSC_AVB_INTERFACE_DOMAIN_NUMBER; + iface->log_sync_interval = DSC_AVB_INTERFACE_LOG_SYNC_INTERVAL; + iface->log_announce_interval = DSC_AVB_INTERFACE_LOG_ANNOUNCE_INTERVAL; + iface->log_pdelay_interval = DSC_AVB_INTERFACE_PDELAY_INTERVAL; + iface->port_number = DSC_AVB_INTERFACE_PORT_NUMBER; + } + + if (had_data) { + pw_log_info("PTP management lost contact with ptp4l, " + "clearing cached gPTP state"); + mark_gptp_info_dirty(gptp); + mark_as_path_dirty(gptp); + } +} + +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; + ssize_t ret; + uint16_t seq; + + spa_zero(req); + + seq = gptp->ptp_seq++; + req.major_sdo_id_message_type = PTP_GPTP_MANAGEMENT_TYPE; + 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(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); + req.management_message_length_be = htons(2); + 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"); + return -errno; + } + if (ret != (ssize_t)sizeof(req)) { + pw_log_warn("Incomplete PTP management request: %zd of %zu bytes", + ret, sizeof(req)); + return -EIO; + } + + gptp->req_in_flight = true; + gptp->req_sequence_id = seq; + gptp->req_management_id = management_id; + 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; + } + + parent = (const struct ptp_parent_data_set *)payload; + + 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; + 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; + } + + 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); + gptp->data_valid = true; + + if (cid_changed) { + update_avb_interface_clock_identity(gptp, cid); + } + + if (cid_changed || gmid_changed) { + mark_gptp_info_dirty(gptp); + } + + 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); + mark_gptp_info_dirty(gptp); +} + +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 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); + mark_gptp_info_dirty(gptp); + } + + gptp->steps_removed = steps_removed; + gptp->offset_from_master_scaled_ns = offset_from_master; + gptp->data_valid_current = true; +} + +/* IEEE 1588-2008 Section 16.2.1 PATH_TRACE_LIST */ +static void handle_path_trace_list(struct gptp *gptp, + const struct ptp_management_msg *res, + const uint8_t *payload, size_t payload_len) +{ + uint16_t data_len; + uint16_t entries; + uint16_t i; + uint64_t new_path[PTP_AS_PATH_MAX_ENTRIES]; + bool changed; + + data_len = ntohs(res->management_message_length_be) - 2; + if ((data_len % 8) != 0 || payload_len < data_len) { + pw_log_warn("Unexpected PTP GET PATH_TRACE_LIST response length: " + "tlv=%u payload=%zu", data_len, payload_len); + return; + } + + entries = data_len / 8; + if (entries > PTP_AS_PATH_MAX_ENTRIES) { + pw_log_warn("PTP PATH_TRACE_LIST has %u entries, capping to %u", + entries, PTP_AS_PATH_MAX_ENTRIES); + entries = PTP_AS_PATH_MAX_ENTRIES; + } + + for (i = 0; i < entries; i++) { + memcpy(&new_path[i], payload + i * 8, 8); + } + + changed = !gptp->path_trace_valid || + gptp->path_trace_count != entries || + memcmp(gptp->path_trace, new_path, + entries * sizeof(uint64_t)) != 0; + + if (changed) { + pw_log_info("PTP path_trace updated: %u entries", entries); + memcpy(gptp->path_trace, new_path, entries * sizeof(uint64_t)); + gptp->path_trace_count = entries; + gptp->path_trace_valid = true; + mark_as_path_dirty(gptp); + } +} + +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_ERROR_STATUS) { + pw_log_debug("PTP management error TLV for id=%04x seq=%u", + mgmt_id, seq); + gptp->req_in_flight = false; + gptp->consecutive_timeouts++; + if (gptp->consecutive_timeouts >= PTP_LOST_TIMEOUT_THRESHOLD) { + gptp_invalidate_state(gptp); + } + continue; + } + + if (ntohs(res.tlv_type_be) != PTP_TLV_TYPE_MGMT) { + pw_log_debug("PTP management returned unexpected tlv type %d", + 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; + } + + gptp->consecutive_timeouts = 0; + + 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; + 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; + case PTP_MGMT_ID_PATH_TRACE_LIST: + handle_path_trace_list(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 % 5) { + case 0: return PTP_MGMT_ID_PARENT_DATA_SET; + case 1: return PTP_MGMT_ID_PATH_TRACE_LIST; + case 2: return PTP_MGMT_ID_CURRENT_DATA_SET; + case 3: return PTP_MGMT_ID_DEFAULT_DATA_SET; + default: return PTP_MGMT_ID_PORT_DATA_SET; + } +} + +static void gptp_periodic(void *data, uint64_t now) +{ + struct gptp *gptp = data; + struct timespec mono_ts; + uint64_t mono_now; + int err; + + (void)now; + + if (!gptp->ptp_mgmt_socket_path) { + return; + } + + if (gptp->ptp_fd < 0 && gptp_open_socket(gptp) < 0) { + return; + } + + clock_gettime(CLOCK_MONOTONIC, &mono_ts); + mono_now = SPA_TIMESPEC_TO_NSEC(&mono_ts); + + if (gptp->req_in_flight && (mono_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; + gptp->consecutive_timeouts++; + if (gptp->consecutive_timeouts >= PTP_LOST_TIMEOUT_THRESHOLD) { + gptp_invalidate_state(gptp); + } + } + + if (gptp->req_in_flight) { + return; + } + + if (gptp->req_sent_ns != 0 && + (mono_now - gptp->req_sent_ns) < PTP_REQUEST_INTERVAL_NS) { + return; + } + + err = send_management_request(gptp, next_management_id(gptp->tick_count), mono_now); + if (err == 0) { + gptp->tick_count++; + } else if (err == -ENOTCONN) { + pw_log_info("PTP management socket disconnected, will reopen"); + gptp_invalidate_state(gptp); + gptp_close_socket(gptp); + } +} + +static void gptp_destroy(void *data) +{ + struct gptp *gptp = data; + spa_hook_remove(&gptp->server_listener); + + gptp_close_socket(gptp); + + free(gptp->ptp_mgmt_socket_path); + free(gptp); +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = gptp_destroy, + .periodic = gptp_periodic, +}; + +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; + + str = pw_properties_get(impl->props, "ptp.management-socket"); + gptp->ptp_mgmt_socket_path = str ? strdup(str) : NULL; + + if (gptp->ptp_mgmt_socket_path) { + if (gptp_open_socket(gptp) < 0) { + pw_log_warn("server %p: PTP management socket unavailable, " + "continuing without GM tracking; will retry on '%s'", + impl, gptp->ptp_mgmt_socket_path); + } + } 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); + + 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) { + 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; +} + +uint16_t avb_gptp_get_path_trace(const struct avb_gptp *agptp, + uint64_t *path_be, uint16_t max_entries) +{ + const struct gptp *gptp = (const struct gptp *)agptp; + uint16_t count; + + if (gptp == NULL || !gptp->path_trace_valid) { + return 0; + } + count = gptp->path_trace_count; + if (count > max_entries) { + count = max_entries; + } + memcpy(path_be, gptp->path_trace, count * sizeof(uint64_t)); + return count; +} diff --git a/src/modules/module-avb/gptp.h b/src/modules/module-avb/gptp.h new file mode 100644 index 000000000..7cf0bc6b1 --- /dev/null +++ b/src/modules/module-avb/gptp.h @@ -0,0 +1,133 @@ +/* PipeWire */ +/* SPDX-FileCopyrightText: Copyright © 2024 Dmitry Sharshakov */ +/* SPDX-FileCopyrightText: Copyright © 2025 Nils Tonnaett */ +/* SPDX-FileCopyrightText: Copyright © 2026 Alexandre Malki */ +/* SPDX-License-Identifier: MIT */ + +#ifndef AVB_GPTP_H +#define AVB_GPTP_H + +#include +#include +#include "internal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define PTP_MESSAGE_TYPE_MANAGEMENT 0x0d +#define PTP_MAJOR_SDO_ID_GPTP (0x1u << 4) +#define PTP_GPTP_MANAGEMENT_TYPE \ + (PTP_MAJOR_SDO_ID_GPTP | PTP_MESSAGE_TYPE_MANAGEMENT) +#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_TLV_TYPE_MGMT_ERROR_STATUS 0x0002 +#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 +#define PTP_MGMT_ID_PATH_TRACE_LIST 0x401C +#define PTP_AS_PATH_MAX_ENTRIES 16 + +/**************************************************************************************/ +/* IEEE 1588-2019, Sec. 15.4.1 PTP management message format - Common Fields */ + +struct ptp_management_msg { +/* IEEE 1588-2019, Sec. 13.3 Header */ + // 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)); + +/* 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.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]; + 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); + +uint16_t avb_gptp_get_path_trace(const struct avb_gptp *gptp, + uint64_t *path_be, uint16_t max_entries); + +#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..4fe8e7e75 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 @@ -60,6 +61,8 @@ struct server_events { void (*periodic) (void *data, uint64_t now); int (*command) (void *data, uint64_t now, const char *command, const char *args, FILE *out); + + void (*gm_changed) (void *data, uint64_t now, uint8_t gm_id[8]); }; struct descriptor { @@ -114,6 +117,7 @@ struct server { unsigned debug_messages:1; + struct avb_gptp *gptp; struct avb_mrp *mrp; struct avb_mmrp *mmrp; struct avb_mvrp *mvrp;