avb: implement maap

Implement the maap state machine.
Use maap for the destination address of talker streams.

spa/plugins/avb/avb-pcm-sink.c

@@ -709,7 +709,7 @@ static int impl_node_process(void *object)
 	spa_return_val_if_fail(input != NULL, -EIO);
 
 	spa_log_trace_fp(this->log, "%p: process %d %d/%d", this, input->status,
-			input->buffer_id, this->n_buffers);
+			input->buffer_id, port->n_buffers);
 
 	if (this->position && this->position->clock.flags & SPA_IO_CLOCK_FLAG_FREEWHEEL) {
 		input->status = SPA_STATUS_NEED_DATA;

src/modules/module-avb/acmp.c

@@ -129,7 +129,6 @@ static int handle_connect_tx_command(struct acmp *acmp, uint64_t now, const void
 	const struct avb_packet_acmp *p = m;
 	struct avb_packet_acmp *reply = (struct avb_packet_acmp*)buf;
 	int status = AVB_ACMP_STATUS_SUCCESS;
-	uint8_t mac[6] = { 0x91, 0xe0, 0xf0, 0x00, 0x10, 0x00 };
 	struct stream *stream;
 
 	if (be64toh(p->talker_guid) != server->entity_id)
@@ -146,12 +145,11 @@ static int handle_connect_tx_command(struct acmp *acmp, uint64_t now, const void
 	AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE);
 	reply->stream_id = htobe64(stream->id);
 
-	memcpy(stream->addr, mac, 6);
+	stream_activate(stream, now);
 
 	memcpy(reply->stream_dest_mac, stream->addr, 6);
 	reply->connection_count = htons(1);
 	reply->stream_vlan_id = htons(2);
-	stream_activate(stream, now);
 
 done:
 	AVB_PACKET_ACMP_SET_STATUS(reply, status);

src/modules/module-avb/avdecc.c

@@ -285,7 +285,7 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s
 		goto error_free;
 
 	avb_aecp_register(server);
-	avb_maap_register(server);
+	server->maap = avb_maap_register(server);
 	server->mmrp = avb_mmrp_register(server);
 	server->msrp = avb_msrp_register(server);
 	server->mvrp = avb_mvrp_register(server);
@@ -304,6 +304,8 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s
 	server_create_stream(server, SPA_DIRECTION_INPUT, 0);
 	server_create_stream(server, SPA_DIRECTION_OUTPUT, 0);
 
+	avb_maap_reserve(server->maap, 1);
+
 	return server;
 
 error_free:

src/modules/module-avb/internal.h

@@ -95,6 +95,7 @@ struct server {
 	struct avb_mmrp *mmrp;
 	struct avb_mvrp *mvrp;
 	struct avb_msrp *msrp;
+	struct avb_maap *maap;
 
 	struct avb_msrp_attribute *domain_attr;
 };

src/modules/module-avb/maap.c

@@ -25,14 +25,41 @@
 #include <unistd.h>
 
 #include <pipewire/pipewire.h>
+#include <pipewire/conf.h>
 
 #include "maap.h"
 
+#define MAAP_ALLOCATION_POOL_SIZE	0xFE00
+#define MAAP_ALLOCATION_POOL_BASE	 { 0x91, 0xe0, 0xf0, 0x00, 0x00, 0x00 }
+static uint8_t maap_base[6] = MAAP_ALLOCATION_POOL_BASE;
+
+#define MAAP_PROBE_RETRANSMITS		3
+
+#define MAAP_PROBE_INTERVAL_MS		500
+#define MAAP_PROBE_INTERVAL_VAR_MS	100
+
+#define MAAP_ANNOUNCE_INTERVAL_MS	3000
+#define MAAP_ANNOUNCE_INTERVAL_VAR_MS	2000
+
 struct maap {
 	struct server *server;
 	struct spa_hook server_listener;
 
+	struct pw_properties *props;
+
 	struct spa_source *source;
+
+#define STATE_IDLE	0
+#define STATE_PROBE	1
+#define STATE_ANNOUNCE	2
+	uint32_t state;
+	uint64_t timeout;
+	uint32_t probe_count;
+
+	unsigned short xsubi[3];
+
+	uint16_t offset;
+	uint16_t count;
 };
 
 static const char *message_type_as_string(uint8_t message_type)
@@ -69,6 +96,123 @@ static void maap_message_debug(struct maap *maap, const struct avb_packet_maap *
 	pw_log_info("  conflict-count: %d", AVB_PACKET_MAAP_GET_CONFLICT_COUNT(p));
 }
 
+#define PROBE_TIMEOUT(n) ((n) + (MAAP_PROBE_INTERVAL_MS + \
+                        drand48() * MAAP_PROBE_INTERVAL_VAR_MS) * SPA_NSEC_PER_MSEC)
+#define ANNOUNCE_TIMEOUT(n) ((n) + (MAAP_ANNOUNCE_INTERVAL_MS + \
+                        drand48() * MAAP_ANNOUNCE_INTERVAL_VAR_MS) * SPA_NSEC_PER_MSEC)
+
+static int make_new_address(struct maap *maap, uint64_t now, int range)
+{
+	maap->offset = nrand48(maap->xsubi) % (MAAP_ALLOCATION_POOL_SIZE - range);
+	maap->count = range;
+	maap->state = STATE_PROBE;
+	maap->probe_count = MAAP_PROBE_RETRANSMITS;
+	maap->timeout = PROBE_TIMEOUT(now);
+	return 0;
+}
+
+static uint16_t maap_check_conflict(struct maap *maap, const uint8_t request_start[6],
+		uint16_t request_count, uint8_t conflict_start[6])
+{
+	uint16_t our_start, our_end;
+	uint16_t req_start, req_end;
+	uint16_t conf_start, conf_count = 0;
+
+	if (memcmp(request_start, maap_base, 4) != 0)
+		return 0;
+
+	our_start = maap->offset;
+	our_end = our_start + maap->count;
+	req_start = request_start[4] << 8 | request_start[5];
+	req_end = req_start + request_count;
+
+	if (our_start >= req_start && our_start <= req_end) {
+		conf_start = our_start;
+		conf_count = SPA_MIN(our_end, req_end) - our_start;
+	}
+	else if (req_start >= our_start && req_start <= our_end) {
+		conf_start = req_start;
+		conf_count = SPA_MIN(req_end, our_end) - req_start;
+	}
+	if (conf_count == 0)
+		return 0;
+
+	conflict_start[4] = conf_start >> 8;
+	conflict_start[5] = conf_start;
+	return conf_count;
+}
+
+static int send_packet(struct maap *maap, uint64_t now,
+		uint8_t type, const uint8_t conflict_start[6], uint16_t conflict_count)
+{
+	struct avb_packet_maap p;
+	uint8_t bmac[6] = AVB_MAAP_MAC;
+	int res = 0;
+	uint8_t start[6];
+
+	spa_zero(p);
+	memcpy(p.hdr.eth.dest, bmac, 6);
+	memcpy(p.hdr.eth.src, maap->server->mac_addr, 6);
+	p.hdr.eth.type = htons(AVB_TSN_ETH);
+	p.hdr.subtype = AVB_SUBTYPE_MAAP;
+	AVB_PACKET_SET_LENGTH(&p.hdr, sizeof(p) - sizeof(p.hdr.eth));
+
+	AVB_PACKET_MAAP_SET_MAAP_VERSION(&p, 1);
+	AVB_PACKET_MAAP_SET_MESSAGE_TYPE(&p, type);
+
+	memcpy(start, maap_base, 4);
+	start[4] = maap->offset >> 8;
+	start[5] = maap->offset;
+	AVB_PACKET_MAAP_SET_REQUEST_START(&p, start);
+	AVB_PACKET_MAAP_SET_REQUEST_COUNT(&p, maap->count);
+	if (conflict_count) {
+		AVB_PACKET_MAAP_SET_CONFLICT_START(&p, conflict_start);
+		AVB_PACKET_MAAP_SET_CONFLICT_COUNT(&p, conflict_count);
+	}
+
+	pw_log_info("send: %d (%s)", type, message_type_as_string(type));
+	maap_message_debug(maap, &p);
+
+	if (send(maap->source->fd, &p, sizeof(p), 0) < 0) {
+		res = -errno;
+		pw_log_warn("got send error: %m");
+	}
+	return res;
+}
+
+static int handle_probe(struct maap *maap, uint64_t now, const struct avb_packet_maap *p)
+{
+	uint8_t conflict_start[6];
+	uint16_t conflict_count;
+
+	conflict_count = maap_check_conflict(maap, p->request_start, ntohs(p->request_count),
+				conflict_start);
+	if (conflict_count == 0)
+		return 0;
+
+	switch (maap->state) {
+	case STATE_PROBE:
+		make_new_address(maap, now, 8);
+		break;
+	case STATE_ANNOUNCE:
+		send_packet(maap, now, AVB_MAAP_MESSAGE_TYPE_DEFEND, conflict_start, conflict_count);
+		break;
+	}
+	return 0;
+}
+
+static int handle_defend(struct maap *maap, uint64_t now, const struct avb_packet_maap *p)
+{
+	uint8_t conflict_start[6];
+	uint16_t conflict_count;
+
+	conflict_count = maap_check_conflict(maap, p->conflict_start, ntohs(p->conflict_count),
+				conflict_start);
+	if (conflict_count != 0)
+		make_new_address(maap, now, 8);
+	return 0;
+}
+
 static int maap_message(struct maap *maap, uint64_t now, const void *message, int len)
 {
 	const struct avb_packet_maap *p = message;
@@ -78,6 +222,15 @@ static int maap_message(struct maap *maap, uint64_t now, const void *message, in
 
 	maap_message_debug(maap, p);
 
+	switch (AVB_PACKET_MAAP_GET_MESSAGE_TYPE(p)) {
+	case AVB_MAAP_MESSAGE_TYPE_PROBE:
+		handle_probe(maap, now, p);
+		break;
+	case AVB_MAAP_MESSAGE_TYPE_DEFEND:
+	case AVB_MAAP_MESSAGE_TYPE_ANNOUNCE:
+		handle_defend(maap, now, p);
+		break;
+	}
 	return 0;
 }
 
@@ -105,52 +258,117 @@ static void on_socket_data(void *data, int fd, uint32_t mask)
 	}
 }
 
