diff --git a/src/modules/module-avb/avdecc.c b/src/modules/module-avb/avdecc.c index 388b737b3..4b4fbc39e 100644 --- a/src/modules/module-avb/avdecc.c +++ b/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; } diff --git a/src/modules/module-avb/internal.h b/src/modules/module-avb/internal.h index fef599c5f..731bb3b2e 100644 --- a/src/modules/module-avb/internal.h +++ b/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); diff --git a/src/modules/module-avb/maap.c b/src/modules/module-avb/maap.c index 00221a4d8..ecdcd67b9 100644 --- a/src/modules/module-avb/maap.c +++ b/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) - return -errno; + if (maap == NULL) { + 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; } diff --git a/src/modules/module-avb/mmrp.c b/src/modules/module-avb/mmrp.c index 01330efa9..147978f18 100644 --- a/src/modules/module-avb/mmrp.c +++ b/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)); - if (mmrp == NULL) + fd = avb_server_make_socket(server, AVB_MMRP_ETH, mmrp_mac); + 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; } diff --git a/src/modules/module-avb/msrp.c b/src/modules/module-avb/msrp.c index 3896eb5dc..1d0649635 100644 --- a/src/modules/module-avb/msrp.c +++ b/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)); - if (msrp == NULL) + fd = avb_server_make_socket(server, AVB_MSRP_ETH, msrp_mac); + 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; } diff --git a/src/modules/module-avb/mvrp.c b/src/modules/module-avb/mvrp.c index 867ca40c6..f9c541bd9 100644 --- a/src/modules/module-avb/mvrp.c +++ b/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)); - if (mvrp == NULL) + fd = avb_server_make_socket(server, AVB_MVRP_ETH, mvrp_mac); + 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; }