diff --git a/src/modules/module-avb/acmp.h b/src/modules/module-avb/acmp.h index bcbdc0d92..5a41c661f 100644 --- a/src/modules/module-avb/acmp.h +++ b/src/modules/module-avb/acmp.h @@ -40,22 +40,22 @@ #define AVB_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE 9 #define AVB_ACMP_MESSAGE_TYPE_GET_RX_STATE_COMMAND 10 #define AVB_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE 11 -#define AVB_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_COMMAND 12 +#define AVB_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_COMMAND 12 #define AVB_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE 13 -#define AVB_ACMP_STATUS_SUCCESS 0 +#define AVB_ACMP_STATUS_SUCCESS 0 #define AVB_ACMP_STATUS_LISTENER_UNKNOWN_ID 1 #define AVB_ACMP_STATUS_TALKER_UNKNOWN_ID 2 #define AVB_ACMP_STATUS_TALKER_DEST_MAC_FAIL 3 -#define AVB_ACMP_STATUS_TALKER_NO_STREAM_INDEX 4 +#define AVB_ACMP_STATUS_TALKER_NO_STREAM_INDEX 4 #define AVB_ACMP_STATUS_TALKER_NO_BANDWIDTH 5 #define AVB_ACMP_STATUS_TALKER_EXCLUSIVE 6 -#define AVB_ACMP_STATUS_LISTENER_TALKER_TIMEOUT 7 +#define AVB_ACMP_STATUS_LISTENER_TALKER_TIMEOUT 7 #define AVB_ACMP_STATUS_LISTENER_EXCLUSIVE 8 #define AVB_ACMP_STATUS_STATE_UNAVAILABLE 9 #define AVB_ACMP_STATUS_NOT_CONNECTED 10 #define AVB_ACMP_STATUS_NO_SUCH_CONNECTION 11 -#define AVB_ACMP_STATUS_COULD_NOT_SEND_MESSAGE 12 +#define AVB_ACMP_STATUS_COULD_NOT_SEND_MESSAGE 12 #define AVB_ACMP_STATUS_TALKER_MISBEHAVING 13 #define AVB_ACMP_STATUS_LISTENER_MISBEHAVING 14 #define AVB_ACMP_STATUS_RESERVED 15 @@ -64,10 +64,10 @@ #define AVB_ACMP_STATUS_LISTENER_INVALID_CONNECTION 18 #define AVB_ACMP_STATUS_NOT_SUPPORTED 31 -#define AVB_ACMP_TIMEOUT_CONNECT_TX_COMMAND_MS 2000 +#define AVB_ACMP_TIMEOUT_CONNECT_TX_COMMAND_MS 2000 #define AVB_ACMP_TIMEOUT_DISCONNECT_TX_COMMAND_MS 200 #define AVB_ACMP_TIMEOUT_GET_TX_STATE_COMMAND 200 -#define AVB_ACMP_TIMEOUT_CONNECT_RX_COMMAND_MS 4500 +#define AVB_ACMP_TIMEOUT_CONNECT_RX_COMMAND_MS 4500 #define AVB_ACMP_TIMEOUT_DISCONNECT_RX_COMMAND_MS 500 #define AVB_ACMP_TIMEOUT_GET_RX_STATE_COMMAND_MS 200 #define AVB_ACMP_TIMEOUT_GET_TX_CONNECTION_COMMAND 200 diff --git a/src/modules/module-avb/avdecc.c b/src/modules/module-avb/avdecc.c index 084990ea9..388b737b3 100644 --- a/src/modules/module-avb/avdecc.c +++ b/src/modules/module-avb/avdecc.c @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -47,6 +48,7 @@ #include "msrp.h" #include "mvrp.h" #include "descriptors.h" +#include "utils.h" #define DEFAULT_INTERVAL 1 @@ -105,6 +107,41 @@ int avb_server_send_packet(struct server *server, const uint8_t dest[6], return res; } +static int load_filter(int fd, uint16_t eth, const uint8_t dest[6], const uint8_t mac[6]) +{ + struct sock_fprog filter; + struct sock_filter bpf_code[] = { + BPF_STMT(BPF_LD|BPF_H|BPF_ABS, 12), + BPF_JUMP(BPF_JMP|BPF_JEQ, eth, 0, 8), + BPF_STMT(BPF_LD|BPF_W|BPF_ABS, 2), + BPF_JUMP(BPF_JMP|BPF_JEQ, (dest[2] << 24) | + (dest[3] << 16) | + (dest[4] << 8) | + (dest[5]), 0, 2), + BPF_STMT(BPF_LD|BPF_H|BPF_ABS, 0), + BPF_JUMP(BPF_JMP|BPF_JEQ, (dest[0] << 8) | + (dest[1]), 3, 4), + BPF_JUMP(BPF_JMP|BPF_JEQ, (mac[2] << 24) | + (mac[3] << 16) | + (mac[4] << 8) | + (mac[5]), 0, 3), + BPF_STMT(BPF_LD|BPF_H|BPF_ABS, 0), + BPF_JUMP(BPF_JMP|BPF_JEQ, (mac[0] << 8) | + (mac[1]), 0, 1), + BPF_STMT(BPF_RET, 0x00040000), + BPF_STMT(BPF_RET, 0x00000000), + }; + filter.len = sizeof(bpf_code) / 8; + filter.filter = bpf_code; + + if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, + &filter, sizeof(filter)) < 0) { + pw_log_error("setsockopt(ATTACH_FILTER) failed: %m"); + return -errno; + } + return 0; +} + static int setup_socket(struct server *server) { struct impl *impl = server->impl; @@ -113,6 +150,8 @@ static int setup_socket(struct server *server) 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) { @@ -149,9 +188,24 @@ static int setup_socket(struct server *server) 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"); + goto error_close; + } + spa_zero(mreq); mreq.mr_ifindex = server->ifindex; - mreq.mr_type = PACKET_MR_PROMISC; + mreq.mr_type = PACKET_MR_MULTICAST; + mreq.mr_alen = ETH_ALEN; + memcpy(mreq.mr_address, bmac, ETH_ALEN); + if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) { res = -errno; @@ -159,15 +213,8 @@ static int setup_socket(struct server *server) goto error_close; } - spa_zero(sll); - sll.sll_family = AF_PACKET; - sll.sll_protocol = htons(ETH_P_ALL); - sll.sll_ifindex = server->ifindex; - if (bind(fd, (struct sockaddr *) &sll, sizeof(sll)) < 0) { - res = -errno; - pw_log_error("bind() failed: %m"); + if ((res = load_filter(fd, AVB_TSN_ETH, bmac, server->mac_addr)) < 0) goto error_close; - } server->source = pw_loop_add_io(impl->loop, fd, SPA_IO_IN, true, on_socket_data, server); if (server->source == NULL) { diff --git a/src/modules/module-avb/internal.h b/src/modules/module-avb/internal.h index 3683d5ae0..fef599c5f 100644 --- a/src/modules/module-avb/internal.h +++ b/src/modules/module-avb/internal.h @@ -34,7 +34,6 @@ extern "C" { #define AVB_TSN_ETH 0x22f0 #define AVB_BROADCAST_MAC { 0x91, 0xe0, 0xf0, 0x01, 0x00, 0x00 }; - struct impl { struct pw_loop *loop; struct pw_context *context; diff --git a/src/modules/module-avb/maap.c b/src/modules/module-avb/maap.c index aad811e2f..00221a4d8 100644 --- a/src/modules/module-avb/maap.c +++ b/src/modules/module-avb/maap.c @@ -26,7 +26,7 @@ #include "maap.h" -static const uint8_t mac[6] = AVB_BROADCAST_MAC; +static const uint8_t mac[6] = AVB_MAAP_MAC; struct maap { struct server *server; diff --git a/src/modules/module-avb/maap.h b/src/modules/module-avb/maap.h index 05489db39..e9090ba38 100644 --- a/src/modules/module-avb/maap.h +++ b/src/modules/module-avb/maap.h @@ -28,6 +28,9 @@ #include "packets.h" #include "internal.h" +#define AVB_TSN_ETH 0x22f0 +#define AVB_MAAP_MAC { 0x91, 0xe0, 0xf0, 0x00, 0xff, 0x00 }; + #define AVB_MAAP_MESSAGE_TYPE_PROBE 1 #define AVB_MAAP_MESSAGE_TYPE_DEFEND 2 #define AVB_MAAP_MESSAGE_TYPE_ANNOUNCE 3