+static void maap_periodic(void *data, uint64_t now)
+{
+	struct maap *maap = data;
+
+	if (now < maap->timeout)
+		return;
+
+	switch(maap->state) {
+	case STATE_IDLE:
+		break;
+	case STATE_PROBE:
+		send_packet(maap, now, AVB_MAAP_MESSAGE_TYPE_PROBE, NULL, 0);
+		if (--maap->probe_count == 0)
+			maap->state = STATE_ANNOUNCE;
+		maap->timeout = PROBE_TIMEOUT(now);
+		break;
+	case STATE_ANNOUNCE:
+		send_packet(maap, now, AVB_MAAP_MESSAGE_TYPE_ANNOUNCE, NULL, 0);
+		maap->timeout = ANNOUNCE_TIMEOUT(now);
+		break;
+	}
+}
+
+static void maap_free(struct maap *maap)
+{
+	pw_loop_destroy_source(maap->server->impl->loop, maap->source);
+	spa_hook_remove(&maap->server_listener);
+	pw_properties_free(maap->props);
+	free(maap);
+}
+
 static void maap_destroy(void *data)
 {
 	struct maap *maap = data;
-	pw_loop_destroy_source(maap->server->impl->loop, maap->source);
-	spa_hook_remove(&maap->server_listener);
-	free(maap);
+	maap_free(maap);
 }
 
 static const struct server_events server_events = {
 	AVB_VERSION_SERVER_EVENTS,
 	.destroy = maap_destroy,
+	.periodic = maap_periodic,
 };
 
