diff --git a/src/modules/module-avb/aecp-aem.c b/src/modules/module-avb/aecp-aem.c index f244c5440..17f8fbf61 100644 --- a/src/modules/module-avb/aecp-aem.c +++ b/src/modules/module-avb/aecp-aem.c @@ -7,6 +7,7 @@ #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" @@ -131,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) @@ -145,8 +165,6 @@ static int handle_get_avb_info_common(struct aecp *aecp, int64_t now, const struct descriptor *desc; uint8_t buf[2048]; size_t size, psize; - uint64_t gm_id_be; - uint8_t flags; i = (struct avb_packet_aecp_aem_get_avb_info*)p->payload; @@ -177,22 +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; - flags = AVB_AEM_AVB_INFO_FLAG_SRP_ENABLED; - /* IEEE 1722.1-2021 Section 7.4.40 GET_AVB_INFO. */ - 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); + 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 */ @@ -586,6 +631,18 @@ 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; + } + } } } diff --git a/src/modules/module-avb/gptp.c b/src/modules/module-avb/gptp.c index 64b27da7f..90c1f15f1 100644 --- a/src/modules/module-avb/gptp.c +++ b/src/modules/module-avb/gptp.c @@ -40,6 +40,7 @@ #include #include "aecp-aem-descriptors.h" +#include "aecp-aem-state.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) @@ -193,6 +194,19 @@ static void update_avb_interface_clock_identity(struct gptp *gptp, 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 update_avb_interface_default(struct gptp *gptp, const struct ptp_default_data_set *dds) { @@ -323,12 +337,14 @@ static void handle_parent_data_set(struct gptp *gptp, memcpy(gptp->gm_id, gmid, 8); gptp->data_valid = true; - /* IEEE 1722.1-2021 Section 7.2.8: AVB_INTERFACE.clock_identity is - * the local gPTP clock. */ 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, @@ -354,6 +370,7 @@ static void handle_default_data_set(struct gptp *gptp, } 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, @@ -404,6 +421,7 @@ static void handle_current_data_set(struct gptp *gptp, 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;