1 files changed, 381 insertions, 0 deletions

diff --git a/src/modules/module-avb/adp.c b/src/modules/module-avb/adp.c
new file mode 100644
index 0000000..6b13c41
--- /dev/null
+++ b/src/modules/module-avb/adp.c
@@ -0,0 +1,381 @@
+/* AVB support
+ *
+ * Copyright © 2022 Wim Taymans
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#include <spa/utils/json.h>
+
+#include <pipewire/pipewire.h>
+
+#include "adp.h"
+#include "aecp-aem-descriptors.h"
+#include "internal.h"
+#include "utils.h"
+
+static const uint8_t mac[6] = AVB_BROADCAST_MAC;
+
+struct entity {
+	struct spa_list link;
+	uint64_t entity_id;
+	uint64_t last_time;
+	int valid_time;
+	unsigned advertise:1;
+	size_t len;
+	uint8_t buf[128];
+};
+
+struct adp {
+	struct server *server;
+	struct spa_hook server_listener;
+
+	struct spa_list entities;
+	uint32_t available_index;
+};
+
+static struct entity *find_entity_by_id(struct adp *adp, uint64_t id)
+{
+	struct entity *e;
+	spa_list_for_each(e, &adp->entities, link)
+		if (e->entity_id == id)
+			return e;
+	return NULL;
+}
+static void entity_free(struct entity *e)
+{
+	spa_list_remove(&e->link);
+	free(e);
+}
+
+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);
+
+	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);
+	e->last_time = now;
+	return 0;
+}
+
+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);
+
+	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);
+	e->last_time = now;
+	return 0;
+}
+
+static int send_discover(struct adp *adp, uint64_t entity_id)
+{
+	uint8_t buf[128];
+	struct avb_ethernet_header *h = (void*)buf;
+	struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void);
+	size_t len = sizeof(*h) + sizeof(*p);
+
+	spa_memzero(buf, sizeof(buf));
+	AVB_PACKET_SET_SUBTYPE(&p->hdr, AVB_SUBTYPE_ADP);
+	AVB_PACKET_SET_LENGTH(&p->hdr, AVB_ADP_CONTROL_DATA_LENGTH);
+	AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_DISCOVER);
+	p->entity_id = htonl(entity_id);
+	avb_server_send_packet(adp->server, mac, AVB_TSN_ETH, buf, len);
+	return 0;
+}
+
+static int adp_message(void *data, uint64_t now, const void *message, int len)
+{
+	struct adp *adp = data;
+	struct server *server = adp->server;
+	const struct avb_ethernet_header *h = message;
+	const struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void);
+	struct entity *e;
+	int message_type;
+	char buf[128];
+	uint64_t entity_id;
+
+	if (ntohs(h->type) != AVB_TSN_ETH)
+		return 0;
+	if (memcmp(h->dest, mac, 6) != 0 &&
+	    memcmp(h->dest, server->mac_addr, 6) != 0)
+		return 0;
+
+	if (AVB_PACKET_GET_SUBTYPE(&p->hdr) != AVB_SUBTYPE_ADP ||
+	    AVB_PACKET_GET_LENGTH(&p->hdr) < AVB_ADP_CONTROL_DATA_LENGTH)
+		return 0;
+
+	message_type = AVB_PACKET_ADP_GET_MESSAGE_TYPE(p);
+	entity_id = be64toh(p->entity_id);
+
+	e = find_entity_by_id(adp, entity_id);
+
+	switch (message_type) {
+	case AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE:
+		if (e == NULL) {
+			e = calloc(1, sizeof(*e));
+			if (e == NULL)
+				return -errno;
+
+			memcpy(e->buf, message, len);
+			e->len = len;
+			e->valid_time = AVB_PACKET_ADP_GET_VALID_TIME(p);
+			e->entity_id = entity_id;
+			spa_list_append(&adp->entities, &e->link);
+			pw_log_info("entity %s available",
+				avb_utils_format_id(buf, sizeof(buf), entity_id));
+		}
+		e->last_time = now;
+		break;
+	case AVB_ADP_MESSAGE_TYPE_ENTITY_DEPARTING:
+		if (e != NULL) {
+			pw_log_info("entity %s departing",
+				avb_utils_format_id(buf, sizeof(buf), entity_id));
+			entity_free(e);
+		}
+		break;
+	case AVB_ADP_MESSAGE_TYPE_ENTITY_DISCOVER:
+		pw_log_info("entity %s advertise",
+				avb_utils_format_id(buf, sizeof(buf), entity_id));
+		if (entity_id == 0UL) {
+			spa_list_for_each(e, &adp->entities, link)
+				if (e->advertise)
+					send_advertise(adp, now, e);
+		} else if (e != NULL &&
+		    e->advertise && e->entity_id == entity_id) {
+			send_advertise(adp, now, e);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void adp_destroy(void *data)
+{
+	struct adp *adp = data;
+	spa_hook_remove(&adp->server_listener);
+	free(adp);
+}
+
+static void check_timeout(struct adp *adp, uint64_t now)
+{
+	struct entity *e, *t;
+	char buf[128];
+
+	spa_list_for_each_safe(e, t, &adp->entities, link) {
+		if (e->last_time + (e->valid_time + 2) * SPA_NSEC_PER_SEC > now)
+			continue;
+
+		pw_log_info("entity %s timeout",
+			avb_utils_format_id(buf, sizeof(buf), e->entity_id));
+
+		if (e->advertise)
+			send_departing(adp, now, e);
+
+		entity_free(e);
+	}
+}
+static void check_readvertize(struct adp *adp, uint64_t now, struct entity *e)
+{
+	char buf[128];
+
+	if (!e->advertise)
+		return;
+
+	if (e->last_time + (e->valid_time / 2) * SPA_NSEC_PER_SEC > now)
+		return;
+
+	pw_log_debug("entity %s readvertise",
+		avb_utils_format_id(buf, sizeof(buf), e->entity_id));
+
+	send_advertise(adp, now, e);
+}
+
+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;
+	struct avb_packet_adp *p;
+	char buf[128];
+
+	d = server_find_descriptor(server, AVB_AEM_DESC_ENTITY, 0);
+	if (d == NULL)
+		return 0;
+
+	entity = d->ptr;
+	entity_id = be64toh(entity->entity_id);
+
+	if ((e = find_entity_by_id(adp, entity_id)) != NULL) {
+		if (e->advertise)
+			check_readvertize(adp, now, e);
+		return 0;
+	}
+
+	d = server_find_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, 0);
+	avb_interface = d ? d->ptr : NULL;
+
+	pw_log_info("entity %s advertise",
+		avb_utils_format_id(buf, sizeof(buf), entity_id));
+
+	e = calloc(1, sizeof(*e));
+	if (e == NULL)
+		return -errno;
+
+	e->advertise = true;
+	e->valid_time = 10;
+	e->last_time = now;
+	e->entity_id = entity_id;
+	e->len = sizeof(*h) + sizeof(*p);
+
+	h = (void*)e->buf;
+	p = SPA_PTROFF(h, sizeof(*h), void);
+	AVB_PACKET_SET_LENGTH(&p->hdr, AVB_ADP_CONTROL_DATA_LENGTH);
+	AVB_PACKET_SET_SUBTYPE(&p->hdr, AVB_SUBTYPE_ADP);
+	AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE);
+	AVB_PACKET_ADP_SET_VALID_TIME(p, e->valid_time);
+
+	p->entity_id = entity->entity_id;
+	p->entity_model_id = entity->entity_model_id;
+	p->entity_capabilities = entity->entity_capabilities;
+	p->talker_stream_sources = entity->talker_stream_sources;
+	p->talker_capabilities = entity->talker_capabilities;
+	p->listener_stream_sinks = entity->listener_stream_sinks;
+	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;
+
+	spa_list_append(&adp->entities, &e->link);
+
+	return 0;
+}
+
+static void adp_periodic(void *data, uint64_t now)
+{
+	struct adp *adp = data;
+	check_timeout(adp, now);
+	check_advertise(adp, now);
+}
+
+static int do_help(struct adp *adp, const char *args, FILE *out)
+{
+	fprintf(out, "{ \"type\": \"help\","
+			"\"text\": \""
+			  "/adp/help: this help \\n"
+			  "/adp/discover [{ \"entity-id\": <id> }] : trigger discover\\n"
+			"\" }");
+	return 0;
+}
+
+static int do_discover(struct adp *adp, const char *args, FILE *out)
+{
+	struct spa_json it[2];
+	char key[128];
+	uint64_t entity_id = 0ULL;
+
+	spa_json_init(&it[0], args, strlen(args));
+	if (spa_json_enter_object(&it[0], &it[1]) <= 0)
+		return -EINVAL;
+
+	while (spa_json_get_string(&it[1], key, sizeof(key)) > 0) {
+		int len;
+		const char *value;
+		uint64_t id_val;
+
+		if ((len = spa_json_next(&it[1], &value)) <= 0)
+			break;
+
+		if (spa_json_is_null(value, len))
+			continue;
+
+		if (spa_streq(key, "entity-id")) {
+			if (avb_utils_parse_id(value, len, &id_val) >= 0)
+				entity_id = id_val;
+		}
+	}
+	send_discover(adp, entity_id);
+	return 0;
+}
+
+static int adp_command(void *data, uint64_t now, const char *command, const char *args, FILE *out)
+{
+	struct adp *adp = data;
+	int res;
+
+	if (!spa_strstartswith(command, "/adp/"))
+		return 0;
+
+	command += strlen("/adp/");
+
+	if (spa_streq(command, "help"))
+		res = do_help(adp, args, out);
+	else if (spa_streq(command, "discover"))
+		res = do_discover(adp, args, out);
+	else
+		res = -ENOTSUP;
+
+	return res;
+}
+
+static const struct server_events server_events = {
+	AVB_VERSION_SERVER_EVENTS,
+	.destroy = adp_destroy,
+	.message = adp_message,
+	.periodic = adp_periodic,
+	.command = adp_command
+};
+
+struct avb_adp *avb_adp_register(struct server *server)
+{
+	struct adp *adp;
+
+	adp = calloc(1, sizeof(*adp));
+	if (adp == NULL)
+		return NULL;
+
+	adp->server = server;
+	spa_list_init(&adp->entities);
+
+	avdecc_server_add_listener(server, &adp->server_listener, &server_events, adp);
+
+	return (struct avb_adp*)adp;
+}
+
+void avb_adp_unregister(struct avb_adp *adp)
+{
+	adp_destroy(adp);
+}