avb: use separate sockets per mac/type

src/modules/module-avb/avdecc.c

@@ -142,16 +142,12 @@ static int load_filter(int fd, uint16_t eth, const uint8_t dest[6], const uint8_
 	return 0;
 }
 
-static int setup_socket(struct server *server)
+int avb_server_make_socket(struct server *server, uint16_t type, const uint8_t mac[6])
 {
-	struct impl *impl = server->impl;
 	int fd, res;
 	struct ifreq req;
 	struct packet_mreq mreq;
 	struct sockaddr_ll sll;
-	struct timespec value, interval;
-	static const uint8_t bmac[6] = AVB_BROADCAST_MAC;
-
 
 	fd = socket(AF_PACKET, SOCK_RAW|SOCK_NONBLOCK, htons(ETH_P_ALL));
 	if (fd < 0) {
@@ -186,14 +182,10 @@ static int setup_socket(struct server *server)
 			(uint64_t)server->mac_addr[4] << 8 |
 			(uint64_t)server->mac_addr[5];
 
-	pw_log_info("%lx %d", server->entity_id, server->ifindex);
-
 	spa_zero(sll);
 	sll.sll_family = AF_PACKET;
 	sll.sll_protocol = htons(ETH_P_ALL);
 	sll.sll_ifindex = server->ifindex;
-	sll.sll_halen = ETH_ALEN;
-	memcpy(sll.sll_addr, bmac, ETH_ALEN);
 	if (bind(fd, (struct sockaddr *) &sll, sizeof(sll)) < 0) {
 		res = -errno;
 		pw_log_error("bind() failed: %m");
@@ -204,7 +196,7 @@ static int setup_socket(struct server *server)
 	mreq.mr_ifindex = server->ifindex;
 	mreq.mr_type = PACKET_MR_MULTICAST;
 	mreq.mr_alen = ETH_ALEN;
-	memcpy(mreq.mr_address, bmac, ETH_ALEN);
+	memcpy(mreq.mr_address, mac, ETH_ALEN);
 
 	if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
 				&mreq, sizeof(mreq)) < 0) {
@@ -213,20 +205,40 @@ static int setup_socket(struct server *server)
 		goto error_close;
 	}
 
-	if ((res = load_filter(fd, AVB_TSN_ETH, bmac, server->mac_addr)) < 0)
+	if ((res = load_filter(fd, type, mac, server->mac_addr)) < 0)
 		goto error_close;
 
+	return fd;
+
+error_close:
+	close(fd);
+	return res;
+}
+
+static int setup_socket(struct server *server)
+{
+	struct impl *impl = server->impl;
+	int fd, res;
+	static const uint8_t bmac[6] = AVB_BROADCAST_MAC;
+	struct timespec value, interval;
+
+	fd = avb_server_make_socket(server, AVB_TSN_ETH, bmac);
+	if (fd < 0)
+		return fd;
+
+	pw_log_info("%lx %d", server->entity_id, server->ifindex);
+
 	server->source = pw_loop_add_io(impl->loop, fd, SPA_IO_IN, true, on_socket_data, server);
 	if (server->source == NULL) {
 		res = -errno;
 		pw_log_error("server %p: can't create server source: %m", impl);
-		goto error_close;
+		goto error_no_source;
 	}
 	server->timer = pw_loop_add_timer(impl->loop, on_timer_event, server);
 	if (server->timer == NULL) {
 		res = -errno;
 		pw_log_error("server %p: can't create timer source: %m", impl);
-		goto error_close;
+		goto error_no_timer;
 	}
 	value.tv_sec = 0;
 	value.tv_nsec = 1;
@@ -236,7 +248,10 @@ static int setup_socket(struct server *server)
 
 	return 0;
 
-error_close:
+error_no_timer:
+	pw_loop_destroy_source(impl->loop, server->source);
+	server->source = NULL;
+error_no_source:
 	close(fd);
 	return res;
 }

src/modules/module-avb/internal.h

@@ -130,6 +130,8 @@ void avdecc_server_free(struct server *server);
 void avdecc_server_add_listener(struct server *server, struct spa_hook *listener,
 		const struct server_events *events, void *data);
 
