milan-avb: aecp-aem: emit unsolicited GET_AVB_INFO when gPTP changes

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;
+	fill_get_avb_info_body(server, avb_interface, i);
-	/* 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);
 
 	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;
+			}
+		}
 	}
 }
 

src/modules/module-avb/gptp.c

@@ -40,6 +40,7 @@
 #include <spa/utils/hook.h>
 
 #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;