diff --git a/src/modules/meson.build b/src/modules/meson.build index 0214abc78..b92c27679 100644 --- a/src/modules/meson.build +++ b/src/modules/meson.build @@ -524,6 +524,7 @@ pipewire_module_avbtp = shared_library('pipewire-module-avbtp', [ 'module-avbtp.c', 'module-avbtp/avb.c', 'module-avbtp/adp.c', + 'module-avbtp/aecp.c', 'module-avbtp/avdecc.c', 'module-avbtp/maap.c' ], include_directories : [configinc], diff --git a/src/modules/module-avbtp/adp.c b/src/modules/module-avbtp/adp.c index 2be841e2f..416cd6128 100644 --- a/src/modules/module-avbtp/adp.c +++ b/src/modules/module-avbtp/adp.c @@ -42,6 +42,7 @@ struct adp { struct spa_list entities; uint64_t now; + uint32_t available_index; }; static struct entity *find_entity_by_id(struct adp *adp, uint64_t id) @@ -238,6 +239,8 @@ static void adp_destroy(void *data) static int send_advertise(struct adp *adp, uint64_t now, struct entity *e) { + AVBTP_PACKET_ADP_SET_AVAILABLE_INDEX(&e->packet, adp->available_index++); + avbtp_server_send_packet(adp->server, &e->packet, sizeof(e->packet)); e->last_time = now; return 0; } @@ -326,6 +329,7 @@ static int do_help(struct adp *adp, const char *args) static int do_advertise(struct adp *adp, const char *args) { + struct server *server = adp->server; struct entity *e; struct spa_json it[2]; char key[128]; @@ -340,6 +344,8 @@ static int do_advertise(struct adp *adp, const char *args) p = &e->packet; AVBTP_PACKET_ADP_SET_LENGTH(p, AVBTP_ADP_DATA_LENGTH); + AVBTP_PACKET_ADP_SET_SUBTYPE(p, AVBTP_SUBTYPE_ADP); + AVBTP_PACKET_ADP_SET_ENTITY_ID(p, server->entity_id); spa_json_init(&it[0], args, strlen(args)); if (spa_json_enter_object(&it[0], &it[1]) <= 0) diff --git a/src/modules/module-avbtp/aecp.c b/src/modules/module-avbtp/aecp.c new file mode 100644 index 000000000..5bcc960a5 --- /dev/null +++ b/src/modules/module-avbtp/aecp.c @@ -0,0 +1,118 @@ +/* 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 +#include + +#include + +#include "aecp.h" +#include "internal.h" + +struct aecp { + struct server *server; + struct spa_hook server_listener; + + uint64_t now; +}; + +static void aecp_message_debug(struct aecp *aecp, const struct avbtp_packet_aecp *p) +{ +} + +static int aecp_message(void *data, uint64_t now, const void *message, int len) +{ + struct aecp *aecp = data; + const struct avbtp_packet_aecp *p = message; + + if (AVBTP_PACKET_GET_SUBTYPE(p) != AVBTP_SUBTYPE_AECP) + return 0; + + spa_debug_mem(0, p, len); + + return 0; +} + +static void aecp_destroy(void *data) +{ + struct aecp *aecp = data; + spa_hook_remove(&aecp->server_listener); + free(aecp); +} + +static void aecp_periodic(void *data, uint64_t now) +{ +} + +static int do_help(struct aecp *aecp, const char *args) +{ + return 0; +} + +static int aecp_command(void *data, uint64_t now, const char *command, const char *args) +{ + struct aecp *aecp = data; + int res; + + if (!spa_strstartswith(command, "/aecp/")) + return 0; + + command += strlen("/aecp/"); + aecp->now = now; + + if (spa_streq(command, "help")) + res = do_help(aecp, args); + else + res = -ENOTSUP; + + return res; +} + +static const struct server_events server_events = { + AVBTP_VERSION_SERVER_EVENTS, + .destroy = aecp_destroy, + .message = aecp_message, + .periodic = aecp_periodic, + .command = aecp_command +}; + +struct avbtp_aecp *avbtp_aecp_register(struct server *server) +{ + struct aecp *aecp; + + aecp = calloc(1, sizeof(*aecp)); + if (aecp == NULL) + return NULL; + + aecp->server = server; + + avdecc_server_add_listener(server, &aecp->server_listener, &server_events, aecp); + + return (struct avbtp_aecp*)aecp; +} + +void avbtp_aecp_unregister(struct avbtp_aecp *aecp) +{ + aecp_destroy(aecp); +} diff --git a/src/modules/module-avbtp/aecp.h b/src/modules/module-avbtp/aecp.h new file mode 100644 index 000000000..d41bedb8c --- /dev/null +++ b/src/modules/module-avbtp/aecp.h @@ -0,0 +1,69 @@ +/* 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. + */ + +#ifndef AVBTP_AECP_H +#define AVBTP_AECP_H + +#include "packets.h" +#include "internal.h" + +#define AVBTP_AECP_DATA_LENGTH 56 + +struct avbtp_packet_aecp { + uint8_t subtype; +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned sv:1; + unsigned version:3; + unsigned message_type:4; + + unsigned valid_time:5; + unsigned len1:3; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned message_type:4; + unsigned version:3; + unsigned sv:1; + + unsigned len1:3; + unsigned valid_time:5; +#endif + uint8_t len2:8; +} __attribute__ ((__packed__)); + +#define AVBTP_PACKET_AECP_SET_SUBTYPE(p,v) ((p)->subtype = (v)) +#define AVBTP_PACKET_AECP_SET_SV(p,v) ((p)->sv = (v)) +#define AVBTP_PACKET_AECP_SET_VERSION(p,v) ((p)->version = (v)) +#define AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(p,v) ((p)->message_type = (v)) +#define AVBTP_PACKET_AECP_SET_VALID_TIME(p,v) ((p)->valid_time = (v)) +#define AVBTP_PACKET_AECP_SET_LENGTH(p,v) ((p)->len1 = ((v) >> 8),(p)->len2 = (v)) + +#define AVBTP_PACKET_AECP_GET_SUBTYPE(p) ((p)->subtype) +#define AVBTP_PACKET_AECP_GET_SV(p) ((p)->sv) +#define AVBTP_PACKET_AECP_GET_VERSION(p) ((p)->version) +#define AVBTP_PACKET_AECP_GET_MESSAGE_TYPE(p) ((p)->message_type) +#define AVBTP_PACKET_AECP_GET_VALID_TIME(p) ((p)->valid_time) +#define AVBTP_PACKET_AECP_GET_LENGTH(p) (((p)->len1 << 8) | (p)->len2) + +struct avbtp_aecp *avbtp_aecp_register(struct server *server); + +#endif /* AVBTP_AECP_H */ diff --git a/src/modules/module-avbtp/avdecc.c b/src/modules/module-avbtp/avdecc.c index c76e96c6e..dde48b9cd 100644 --- a/src/modules/module-avbtp/avdecc.c +++ b/src/modules/module-avbtp/avdecc.c @@ -40,6 +40,7 @@ #include "packets.h" #include "internal.h" #include "adp.h" +#include "aecp.h" #include "maap.h" #define DEFAULT_INTERVAL 1 @@ -64,7 +65,7 @@ static void on_socket_data(void *data, int fd, uint32_t mask) struct timespec now; if (mask & SPA_IO_IN) { - int len; + int len, count; uint8_t buffer[2048]; len = read(fd, buffer, sizeof(buffer)); @@ -82,10 +83,26 @@ static void on_socket_data(void *data, int fd, uint32_t mask) } } +int avbtp_server_send_packet(struct server *server, void *data, size_t size) +{ + struct sockaddr_ll sll; + uint8_t dest[ETH_ALEN] = { 0x91, 0xe0, 0xf0, 0x01, 0x00, 0x00 }; + + spa_zero(sll); + sll.sll_family = AF_PACKET; + sll.sll_protocol = htons(ETH_P_TSN); + sll.sll_ifindex = server->ifindex; + sll.sll_halen = ETH_ALEN; + memcpy(sll.sll_addr, dest, ETH_ALEN); + + return sendto(server->source->fd, data, size, 0, + (struct sockaddr *)&sll, sizeof(sll)); +} + static int setup_socket(struct server *server) { struct impl *impl = server->impl; - int fd, res, ifindex; + int fd, res; struct ifreq req; struct packet_mreq mreq; struct sockaddr_ll sll; @@ -104,7 +121,7 @@ static int setup_socket(struct server *server) pw_log_error("SIOCGIFINDEX %s failed: %m", server->ifname); goto error_close; } - ifindex = req.ifr_ifindex; + server->ifindex = req.ifr_ifindex; spa_zero(req); snprintf(req.ifr_name, sizeof(req.ifr_name), "%s", server->ifname); @@ -115,10 +132,21 @@ static int setup_socket(struct server *server) } memcpy(server->mac_addr, req.ifr_hwaddr.sa_data, sizeof(server->mac_addr)); + server->entity_id = (uint64_t)server->mac_addr[0] << 56 | + (uint64_t)server->mac_addr[1] << 48 | + (uint64_t)server->mac_addr[2] << 40 | + (uint64_t)0xff << 32 | + (uint64_t)0xfe << 24 | + (uint64_t)server->mac_addr[3] << 16 | + (uint64_t)server->mac_addr[4] << 8 | + (uint64_t)server->mac_addr[5]; + + pw_log_info("%lx", server->entity_id); + spa_zero(sll); sll.sll_family = AF_PACKET; sll.sll_protocol = htons(ETH_P_TSN); - sll.sll_ifindex = ifindex; + sll.sll_ifindex = server->ifindex; if (bind(fd, (struct sockaddr *) &sll, sizeof(sll)) < 0) { res = -errno; pw_log_error("bind() failed: %m"); @@ -126,7 +154,7 @@ static int setup_socket(struct server *server) } spa_zero(mreq); - mreq.mr_ifindex = ifindex; + mreq.mr_ifindex = server->ifindex; mreq.mr_type = PACKET_MR_ALLMULTI; if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(struct packet_mreq)) < 0) { @@ -181,6 +209,7 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s goto error_free; avbtp_adp_register(server); + avbtp_aecp_register(server); avbtp_maap_register(server); clock_gettime(CLOCK_REALTIME, &now); @@ -188,9 +217,13 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s "/adp/advertise", "{" " valid-time = 10 " - " entity-id = \"00:01:02:03:04:05:0001\" " " entity-model-id = \"00:01:02:03:04:05:0600\" " - " entity-capabilities = [ efu-mode aem-supported class-a-supported gptp-supported ] " + " entity-capabilities = [ " + " aem-supported " + " class-a-supported " + " gptp-supported " + " aem-identify-control-index-valid " + " aem-interface-index-valid ] " " talker-stream-sources = 5 " " talker-capabilities = [ implemented audio-source ] " " listener-stream-sinks = 4 " diff --git a/src/modules/module-avbtp/internal.h b/src/modules/module-avbtp/internal.h index f1a3a22d3..3e560bad0 100644 --- a/src/modules/module-avbtp/internal.h +++ b/src/modules/module-avbtp/internal.h @@ -61,8 +61,11 @@ struct server { struct impl *impl; char *ifname; + uint8_t mac_addr[6]; + uint64_t entity_id; + int ifindex; + struct spa_source *source; - char mac_addr[6]; struct spa_source *timer; struct spa_hook_list listener_list; @@ -75,6 +78,7 @@ 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 avbtp_server_send_packet(struct server *server, void *data, size_t size); #ifdef __cplusplus } /* extern "C" */