-int avb_maap_register(struct server *server)
+struct avb_maap *avb_maap_register(struct server *server)
 {
 	struct maap *maap;
 	uint8_t bmac[6] = AVB_MAAP_MAC;
-	int res, fd;
+	int fd, res;
 
 	fd = avb_server_make_socket(server, AVB_TSN_ETH, bmac);
-	if (fd < 0)
-		return fd;
+	if (fd < 0) {
+		res = fd;
+		goto error;
+	}
 
 	maap = calloc(1, sizeof(*maap));
 	if (maap == NULL) {
 		res = -errno;
 		goto error_close;
 	}
+	maap->props = pw_properties_new(NULL, NULL);
+	if (maap->props == NULL) {
+		res = -errno;
+		goto error_free;
+	}
+
+	pw_conf_load_state("module-avb", "maap", maap->props);
 
 	maap->server = server;
 
+	pw_getrandom(maap->xsubi, sizeof(maap->xsubi), 0);
+
 	pw_log_info("%lx %d", server->entity_id, server->ifindex);
 
 	maap->source = pw_loop_add_io(server->impl->loop, fd, SPA_IO_IN, true, on_socket_data, maap);
 	if (maap->source == NULL) {
 		res = -errno;
 		pw_log_error("maap %p: can't create maap source: %m", maap);
-		goto error_no_source;
+		goto error_free;
 	}
 	avdecc_server_add_listener(server, &maap->server_listener, &server_events, maap);
 
-	return 0;
+	return (struct avb_maap *)maap;
 
-error_no_source:
+error_free:
 	free(maap);
 error_close:
 	close(fd);
-	return res;
+error:
+	errno = -res;
+	return NULL;
+}
+
+int avb_maap_reserve(struct avb_maap *m, uint32_t count)
+{
+	struct maap *maap = (struct maap*)m;
+	make_new_address(maap, 0, count);
+	return 0;
+}
+
+int avb_maap_get_address(struct avb_maap *m, uint8_t addr[6], uint32_t index)
+{
+	struct maap *maap = (struct maap*)m;
+	uint16_t offset;
+
+	if (maap->state != STATE_ANNOUNCE)
+		return -EAGAIN;
+
+	memcpy(addr, maap_base, 6);
+	offset = maap->offset + index;
+	addr[4] = offset >> 8;
+	addr[5] = offset;
+	return 0;
 }