+int avb_server_make_socket(struct server *server, uint16_t type, const uint8_t mac[6]);
+
 int avb_server_send_packet(struct server *server, const uint8_t dest[6],
 		uint16_t type, void *data, size_t size);
 

src/modules/module-avb/maap.c

@@ -26,11 +26,11 @@
 
 #include "maap.h"
 
-static const uint8_t mac[6] = AVB_MAAP_MAC;
-
 struct maap {
 	struct server *server;
 	struct spa_hook server_listener;
+
+	struct spa_source *source;
 };
 
 static const char *message_type_as_string(uint8_t message_type)
@@ -67,29 +67,46 @@ 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));
 }
 
-static int maap_message(void *data, uint64_t now, const void *message, int len)
+static int maap_message(struct maap *maap, uint64_t now, const void *message, int len)
 {
-	struct maap *maap = data;
-	struct server *server = maap->server;
 	const struct avb_packet_maap *p = message;
 
-	if (ntohs(p->hdr.eth.type) != AVB_TSN_ETH)
-		return 0;
-	if (memcmp(p->hdr.eth.dest, mac, 6) != 0 &&
-	    memcmp(p->hdr.eth.dest, server->mac_addr, 6) != 0)
-		return 0;
 	if (AVB_PACKET_GET_SUBTYPE(&p->hdr) != AVB_SUBTYPE_MAAP)
 		return 0;
 
-	if (maap->server->debug_messages)
+	maap_message_debug(maap, p);
-		maap_message_debug(maap, p);
 
 	return 0;
 }
 
+static void on_socket_data(void *data, int fd, uint32_t mask)
+{
+	struct maap *maap = data;
+	struct timespec now;
+
+	if (mask & SPA_IO_IN) {
+		int len;
+		uint8_t buffer[2048];
+
+		len = recv(fd, buffer, sizeof(buffer), 0);
+
+		if (len < 0) {
+			pw_log_warn("got recv error: %m");
+		}
+		else if (len < (int)sizeof(struct avb_packet_header)) {
+			pw_log_warn("short packet received (%d < %d)", len,
+					(int)sizeof(struct avb_packet_header));
+		} else {
+			clock_gettime(CLOCK_REALTIME, &now);
+			maap_message(maap, SPA_TIMESPEC_TO_NSEC(&now), buffer, len);
+		}
+	}
+}
+
 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);
 }
@@ -97,20 +114,41 @@ static void maap_destroy(void *data)
 static const struct server_events server_events = {
 	AVB_VERSION_SERVER_EVENTS,
 	.destroy = maap_destroy,
-	.message = maap_message
 };
 
 int avb_maap_register(struct server *server)
 {
 	struct maap *maap;
+	uint8_t bmac[6] = AVB_MAAP_MAC;
+	int res, fd;
+
+	fd = avb_server_make_socket(server, AVB_TSN_ETH, bmac);
+	if (fd < 0)
+		return fd;
 
 	maap = calloc(1, sizeof(*maap));
-	if (maap == NULL)
+	if (maap == NULL) {
-		return -errno;
+		res = -errno;
+		goto error_close;
+	}
 
 	maap->server = server;
 
+	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;
+	}
 	avdecc_server_add_listener(server, &maap->server_listener, &server_events, maap);
 
 	return 0;
+
+error_no_source:
+	free(maap);
+error_close:
+	close(fd);
+	return res;
 }

src/modules/module-avb/mmrp.c

@@ -27,7 +27,7 @@
 #include "utils.h"
 #include "mmrp.h"
 