src/modules/module-avb/maap.h

@@ -60,6 +60,11 @@ struct avb_packet_maap {
 #define AVB_PACKET_MAAP_GET_CONFLICT_START(p)		((p)->conflict_start)
 #define AVB_PACKET_MAAP_GET_CONFLICT_COUNT(p)		ntohs((p)->conflict_count)
 
-int avb_maap_register(struct server *server);
+struct avb_maap;
+
+struct avb_maap *avb_maap_register(struct server *server);
+
+int avb_maap_reserve(struct avb_maap *maap, uint32_t count);
+int avb_maap_get_address(struct avb_maap *maap, uint8_t addr[6], uint32_t index);
 
 #endif /* AVB_MAAP_H */

src/modules/module-avb/mrp.c

@@ -315,7 +315,7 @@ void avb_mrp_attribute_update_state(struct avb_mrp_attribute *attr, uint64_t now
 	}
 
 	if (a->registrar_state != state || notify) {
-		pw_log_info("attr %p: %d %d -> %d %d", a, event, a->registrar_state, state, notify);
+		pw_log_debug("attr %p: %d %d -> %d %d", a, event, a->registrar_state, state, notify);
 		a->registrar_state = state;
 	}
 

src/modules/module-avb/msrp.c

@@ -396,7 +396,7 @@ static void msrp_event(void *data, uint64_t now, uint8_t event)
 		if (dispatch[a->attr.type].encode == NULL)
 			continue;
 
-		pw_log_info("send %s %s", dispatch[a->attr.type].name,
+		pw_log_debug("send %s %s", dispatch[a->attr.type].name,
 				avb_mrp_send_name(a->attr.mrp->pending_send));
 
 		len = dispatch[a->attr.type].encode(msrp, a, msg);

src/modules/module-avb/stream.c

@@ -534,6 +534,9 @@ int stream_activate(struct stream *stream, uint64_t now)
 		stream->talker_attr->attr.talker.stream_id = htobe64(stream->peer_id);
 		avb_mrp_attribute_begin(stream->talker_attr->mrp, now);
 	} else {
+		if ((res = avb_maap_get_address(server->maap, stream->addr, stream->index)) < 0)
+			return res;
+
 		stream->listener_attr->attr.listener.stream_id = htobe64(stream->id);
 		stream->listener_attr->param = AVB_MSRP_LISTENER_PARAM_IGNORE;
 		avb_mrp_attribute_begin(stream->listener_attr->mrp, now);

src/modules/module-avb/stream.h

@@ -89,6 +89,7 @@ struct stream {
 
 #include "msrp.h"
 #include "mvrp.h"
+#include "maap.h"
 
 struct stream *server_create_stream(struct server *server,
 		enum spa_direction direction, uint16_t index);