-static const uint8_t mac[6] = AVB_MMRP_MAC;
+static const uint8_t mmrp_mac[6] = AVB_MMRP_MAC;
 
 struct attr {
 	struct avb_mmrp_attribute attr;
@@ -38,6 +38,8 @@ struct mmrp {
 	struct server *server;
 	struct spa_hook server_listener;
 
+	struct spa_source *source;
+
 	struct spa_list attributes;
 };
 
@@ -131,32 +133,47 @@ static const struct avb_mrp_parse_info info = {
 	.process = mmrp_process,
 };
 
-static int mmrp_message(void *data, uint64_t now, const void *message, int len)
+static int mmrp_message(struct mmrp *mmrp, uint64_t now, const void *message, int len)
 {
-	struct mmrp *mmrp = data;
-	const struct avb_packet_mrp *p = message;
-
-	if (ntohs(p->eth.type) != AVB_MMRP_ETH)
-		return 0;
-	if (memcmp(p->eth.dest, mac, 6) != 0)
-		return 0;
-
 	pw_log_debug("MMRP");
 	return avb_mrp_parse_packet(mmrp->server->mrp,
 			now, message, len, &info, mmrp);
 }
 
+static void on_socket_data(void *data, int fd, uint32_t mask)
+{
+	struct mmrp *mmrp = data;
+	struct timespec now;
+
+	if (mask & SPA_IO_IN) {
+		int len;
+		uint8_t buffer[2048];
+
+		len = recv(fd, buffer, sizeof(buffer), 0);
+
+		if (len < 0) {
+			pw_log_warn("got recv error: %m");
+		}
+		else if (len < (int)sizeof(struct avb_packet_header)) {
+			pw_log_warn("short packet received (%d < %d)", len,
+					(int)sizeof(struct avb_packet_header));
+		} else {
+			clock_gettime(CLOCK_REALTIME, &now);
+			mmrp_message(mmrp, SPA_TIMESPEC_TO_NSEC(&now), buffer, len);
+		}
+	}
+}
 static void mmrp_destroy(void *data)
 {
 	struct mmrp *mmrp = data;
 	spa_hook_remove(&mmrp->server_listener);
+	pw_loop_destroy_source(mmrp->server->impl->loop, mmrp->source);
 	free(mmrp);
 }
 
 static const struct server_events server_events = {
 	AVB_VERSION_SERVER_EVENTS,
 	.destroy = mmrp_destroy,
-	.message = mmrp_message
 };
 
 struct avb_mmrp_attribute *avb_mmrp_attribute_new(struct avb_mmrp *m,
@@ -179,15 +196,36 @@ struct avb_mmrp_attribute *avb_mmrp_attribute_new(struct avb_mmrp *m,
 struct avb_mmrp *avb_mmrp_register(struct server *server)
 {
 	struct mmrp *mmrp;
+	int fd, res;
 
-	mmrp = calloc(1, sizeof(*mmrp));
+	fd = avb_server_make_socket(server, AVB_MMRP_ETH, mmrp_mac);
-	if (mmrp == NULL)
+	if (fd < 0) {
+		errno = -fd;
 		return NULL;
+	}
+	mmrp = calloc(1, sizeof(*mmrp));
+	if (mmrp == NULL) {
+		res = -errno;
+		goto error_close;
+	}
 
 	mmrp->server = server;
 	spa_list_init(&mmrp->attributes);
 
+	mmrp->source = pw_loop_add_io(server->impl->loop, fd, SPA_IO_IN, true, on_socket_data, mmrp);
+	if (mmrp->source == NULL) {
+		res = -errno;
+		pw_log_error("mmrp %p: can't create mmrp source: %m", mmrp);
+		goto error_no_source;
+	}
 	avdecc_server_add_listener(server, &mmrp->server_listener, &server_events, mmrp);
 
 	return (struct avb_mmrp*)mmrp;
+
+error_no_source:
+	free(mmrp);
+error_close:
+	close(fd);
+	errno = -res;
+	return NULL;
 }

src/modules/module-avb/msrp.c

@@ -29,7 +29,7 @@
 #include "utils.h"
 #include "msrp.h"
 
-static const uint8_t mac[6] = AVB_MSRP_MAC;
+static const uint8_t msrp_mac[6] = AVB_MSRP_MAC;
 
 struct attr {
 	struct avb_msrp_attribute attr;
@@ -41,6 +41,8 @@ struct msrp {
 	struct spa_hook server_listener;
 	struct spa_hook mrp_listener;
 
+	struct spa_source *source;
+
 	struct spa_list attributes;
 };
 
@@ -269,32 +271,46 @@ static const struct avb_mrp_parse_info info = {
 };
 
 
-static int msrp_message(void *data, uint64_t now, const void *message, int len)
+static int msrp_message(struct msrp *msrp, uint64_t now, const void *message, int len)
 {
-	struct msrp *msrp = data;
-	const struct avb_packet_mrp *p = message;
-
-	if (ntohs(p->eth.type) != AVB_MSRP_ETH)
-		return 0;
-	if (memcmp(p->eth.dest, mac, 6) != 0)
-		return 0;
-
-	pw_log_debug("MSRP");
 	return avb_mrp_parse_packet(msrp->server->mrp,
 			now, message, len, &info, msrp);
 }
+static void on_socket_data(void *data, int fd, uint32_t mask)
+{
+	struct msrp *msrp = data;
+	struct timespec now;
+
+	if (mask & SPA_IO_IN) {
+		int len;
+		uint8_t buffer[2048];
+
+		len = recv(fd, buffer, sizeof(buffer), 0);
+
+		if (len < 0) {
+			pw_log_warn("got recv error: %m");
+		}
+		else if (len < (int)sizeof(struct avb_packet_header)) {
+			pw_log_warn("short packet received (%d < %d)", len,
+					(int)sizeof(struct avb_packet_header));
+		} else {
+			clock_gettime(CLOCK_REALTIME, &now);
+			msrp_message(msrp, SPA_TIMESPEC_TO_NSEC(&now), buffer, len);
+		}
+	}
+}
 
 static void msrp_destroy(void *data)
 {
 	struct msrp *msrp = data;
 	spa_hook_remove(&msrp->server_listener);
+	pw_loop_destroy_source(msrp->server->impl->loop, msrp->source);
 	free(msrp);
 }
 
 static const struct server_events server_events = {
 	AVB_VERSION_SERVER_EVENTS,
 	.destroy = msrp_destroy,
-	.message = msrp_message
 };
 
 struct avb_msrp_attribute *avb_msrp_attribute_new(struct avb_msrp *m,
@@ -348,7 +364,7 @@ static void msrp_event(void *data, uint64_t now, uint8_t event)
 	f->end_mark = 0;
 
 	if (count > 0)
-		avb_server_send_packet(msrp->server, mac, AVB_MSRP_ETH,
+		avb_server_send_packet(msrp->server, msrp_mac, AVB_MSRP_ETH,
 				buffer, total);
 }
 
@@ -369,16 +385,37 @@ static const struct avb_mrp_events mrp_events = {
 struct avb_msrp *avb_msrp_register(struct server *server)
 {
 	struct msrp *msrp;
+	int fd, res;
 
-	msrp = calloc(1, sizeof(*msrp));
+	fd = avb_server_make_socket(server, AVB_MSRP_ETH, msrp_mac);
-	if (msrp == NULL)
+	if (fd < 0) {
+		errno = -fd;
 		return NULL;
+	}
+	msrp = calloc(1, sizeof(*msrp));
+	if (msrp == NULL) {
+		res = -errno;
+		goto error_close;
+	}
 
 	msrp->server = server;
 	spa_list_init(&msrp->attributes);
 
+	msrp->source = pw_loop_add_io(server->impl->loop, fd, SPA_IO_IN, true, on_socket_data, msrp);
+	if (msrp->source == NULL) {
+		res = -errno;
+		pw_log_error("msrp %p: can't create msrp source: %m", msrp);
+		goto error_no_source;
+	}
 	avdecc_server_add_listener(server, &msrp->server_listener, &server_events, msrp);
 	avb_mrp_add_listener(server->mrp, &msrp->mrp_listener, &mrp_events, msrp);
 
 	return (struct avb_msrp*)msrp;
+
+error_no_source:
+	free(msrp);
+error_close:
+	close(fd);
+	errno = -res;
+	return NULL;
 }

src/modules/module-avb/mvrp.c

@@ -26,7 +26,7 @@
 
 #include "mvrp.h"
 
-static const uint8_t mac[6] = AVB_MVRP_MAC;
+static const uint8_t mvrp_mac[6] = AVB_MVRP_MAC;
 
 struct attr {
 	struct avb_mvrp_attribute attr;
@@ -37,6 +37,8 @@ struct mvrp {
 	struct server *server;
 	struct spa_hook server_listener;
 
+	struct spa_source *source;
+
 	struct spa_list attributes;
 };
 
@@ -99,32 +101,48 @@ static const struct avb_mrp_parse_info info = {
 	.process = mvrp_process,
 };
 
-static int mvrp_message(void *data, uint64_t now, const void *message, int len)
+static int mvrp_message(struct mvrp *mvrp, uint64_t now, const void *message, int len)
 {
-	struct mvrp *mvrp = data;
-	const struct avb_packet_mrp *p = message;
-
-	if (ntohs(p->eth.type) != AVB_MVRP_ETH)
-		return 0;
-	if (memcmp(p->eth.dest, mac, 6) != 0)
-		return 0;
-
 	pw_log_debug("MVRP");
 	return avb_mrp_parse_packet(mvrp->server->mrp,
 			now, message, len, &info, mvrp);
 }
 
+static void on_socket_data(void *data, int fd, uint32_t mask)
+{
+	struct mvrp *mvrp = data;
+	struct timespec now;
+
+	if (mask & SPA_IO_IN) {
+		int len;
+		uint8_t buffer[2048];
+
+		len = recv(fd, buffer, sizeof(buffer), 0);
+
+		if (len < 0) {
+			pw_log_warn("got recv error: %m");
+		}
+		else if (len < (int)sizeof(struct avb_packet_header)) {
+			pw_log_warn("short packet received (%d < %d)", len,
+					(int)sizeof(struct avb_packet_header));
+		} else {
+			clock_gettime(CLOCK_REALTIME, &now);
+			mvrp_message(mvrp, SPA_TIMESPEC_TO_NSEC(&now), buffer, len);
+		}
+	}
+}
+
 static void mvrp_destroy(void *data)
 {
 	struct mvrp *mvrp = data;
 	spa_hook_remove(&mvrp->server_listener);
+	pw_loop_destroy_source(mvrp->server->impl->loop, mvrp->source);
 	free(mvrp);
 }
 
 static const struct server_events server_events = {
 	AVB_VERSION_SERVER_EVENTS,
 	.destroy = mvrp_destroy,
-	.message = mvrp_message
 };
 
 struct avb_mvrp_attribute *avb_mvrp_attribute_new(struct avb_mvrp *m,
@@ -147,15 +165,36 @@ struct avb_mvrp_attribute *avb_mvrp_attribute_new(struct avb_mvrp *m,
 struct avb_mvrp *avb_mvrp_register(struct server *server)
 {
 	struct mvrp *mvrp;
+	int fd, res;
 
-	mvrp = calloc(1, sizeof(*mvrp));
+	fd = avb_server_make_socket(server, AVB_MVRP_ETH, mvrp_mac);
-	if (mvrp == NULL)
+	if (fd < 0) {
+		errno = -fd;
 		return NULL;
+	}
+	mvrp = calloc(1, sizeof(*mvrp));
+	if (mvrp == NULL) {
+		res = -errno;
+		goto error_close;
+	}
 
 	mvrp->server = server;
 	spa_list_init(&mvrp->attributes);
 
+	mvrp->source = pw_loop_add_io(server->impl->loop, fd, SPA_IO_IN, true, on_socket_data, mvrp);
+	if (mvrp->source == NULL) {
+		res = -errno;
+		pw_log_error("mvrp %p: can't create mvrp source: %m", mvrp);
+		goto error_no_source;
+	}
 	avdecc_server_add_listener(server, &mvrp->server_listener, &server_events, mvrp);
 
 	return (struct avb_mvrp*)mvrp;
+
+error_no_source:
+	free(mvrp);
+error_close:
+	close(fd);
+	errno = -res;
+	return NULL;
 }