diff options
Diffstat (limited to 'src/modules/module-avb')
32 files changed, 6859 insertions, 0 deletions
diff --git a/src/modules/module-avb/aaf.h b/src/modules/module-avb/aaf.h new file mode 100644 index 0000000..b444ce2 --- /dev/null +++ b/src/modules/module-avb/aaf.h @@ -0,0 +1,102 @@ +/* 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 AVB_AAF_H +#define AVB_AAF_H + +struct avb_packet_aaf { + uint8_t subtype; +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned sv:1; + unsigned version:3; + unsigned mr:1; + unsigned _r1:1; + unsigned gv:1; + unsigned tv:1; + + uint8_t seq_num; + + unsigned _r2:7; + unsigned tu:1; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned tv:1; + unsigned gv:1; + unsigned _r1:1; + unsigned mr:1; + unsigned version:3; + unsigned sv:1; + + uint8_t seq_num; + + unsigned tu:1; + unsigned _r2:7; +#endif + uint64_t stream_id; + uint32_t timestamp; +#define AVB_AAF_FORMAT_USER 0x00 +#define AVB_AAF_FORMAT_FLOAT_32BIT 0x01 +#define AVB_AAF_FORMAT_INT_32BIT 0x02 +#define AVB_AAF_FORMAT_INT_24BIT 0x03 +#define AVB_AAF_FORMAT_INT_16BIT 0x04 +#define AVB_AAF_FORMAT_AES3_32BIT 0x05 + uint8_t format; + +#define AVB_AAF_PCM_NSR_USER 0x00 +#define AVB_AAF_PCM_NSR_8KHZ 0x01 +#define AVB_AAF_PCM_NSR_16KHZ 0x02 +#define AVB_AAF_PCM_NSR_32KHZ 0x03 +#define AVB_AAF_PCM_NSR_44_1KHZ 0x04 +#define AVB_AAF_PCM_NSR_48KHZ 0x05 +#define AVB_AAF_PCM_NSR_88_2KHZ 0x06 +#define AVB_AAF_PCM_NSR_96KHZ 0x07 +#define AVB_AAF_PCM_NSR_176_4KHZ 0x08 +#define AVB_AAF_PCM_NSR_192KHZ 0x09 +#define AVB_AAF_PCM_NSR_24KHZ 0x0A +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned nsr:4; + unsigned _r3:4; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned _r3:4; + unsigned nsr:4; +#endif + uint8_t chan_per_frame; + uint8_t bit_depth; + uint16_t data_len; + +#define AVB_AAF_PCM_SP_NORMAL 0x00 +#define AVB_AAF_PCM_SP_SPARSE 0x01 +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned _r4:3; + unsigned sp:1; + unsigned event:4; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned event:4; + unsigned sp:1; + unsigned _r4:3; +#endif + uint8_t _r5; + uint8_t payload[0]; +} __attribute__ ((__packed__)); + +#endif /* AVB_AAF_H */ diff --git a/src/modules/module-avb/acmp.c b/src/modules/module-avb/acmp.c new file mode 100644 index 0000000..a7a409a --- /dev/null +++ b/src/modules/module-avb/acmp.c @@ -0,0 +1,476 @@ +/* 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 <spa/utils/json.h> +#include <spa/debug/mem.h> + +#include <pipewire/pipewire.h> + +#include "acmp.h" +#include "msrp.h" +#include "internal.h" +#include "stream.h" + +static const uint8_t mac[6] = AVB_BROADCAST_MAC; + +struct pending { + struct spa_list link; + uint64_t last_time; + uint64_t timeout; + uint16_t old_sequence_id; + uint16_t sequence_id; + uint16_t retry; + size_t size; + void *ptr; +}; + +struct acmp { + struct server *server; + struct spa_hook server_listener; + +#define PENDING_TALKER 0 +#define PENDING_LISTENER 1 +#define PENDING_CONTROLLER 2 + struct spa_list pending[3]; + uint16_t sequence_id[3]; +}; + +static void *pending_new(struct acmp *acmp, uint32_t type, uint64_t now, uint32_t timeout_ms, + const void *m, size_t size) +{ + struct pending *p; + struct avb_ethernet_header *h; + struct avb_packet_acmp *pm; + + p = calloc(1, sizeof(*p) + size); + if (p == NULL) + return NULL; + p->last_time = now; + p->timeout = timeout_ms * SPA_NSEC_PER_MSEC; + p->sequence_id = acmp->sequence_id[type]++; + p->size = size; + p->ptr = SPA_PTROFF(p, sizeof(*p), void); + memcpy(p->ptr, m, size); + + h = p->ptr; + pm = SPA_PTROFF(h, sizeof(*h), void); + p->old_sequence_id = ntohs(pm->sequence_id); + pm->sequence_id = htons(p->sequence_id); + spa_list_append(&acmp->pending[type], &p->link); + + return p->ptr; +} + +static struct pending *pending_find(struct acmp *acmp, uint32_t type, uint16_t sequence_id) +{ + struct pending *p; + spa_list_for_each(p, &acmp->pending[type], link) + if (p->sequence_id == sequence_id) + return p; + return NULL; +} + +static void pending_free(struct acmp *acmp, struct pending *p) +{ + spa_list_remove(&p->link); + free(p); +} + +struct msg_info { + uint16_t type; + const char *name; + int (*handle) (struct acmp *acmp, uint64_t now, const void *m, int len); +}; + +static int reply_not_supported(struct acmp *acmp, uint8_t type, const void *m, int len) +{ + struct server *server = acmp->server; + uint8_t buf[len]; + struct avb_ethernet_header *h = (void*)buf; + struct avb_packet_acmp *reply = SPA_PTROFF(h, sizeof(*h), void); + + memcpy(h, m, len); + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, type); + AVB_PACKET_ACMP_SET_STATUS(reply, AVB_ACMP_STATUS_NOT_SUPPORTED); + + return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, len); +} + +static int retry_pending(struct acmp *acmp, uint64_t now, struct pending *p) +{ + struct server *server = acmp->server; + struct avb_ethernet_header *h = p->ptr; + p->retry++; + p->last_time = now; + return avb_server_send_packet(server, h->dest, AVB_TSN_ETH, p->ptr, p->size); +} + +static int handle_connect_tx_command(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + struct server *server = acmp->server; + uint8_t buf[len]; + struct avb_ethernet_header *h = (void*)buf; + struct avb_packet_acmp *reply = SPA_PTROFF(h, sizeof(*h), void); + const struct avb_packet_acmp *p = SPA_PTROFF(m, sizeof(*h), void); + int status = AVB_ACMP_STATUS_SUCCESS; + struct stream *stream; + + if (be64toh(p->talker_guid) != server->entity_id) + return 0; + + memcpy(buf, m, len); + stream = server_find_stream(server, SPA_DIRECTION_OUTPUT, + reply->talker_unique_id); + if (stream == NULL) { + status = AVB_ACMP_STATUS_TALKER_NO_STREAM_INDEX; + goto done; + } + + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE); + reply->stream_id = htobe64(stream->id); + + stream_activate(stream, now); + + memcpy(reply->stream_dest_mac, stream->addr, 6); + reply->connection_count = htons(1); + reply->stream_vlan_id = htons(stream->vlan_id); + +done: + AVB_PACKET_ACMP_SET_STATUS(reply, status); + return avb_server_send_packet(server, h->dest, AVB_TSN_ETH, buf, len); +} + +static int handle_connect_tx_response(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + struct server *server = acmp->server; + struct avb_ethernet_header *h; + const struct avb_packet_acmp *resp = SPA_PTROFF(m, sizeof(*h), void); + struct avb_packet_acmp *reply; + struct pending *pending; + uint16_t sequence_id; + struct stream *stream; + int res; + + if (be64toh(resp->listener_guid) != server->entity_id) + return 0; + + sequence_id = ntohs(resp->sequence_id); + + pending = pending_find(acmp, PENDING_TALKER, sequence_id); + if (pending == NULL) + return 0; + + h = pending->ptr; + pending->size = SPA_MIN((int)pending->size, len); + memcpy(h, m, pending->size); + + reply = SPA_PTROFF(h, sizeof(*h), void); + reply->sequence_id = htons(pending->old_sequence_id); + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, AVB_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE); + + stream = server_find_stream(server, SPA_DIRECTION_INPUT, + ntohs(reply->listener_unique_id)); + if (stream == NULL) + return 0; + + stream->peer_id = be64toh(reply->stream_id); + memcpy(stream->addr, reply->stream_dest_mac, 6); + stream_activate(stream, now); + + res = avb_server_send_packet(server, h->dest, AVB_TSN_ETH, h, pending->size); + + pending_free(acmp, pending); + + return res; +} + +static int handle_disconnect_tx_command(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + struct server *server = acmp->server; + uint8_t buf[len]; + struct avb_ethernet_header *h = (void*)buf; + struct avb_packet_acmp *reply = SPA_PTROFF(h, sizeof(*h), void); + const struct avb_packet_acmp *p = SPA_PTROFF(m, sizeof(*h), void); + int status = AVB_ACMP_STATUS_SUCCESS; + struct stream *stream; + + if (be64toh(p->talker_guid) != server->entity_id) + return 0; + + memcpy(buf, m, len); + stream = server_find_stream(server, SPA_DIRECTION_OUTPUT, + reply->talker_unique_id); + if (stream == NULL) { + status = AVB_ACMP_STATUS_TALKER_NO_STREAM_INDEX; + goto done; + } + + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, AVB_ACMP_MESSAGE_TYPE_DISCONNECT_TX_RESPONSE); + + stream_deactivate(stream, now); + +done: + AVB_PACKET_ACMP_SET_STATUS(reply, status); + return avb_server_send_packet(server, h->dest, AVB_TSN_ETH, buf, len); +} + +static int handle_disconnect_tx_response(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + struct server *server = acmp->server; + struct avb_ethernet_header *h; + struct avb_packet_acmp *reply; + const struct avb_packet_acmp *resp = SPA_PTROFF(m, sizeof(*h), void); + struct pending *pending; + uint16_t sequence_id; + struct stream *stream; + int res; + + if (be64toh(resp->listener_guid) != server->entity_id) + return 0; + + sequence_id = ntohs(resp->sequence_id); + + pending = pending_find(acmp, PENDING_TALKER, sequence_id); + if (pending == NULL) + return 0; + + h = pending->ptr; + pending->size = SPA_MIN((int)pending->size, len); + memcpy(h, m, pending->size); + + reply = SPA_PTROFF(h, sizeof(*h), void); + reply->sequence_id = htons(pending->old_sequence_id); + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, AVB_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE); + + stream = server_find_stream(server, SPA_DIRECTION_INPUT, + reply->listener_unique_id); + if (stream == NULL) + return 0; + + stream_deactivate(stream, now); + + res = avb_server_send_packet(server, h->dest, AVB_TSN_ETH, h, pending->size); + + pending_free(acmp, pending); + + return res; +} + +static int handle_connect_rx_command(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + struct server *server = acmp->server; + struct avb_ethernet_header *h; + const struct avb_packet_acmp *p = SPA_PTROFF(m, sizeof(*h), void); + struct avb_packet_acmp *cmd; + + if (be64toh(p->listener_guid) != server->entity_id) + return 0; + + h = pending_new(acmp, PENDING_TALKER, now, + AVB_ACMP_TIMEOUT_CONNECT_TX_COMMAND_MS, m, len); + if (h == NULL) + return -errno; + + cmd = SPA_PTROFF(h, sizeof(*h), void); + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(cmd, AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_COMMAND); + AVB_PACKET_ACMP_SET_STATUS(cmd, AVB_ACMP_STATUS_SUCCESS); + + return avb_server_send_packet(server, h->dest, AVB_TSN_ETH, h, len); +} + +static int handle_ignore(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + return 0; +} + +static int handle_disconnect_rx_command(struct acmp *acmp, uint64_t now, const void *m, int len) +{ + struct server *server = acmp->server; + struct avb_ethernet_header *h; + const struct avb_packet_acmp *p = SPA_PTROFF(m, sizeof(*h), void); + struct avb_packet_acmp *cmd; + + if (be64toh(p->listener_guid) != server->entity_id) + return 0; + + h = pending_new(acmp, PENDING_TALKER, now, + AVB_ACMP_TIMEOUT_DISCONNECT_TX_COMMAND_MS, m, len); + if (h == NULL) + return -errno; + + cmd = SPA_PTROFF(h, sizeof(*h), void); + AVB_PACKET_ACMP_SET_MESSAGE_TYPE(cmd, AVB_ACMP_MESSAGE_TYPE_DISCONNECT_TX_COMMAND); + AVB_PACKET_ACMP_SET_STATUS(cmd, AVB_ACMP_STATUS_SUCCESS); + + return avb_server_send_packet(server, h->dest, AVB_TSN_ETH, h, len); +} + +static const struct msg_info msg_info[] = { + { AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_COMMAND, "connect-tx-command", handle_connect_tx_command, }, + { AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE, "connect-tx-response", handle_connect_tx_response, }, + { AVB_ACMP_MESSAGE_TYPE_DISCONNECT_TX_COMMAND, "disconnect-tx-command", handle_disconnect_tx_command, }, + { AVB_ACMP_MESSAGE_TYPE_DISCONNECT_TX_RESPONSE, "disconnect-tx-response", handle_disconnect_tx_response, }, + { AVB_ACMP_MESSAGE_TYPE_GET_TX_STATE_COMMAND, "get-tx-state-command", NULL, }, + { AVB_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE, "get-tx-state-response", handle_ignore, }, + { AVB_ACMP_MESSAGE_TYPE_CONNECT_RX_COMMAND, "connect-rx-command", handle_connect_rx_command, }, + { AVB_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE, "connect-rx-response", handle_ignore, }, + { AVB_ACMP_MESSAGE_TYPE_DISCONNECT_RX_COMMAND, "disconnect-rx-command", handle_disconnect_rx_command, }, + { AVB_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE, "disconnect-rx-response", handle_ignore, }, + { AVB_ACMP_MESSAGE_TYPE_GET_RX_STATE_COMMAND, "get-rx-state-command", NULL, }, + { AVB_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE, "get-rx-state-response", handle_ignore, }, + { AVB_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_COMMAND, "get-tx-connection-command", NULL, }, + { AVB_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE, "get-tx-connection-response", handle_ignore, }, +}; + +static inline const struct msg_info *find_msg_info(uint16_t type, const char *name) +{ + SPA_FOR_EACH_ELEMENT_VAR(msg_info, i) { + if ((name == NULL && type == i->type) || + (name != NULL && spa_streq(name, i->name))) + return i; + } + return NULL; +} + +static int acmp_message(void *data, uint64_t now, const void *message, int len) +{ + struct acmp *acmp = data; + struct server *server = acmp->server; + const struct avb_ethernet_header *h = message; + const struct avb_packet_acmp *p = SPA_PTROFF(h, sizeof(*h), void); + const struct msg_info *info; + int message_type; + + if (ntohs(h->type) != AVB_TSN_ETH) + return 0; + if (memcmp(h->dest, mac, 6) != 0 && + memcmp(h->dest, server->mac_addr, 6) != 0) + return 0; + + if (AVB_PACKET_GET_SUBTYPE(&p->hdr) != AVB_SUBTYPE_ACMP) + return 0; + + message_type = AVB_PACKET_ACMP_GET_MESSAGE_TYPE(p); + + info = find_msg_info(message_type, NULL); + if (info == NULL) + return 0; + + pw_log_info("got ACMP message %s", info->name); + + if (info->handle == NULL) + return reply_not_supported(acmp, message_type | 1, message, len); + + return info->handle(acmp, now, message, len); +} + +static void acmp_destroy(void *data) +{ + struct acmp *acmp = data; + spa_hook_remove(&acmp->server_listener); + free(acmp); +} + +static void check_timeout(struct acmp *acmp, uint64_t now, uint16_t type) +{ + struct pending *p, *t; + + spa_list_for_each_safe(p, t, &acmp->pending[type], link) { + if (p->last_time + p->timeout > now) + continue; + + if (p->retry == 0) { + pw_log_info("%p: pending timeout, retry", p); + retry_pending(acmp, now, p); + } else { + pw_log_info("%p: pending timeout, fail", p); + pending_free(acmp, p); + } + } +} +static void acmp_periodic(void *data, uint64_t now) +{ + struct acmp *acmp = data; + check_timeout(acmp, now, PENDING_TALKER); + check_timeout(acmp, now, PENDING_LISTENER); + check_timeout(acmp, now, PENDING_CONTROLLER); +} + +static int do_help(struct acmp *acmp, const char *args, FILE *out) +{ + fprintf(out, "{ \"type\": \"help\"," + "\"text\": \"" + "/adp/help: this help \\n" + "\" }"); + return 0; +} + +static int acmp_command(void *data, uint64_t now, const char *command, const char *args, FILE *out) +{ + struct acmp *acmp = data; + int res; + + if (!spa_strstartswith(command, "/acmp/")) + return 0; + + command += strlen("/acmp/"); + + if (spa_streq(command, "help")) + res = do_help(acmp, args, out); + else + res = -ENOTSUP; + + return res; +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = acmp_destroy, + .message = acmp_message, + .periodic = acmp_periodic, + .command = acmp_command +}; + +struct avb_acmp *avb_acmp_register(struct server *server) +{ + struct acmp *acmp; + + acmp = calloc(1, sizeof(*acmp)); + if (acmp == NULL) + return NULL; + + acmp->server = server; + spa_list_init(&acmp->pending[PENDING_TALKER]); + spa_list_init(&acmp->pending[PENDING_LISTENER]); + spa_list_init(&acmp->pending[PENDING_CONTROLLER]); + + avdecc_server_add_listener(server, &acmp->server_listener, &server_events, acmp); + + return (struct avb_acmp*)acmp; +} + +void avb_acmp_unregister(struct avb_acmp *acmp) +{ + acmp_destroy(acmp); +} diff --git a/src/modules/module-avb/acmp.h b/src/modules/module-avb/acmp.h new file mode 100644 index 0000000..5a41c66 --- /dev/null +++ b/src/modules/module-avb/acmp.h @@ -0,0 +1,99 @@ +/* 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 AVB_ACMP_H +#define AVB_ACMP_H + +#include "packets.h" +#include "internal.h" + +#define AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_COMMAND 0 +#define AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE 1 +#define AVB_ACMP_MESSAGE_TYPE_DISCONNECT_TX_COMMAND 2 +#define AVB_ACMP_MESSAGE_TYPE_DISCONNECT_TX_RESPONSE 3 +#define AVB_ACMP_MESSAGE_TYPE_GET_TX_STATE_COMMAND 4 +#define AVB_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE 5 +#define AVB_ACMP_MESSAGE_TYPE_CONNECT_RX_COMMAND 6 +#define AVB_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE 7 +#define AVB_ACMP_MESSAGE_TYPE_DISCONNECT_RX_COMMAND 8 +#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_RESPONSE 13 + +#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_BANDWIDTH 5 +#define AVB_ACMP_STATUS_TALKER_EXCLUSIVE 6 +#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_TALKER_MISBEHAVING 13 +#define AVB_ACMP_STATUS_LISTENER_MISBEHAVING 14 +#define AVB_ACMP_STATUS_RESERVED 15 +#define AVB_ACMP_STATUS_CONTROLLER_NOT_AUTHORIZED 16 +#define AVB_ACMP_STATUS_INCOMPATIBLE_REQUEST 17 +#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_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_DISCONNECT_RX_COMMAND_MS 500 +#define AVB_ACMP_TIMEOUT_GET_RX_STATE_COMMAND_MS 200 +#define AVB_ACMP_TIMEOUT_GET_TX_CONNECTION_COMMAND 200 + +struct avb_packet_acmp { + struct avb_packet_header hdr; + uint64_t stream_id; + uint64_t controller_guid; + uint64_t talker_guid; + uint64_t listener_guid; + uint16_t talker_unique_id; + uint16_t listener_unique_id; + char stream_dest_mac[6]; + uint16_t connection_count; + uint16_t sequence_id; + uint16_t flags; + uint16_t stream_vlan_id; + uint16_t reserved; +} __attribute__ ((__packed__)); + +#define AVB_PACKET_ACMP_SET_MESSAGE_TYPE(p,v) AVB_PACKET_SET_SUB1(&(p)->hdr, v) +#define AVB_PACKET_ACMP_SET_STATUS(p,v) AVB_PACKET_SET_SUB2(&(p)->hdr, v) + +#define AVB_PACKET_ACMP_GET_MESSAGE_TYPE(p) AVB_PACKET_GET_SUB1(&(p)->hdr) +#define AVB_PACKET_ACMP_GET_STATUS(p) AVB_PACKET_GET_SUB2(&(p)->hdr) + +struct avb_acmp *avb_acmp_register(struct server *server); + +#endif /* AVB_ACMP_H */ diff --git a/src/modules/module-avb/adp.c b/src/modules/module-avb/adp.c new file mode 100644 index 0000000..6b13c41 --- /dev/null +++ b/src/modules/module-avb/adp.c @@ -0,0 +1,381 @@ +/* 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 <spa/utils/json.h> + +#include <pipewire/pipewire.h> + +#include "adp.h" +#include "aecp-aem-descriptors.h" +#include "internal.h" +#include "utils.h" + +static const uint8_t mac[6] = AVB_BROADCAST_MAC; + +struct entity { + struct spa_list link; + uint64_t entity_id; + uint64_t last_time; + int valid_time; + unsigned advertise:1; + size_t len; + uint8_t buf[128]; +}; + +struct adp { + struct server *server; + struct spa_hook server_listener; + + struct spa_list entities; + uint32_t available_index; +}; + +static struct entity *find_entity_by_id(struct adp *adp, uint64_t id) +{ + struct entity *e; + spa_list_for_each(e, &adp->entities, link) + if (e->entity_id == id) + return e; + return NULL; +} +static void entity_free(struct entity *e) +{ + spa_list_remove(&e->link); + free(e); +} + +static int send_departing(struct adp *adp, uint64_t now, struct entity *e) +{ + struct avb_ethernet_header *h = (void*)e->buf; + struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void); + + AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_DEPARTING); + p->available_index = htonl(adp->available_index++); + avb_server_send_packet(adp->server, mac, AVB_TSN_ETH, e->buf, e->len); + e->last_time = now; + return 0; +} + +static int send_advertise(struct adp *adp, uint64_t now, struct entity *e) +{ + struct avb_ethernet_header *h = (void*)e->buf; + struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void); + + AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE); + p->available_index = htonl(adp->available_index++); + avb_server_send_packet(adp->server, mac, AVB_TSN_ETH, e->buf, e->len); + e->last_time = now; + return 0; +} + +static int send_discover(struct adp *adp, uint64_t entity_id) +{ + uint8_t buf[128]; + struct avb_ethernet_header *h = (void*)buf; + struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void); + size_t len = sizeof(*h) + sizeof(*p); + + spa_memzero(buf, sizeof(buf)); + AVB_PACKET_SET_SUBTYPE(&p->hdr, AVB_SUBTYPE_ADP); + AVB_PACKET_SET_LENGTH(&p->hdr, AVB_ADP_CONTROL_DATA_LENGTH); + AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_DISCOVER); + p->entity_id = htonl(entity_id); + avb_server_send_packet(adp->server, mac, AVB_TSN_ETH, buf, len); + return 0; +} + +static int adp_message(void *data, uint64_t now, const void *message, int len) +{ + struct adp *adp = data; + struct server *server = adp->server; + const struct avb_ethernet_header *h = message; + const struct avb_packet_adp *p = SPA_PTROFF(h, sizeof(*h), void); + struct entity *e; + int message_type; + char buf[128]; + uint64_t entity_id; + + if (ntohs(h->type) != AVB_TSN_ETH) + return 0; + if (memcmp(h->dest, mac, 6) != 0 && + memcmp(h->dest, server->mac_addr, 6) != 0) + return 0; + + if (AVB_PACKET_GET_SUBTYPE(&p->hdr) != AVB_SUBTYPE_ADP || + AVB_PACKET_GET_LENGTH(&p->hdr) < AVB_ADP_CONTROL_DATA_LENGTH) + return 0; + + message_type = AVB_PACKET_ADP_GET_MESSAGE_TYPE(p); + entity_id = be64toh(p->entity_id); + + e = find_entity_by_id(adp, entity_id); + + switch (message_type) { + case AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE: + if (e == NULL) { + e = calloc(1, sizeof(*e)); + if (e == NULL) + return -errno; + + memcpy(e->buf, message, len); + e->len = len; + e->valid_time = AVB_PACKET_ADP_GET_VALID_TIME(p); + e->entity_id = entity_id; + spa_list_append(&adp->entities, &e->link); + pw_log_info("entity %s available", + avb_utils_format_id(buf, sizeof(buf), entity_id)); + } + e->last_time = now; + break; + case AVB_ADP_MESSAGE_TYPE_ENTITY_DEPARTING: + if (e != NULL) { + pw_log_info("entity %s departing", + avb_utils_format_id(buf, sizeof(buf), entity_id)); + entity_free(e); + } + break; + case AVB_ADP_MESSAGE_TYPE_ENTITY_DISCOVER: + pw_log_info("entity %s advertise", + avb_utils_format_id(buf, sizeof(buf), entity_id)); + if (entity_id == 0UL) { + spa_list_for_each(e, &adp->entities, link) + if (e->advertise) + send_advertise(adp, now, e); + } else if (e != NULL && + e->advertise && e->entity_id == entity_id) { + send_advertise(adp, now, e); + } + break; + default: + return -EINVAL; + } + return 0; +} + +static void adp_destroy(void *data) +{ + struct adp *adp = data; + spa_hook_remove(&adp->server_listener); + free(adp); +} + +static void check_timeout(struct adp *adp, uint64_t now) +{ + struct entity *e, *t; + char buf[128]; + + spa_list_for_each_safe(e, t, &adp->entities, link) { + if (e->last_time + (e->valid_time + 2) * SPA_NSEC_PER_SEC > now) + continue; + + pw_log_info("entity %s timeout", + avb_utils_format_id(buf, sizeof(buf), e->entity_id)); + + if (e->advertise) + send_departing(adp, now, e); + + entity_free(e); + } +} +static void check_readvertize(struct adp *adp, uint64_t now, struct entity *e) +{ + char buf[128]; + + if (!e->advertise) + return; + + if (e->last_time + (e->valid_time / 2) * SPA_NSEC_PER_SEC > now) + return; + + pw_log_debug("entity %s readvertise", + avb_utils_format_id(buf, sizeof(buf), e->entity_id)); + + send_advertise(adp, now, e); +} + +static int check_advertise(struct adp *adp, uint64_t now) +{ + struct server *server = adp->server; + const struct descriptor *d; + struct avb_aem_desc_entity *entity; + struct avb_aem_desc_avb_interface *avb_interface; + struct entity *e; + uint64_t entity_id; + struct avb_ethernet_header *h; + struct avb_packet_adp *p; + char buf[128]; + + d = server_find_descriptor(server, AVB_AEM_DESC_ENTITY, 0); + if (d == NULL) + return 0; + + entity = d->ptr; + entity_id = be64toh(entity->entity_id); + + if ((e = find_entity_by_id(adp, entity_id)) != NULL) { + if (e->advertise) + check_readvertize(adp, now, e); + return 0; + } + + d = server_find_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, 0); + avb_interface = d ? d->ptr : NULL; + + pw_log_info("entity %s advertise", + avb_utils_format_id(buf, sizeof(buf), entity_id)); + + e = calloc(1, sizeof(*e)); + if (e == NULL) + return -errno; + + e->advertise = true; + e->valid_time = 10; + e->last_time = now; + e->entity_id = entity_id; + e->len = sizeof(*h) + sizeof(*p); + + h = (void*)e->buf; + p = SPA_PTROFF(h, sizeof(*h), void); + AVB_PACKET_SET_LENGTH(&p->hdr, AVB_ADP_CONTROL_DATA_LENGTH); + AVB_PACKET_SET_SUBTYPE(&p->hdr, AVB_SUBTYPE_ADP); + AVB_PACKET_ADP_SET_MESSAGE_TYPE(p, AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE); + AVB_PACKET_ADP_SET_VALID_TIME(p, e->valid_time); + + p->entity_id = entity->entity_id; + p->entity_model_id = entity->entity_model_id; + p->entity_capabilities = entity->entity_capabilities; + p->talker_stream_sources = entity->talker_stream_sources; + p->talker_capabilities = entity->talker_capabilities; + p->listener_stream_sinks = entity->listener_stream_sinks; + p->listener_capabilities = entity->listener_capabilities; + p->controller_capabilities = entity->controller_capabilities; + p->available_index = entity->available_index; + if (avb_interface) { + p->gptp_grandmaster_id = avb_interface->clock_identity; + p->gptp_domain_number = avb_interface->domain_number; + } + p->identify_control_index = 0; + p->interface_index = 0; + p->association_id = entity->association_id; + + spa_list_append(&adp->entities, &e->link); + + return 0; +} + +static void adp_periodic(void *data, uint64_t now) +{ + struct adp *adp = data; + check_timeout(adp, now); + check_advertise(adp, now); +} + +static int do_help(struct adp *adp, const char *args, FILE *out) +{ + fprintf(out, "{ \"type\": \"help\"," + "\"text\": \"" + "/adp/help: this help \\n" + "/adp/discover [{ \"entity-id\": <id> }] : trigger discover\\n" + "\" }"); + return 0; +} + +static int do_discover(struct adp *adp, const char *args, FILE *out) +{ + struct spa_json it[2]; + char key[128]; + uint64_t entity_id = 0ULL; + + spa_json_init(&it[0], args, strlen(args)); + if (spa_json_enter_object(&it[0], &it[1]) <= 0) + return -EINVAL; + + while (spa_json_get_string(&it[1], key, sizeof(key)) > 0) { + int len; + const char *value; + uint64_t id_val; + + if ((len = spa_json_next(&it[1], &value)) <= 0) + break; + + if (spa_json_is_null(value, len)) + continue; + + if (spa_streq(key, "entity-id")) { + if (avb_utils_parse_id(value, len, &id_val) >= 0) + entity_id = id_val; + } + } + send_discover(adp, entity_id); + return 0; +} + +static int adp_command(void *data, uint64_t now, const char *command, const char *args, FILE *out) +{ + struct adp *adp = data; + int res; + + if (!spa_strstartswith(command, "/adp/")) + return 0; + + command += strlen("/adp/"); + + if (spa_streq(command, "help")) + res = do_help(adp, args, out); + else if (spa_streq(command, "discover")) + res = do_discover(adp, args, out); + else + res = -ENOTSUP; + + return res; +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = adp_destroy, + .message = adp_message, + .periodic = adp_periodic, + .command = adp_command +}; + +struct avb_adp *avb_adp_register(struct server *server) +{ + struct adp *adp; + + adp = calloc(1, sizeof(*adp)); + if (adp == NULL) + return NULL; + + adp->server = server; + spa_list_init(&adp->entities); + + avdecc_server_add_listener(server, &adp->server_listener, &server_events, adp); + + return (struct avb_adp*)adp; +} + +void avb_adp_unregister(struct avb_adp *adp) +{ + adp_destroy(adp); +} diff --git a/src/modules/module-avb/adp.h b/src/modules/module-avb/adp.h new file mode 100644 index 0000000..c546088 --- /dev/null +++ b/src/modules/module-avb/adp.h @@ -0,0 +1,105 @@ +/* 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 AVB_ADP_H +#define AVB_ADP_H + +#include "packets.h" +#include "internal.h" + +#define AVB_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE 0 +#define AVB_ADP_MESSAGE_TYPE_ENTITY_DEPARTING 1 +#define AVB_ADP_MESSAGE_TYPE_ENTITY_DISCOVER 2 + +#define AVB_ADP_ENTITY_CAPABILITY_EFU_MODE (1u<<0) +#define AVB_ADP_ENTITY_CAPABILITY_ADDRESS_ACCESS_SUPPORTED (1u<<1) +#define AVB_ADP_ENTITY_CAPABILITY_GATEWAY_ENTITY (1u<<2) +#define AVB_ADP_ENTITY_CAPABILITY_AEM_SUPPORTED (1u<<3) +#define AVB_ADP_ENTITY_CAPABILITY_LEGACY_AVC (1u<<4) +#define AVB_ADP_ENTITY_CAPABILITY_ASSOCIATION_ID_SUPPORTED (1u<<5) +#define AVB_ADP_ENTITY_CAPABILITY_ASSOCIATION_ID_VALID (1u<<6) +#define AVB_ADP_ENTITY_CAPABILITY_VENDOR_UNIQUE_SUPPORTED (1u<<7) +#define AVB_ADP_ENTITY_CAPABILITY_CLASS_A_SUPPORTED (1u<<8) +#define AVB_ADP_ENTITY_CAPABILITY_CLASS_B_SUPPORTED (1u<<9) +#define AVB_ADP_ENTITY_CAPABILITY_GPTP_SUPPORTED (1u<<10) +#define AVB_ADP_ENTITY_CAPABILITY_AEM_AUTHENTICATION_SUPPORTED (1u<<11) +#define AVB_ADP_ENTITY_CAPABILITY_AEM_AUTHENTICATION_REQUIRED (1u<<12) +#define AVB_ADP_ENTITY_CAPABILITY_AEM_PERSISTENT_ACQUIRE_SUPPORTED (1u<<13) +#define AVB_ADP_ENTITY_CAPABILITY_AEM_IDENTIFY_CONTROL_INDEX_VALID (1u<<14) +#define AVB_ADP_ENTITY_CAPABILITY_AEM_INTERFACE_INDEX_VALID (1u<<15) +#define AVB_ADP_ENTITY_CAPABILITY_GENERAL_CONTROLLER_IGNORE (1u<<16) +#define AVB_ADP_ENTITY_CAPABILITY_ENTITY_NOT_READY (1u<<17) + +#define AVB_ADP_TALKER_CAPABILITY_IMPLEMENTED (1u<<0) +#define AVB_ADP_TALKER_CAPABILITY_OTHER_SOURCE (1u<<9) +#define AVB_ADP_TALKER_CAPABILITY_CONTROL_SOURCE (1u<<10) +#define AVB_ADP_TALKER_CAPABILITY_MEDIA_CLOCK_SOURCE (1u<<11) +#define AVB_ADP_TALKER_CAPABILITY_SMPTE_SOURCE (1u<<12) +#define AVB_ADP_TALKER_CAPABILITY_MIDI_SOURCE (1u<<13) +#define AVB_ADP_TALKER_CAPABILITY_AUDIO_SOURCE (1u<<14) +#define AVB_ADP_TALKER_CAPABILITY_VIDEO_SOURCE (1u<<15) + +#define AVB_ADP_LISTENER_CAPABILITY_IMPLEMENTED (1u<<0) +#define AVB_ADP_LISTENER_CAPABILITY_OTHER_SINK (1u<<9) +#define AVB_ADP_LISTENER_CAPABILITY_CONTROL_SINK (1u<<10) +#define AVB_ADP_LISTENER_CAPABILITY_MEDIA_CLOCK_SINK (1u<<11) +#define AVB_ADP_LISTENER_CAPABILITY_SMPTE_SINK (1u<<12) +#define AVB_ADP_LISTENER_CAPABILITY_MIDI_SINK (1u<<13) +#define AVB_ADP_LISTENER_CAPABILITY_AUDIO_SINK (1u<<14) +#define AVB_ADP_LISTENER_CAPABILITY_VIDEO_SINK (1u<<15) + +#define AVB_ADP_CONTROLLER_CAPABILITY_IMPLEMENTED (1u<<0) +#define AVB_ADP_CONTROLLER_CAPABILITY_LAYER3_PROXY (1u<<1) + +#define AVB_ADP_CONTROL_DATA_LENGTH 56 + +struct avb_packet_adp { + struct avb_packet_header hdr; + uint64_t entity_id; + uint64_t entity_model_id; + uint32_t entity_capabilities; + uint16_t talker_stream_sources; + uint16_t talker_capabilities; + uint16_t listener_stream_sinks; + uint16_t listener_capabilities; + uint32_t controller_capabilities; + uint32_t available_index; + uint64_t gptp_grandmaster_id; + uint8_t gptp_domain_number; + uint8_t reserved0[3]; + uint16_t identify_control_index; + uint16_t interface_index; + uint64_t association_id; + uint32_t reserved1; +} __attribute__ ((__packed__)); + +#define AVB_PACKET_ADP_SET_MESSAGE_TYPE(p,v) AVB_PACKET_SET_SUB1(&(p)->hdr, v) +#define AVB_PACKET_ADP_SET_VALID_TIME(p,v) AVB_PACKET_SET_SUB2(&(p)->hdr, v) + +#define AVB_PACKET_ADP_GET_MESSAGE_TYPE(p) AVB_PACKET_GET_SUB1(&(p)->hdr) +#define AVB_PACKET_ADP_GET_VALID_TIME(p) AVB_PACKET_GET_SUB2(&(p)->hdr) + +struct avb_adp *avb_adp_register(struct server *server); + +#endif /* AVB_ADP_H */ diff --git a/src/modules/module-avb/aecp-aem-descriptors.h b/src/modules/module-avb/aecp-aem-descriptors.h new file mode 100644 index 0000000..101c331 --- /dev/null +++ b/src/modules/module-avb/aecp-aem-descriptors.h @@ -0,0 +1,247 @@ +/* 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 AVB_AECP_AEM_DESCRIPTORS_H +#define AVB_AECP_AEM_DESCRIPTORS_H + +#include "internal.h" + +#define AVB_AEM_DESC_ENTITY 0x0000 +#define AVB_AEM_DESC_CONFIGURATION 0x0001 +#define AVB_AEM_DESC_AUDIO_UNIT 0x0002 +#define AVB_AEM_DESC_VIDEO_UNIT 0x0003 +#define AVB_AEM_DESC_SENSOR_UNIT 0x0004 +#define AVB_AEM_DESC_STREAM_INPUT 0x0005 +#define AVB_AEM_DESC_STREAM_OUTPUT 0x0006 +#define AVB_AEM_DESC_JACK_INPUT 0x0007 +#define AVB_AEM_DESC_JACK_OUTPUT 0x0008 +#define AVB_AEM_DESC_AVB_INTERFACE 0x0009 +#define AVB_AEM_DESC_CLOCK_SOURCE 0x000a +#define AVB_AEM_DESC_MEMORY_OBJECT 0x000b +#define AVB_AEM_DESC_LOCALE 0x000c +#define AVB_AEM_DESC_STRINGS 0x000d +#define AVB_AEM_DESC_STREAM_PORT_INPUT 0x000e +#define AVB_AEM_DESC_STREAM_PORT_OUTPUT 0x000f +#define AVB_AEM_DESC_EXTERNAL_PORT_INPUT 0x0010 +#define AVB_AEM_DESC_EXTERNAL_PORT_OUTPUT 0x0011 +#define AVB_AEM_DESC_INTERNAL_PORT_INPUT 0x0012 +#define AVB_AEM_DESC_INTERNAL_PORT_OUTPUT 0x0013 +#define AVB_AEM_DESC_AUDIO_CLUSTER 0x0014 +#define AVB_AEM_DESC_VIDEO_CLUSTER 0x0015 +#define AVB_AEM_DESC_SENSOR_CLUSTER 0x0016 +#define AVB_AEM_DESC_AUDIO_MAP 0x0017 +#define AVB_AEM_DESC_VIDEO_MAP 0x0018 +#define AVB_AEM_DESC_SENSOR_MAP 0x0019 +#define AVB_AEM_DESC_CONTROL 0x001a +#define AVB_AEM_DESC_SIGNAL_SELECTOR 0x001b +#define AVB_AEM_DESC_MIXER 0x001c +#define AVB_AEM_DESC_MATRIX 0x001d +#define AVB_AEM_DESC_MATRIX_SIGNAL 0x001e +#define AVB_AEM_DESC_SIGNAL_SPLITTER 0x001f +#define AVB_AEM_DESC_SIGNAL_COMBINER 0x0020 +#define AVB_AEM_DESC_SIGNAL_DEMULTIPLEXER 0x0021 +#define AVB_AEM_DESC_SIGNAL_MULTIPLEXER 0x0022 +#define AVB_AEM_DESC_SIGNAL_TRANSCODER 0x0023 +#define AVB_AEM_DESC_CLOCK_DOMAIN 0x0024 +#define AVB_AEM_DESC_CONTROL_BLOCK 0x0025 +#define AVB_AEM_DESC_INVALID 0xffff + +struct avb_aem_desc_entity { + uint64_t entity_id; + uint64_t entity_model_id; + uint32_t entity_capabilities; + uint16_t talker_stream_sources; + uint16_t talker_capabilities; + uint16_t listener_stream_sinks; + uint16_t listener_capabilities; + uint32_t controller_capabilities; + uint32_t available_index; + uint64_t association_id; + char entity_name[64]; + uint16_t vendor_name_string; + uint16_t model_name_string; + char firmware_version[64]; + char group_name[64]; + char serial_number[64]; + uint16_t configurations_count; + uint16_t current_configuration; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_descriptor_count { + uint16_t descriptor_type; + uint16_t descriptor_count; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_configuration { + char object_name[64]; + uint16_t localized_description; + uint16_t descriptor_counts_count; + uint16_t descriptor_counts_offset; + struct avb_aem_desc_descriptor_count descriptor_counts[0]; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_sampling_rate { + uint32_t pull_frequency; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_audio_unit { + char object_name[64]; + uint16_t localized_description; + uint16_t clock_domain_index; + uint16_t number_of_stream_input_ports; + uint16_t base_stream_input_port; + uint16_t number_of_stream_output_ports; + uint16_t base_stream_output_port; + uint16_t number_of_external_input_ports; + uint16_t base_external_input_port; + uint16_t number_of_external_output_ports; + uint16_t base_external_output_port; + uint16_t number_of_internal_input_ports; + uint16_t base_internal_input_port; + uint16_t number_of_internal_output_ports; + uint16_t base_internal_output_port; + uint16_t number_of_controls; + uint16_t base_control; + uint16_t number_of_signal_selectors; + uint16_t base_signal_selector; + uint16_t number_of_mixers; + uint16_t base_mixer; + uint16_t number_of_matrices; + uint16_t base_matrix; + uint16_t number_of_splitters; + uint16_t base_splitter; + uint16_t number_of_combiners; + uint16_t base_combiner; + uint16_t number_of_demultiplexers; + uint16_t base_demultiplexer; + uint16_t number_of_multiplexers; + uint16_t base_multiplexer; + uint16_t number_of_transcoders; + uint16_t base_transcoder; + uint16_t number_of_control_blocks; + uint16_t base_control_block; + uint32_t current_sampling_rate; + uint16_t sampling_rates_offset; + uint16_t sampling_rates_count; + struct avb_aem_desc_sampling_rate sampling_rates[0]; +} __attribute__ ((__packed__)); + +#define AVB_AEM_DESC_STREAM_FLAG_SYNC_SOURCE (1u<<0) +#define AVB_AEM_DESC_STREAM_FLAG_CLASS_A (1u<<1) +#define AVB_AEM_DESC_STREAM_FLAG_CLASS_B (1u<<2) +#define AVB_AEM_DESC_STREAM_FLAG_SUPPORTS_ENCRYPTED (1u<<3) +#define AVB_AEM_DESC_STREAM_FLAG_PRIMARY_BACKUP_SUPPORTED (1u<<4) +#define AVB_AEM_DESC_STREAM_FLAG_PRIMARY_BACKUP_VALID (1u<<5) +#define AVB_AEM_DESC_STREAM_FLAG_SECONDARY_BACKUP_SUPPORTED (1u<<6) +#define AVB_AEM_DESC_STREAM_FLAG_SECONDARY_BACKUP_VALID (1u<<7) +#define AVB_AEM_DESC_STREAM_FLAG_TERTIARY_BACKUP_SUPPORTED (1u<<8) +#define AVB_AEM_DESC_STREAM_FLAG_TERTIARY_BACKUP_VALID (1u<<9) + +struct avb_aem_desc_stream { + char object_name[64]; + uint16_t localized_description; + uint16_t clock_domain_index; + uint16_t stream_flags; + uint64_t current_format; + uint16_t formats_offset; + uint16_t number_of_formats; + uint64_t backup_talker_entity_id_0; + uint16_t backup_talker_unique_id_0; + uint64_t backup_talker_entity_id_1; + uint16_t backup_talker_unique_id_1; + uint64_t backup_talker_entity_id_2; + uint16_t backup_talker_unique_id_2; + uint64_t backedup_talker_entity_id; + uint16_t backedup_talker_unique; + uint16_t avb_interface_index; + uint32_t buffer_length; + uint64_t stream_formats[0]; +} __attribute__ ((__packed__)); + +#define AVB_AEM_DESC_AVB_INTERFACE_FLAG_GPTP_GRANDMASTER_SUPPORTED (1<<0) +#define AVB_AEM_DESC_AVB_INTERFACE_FLAG_GPTP_SUPPORTED (1<<1) +#define AVB_AEM_DESC_AVB_INTERFACE_FLAG_SRP_SUPPORTED (1<<2) + +struct avb_aem_desc_avb_interface { + char object_name[64]; + uint16_t localized_description; + uint8_t mac_address[6]; + uint16_t interface_flags; + uint64_t clock_identity; + uint8_t priority1; + uint8_t clock_class; + uint16_t offset_scaled_log_variance; + uint8_t clock_accuracy; + uint8_t priority2; + uint8_t domain_number; + int8_t log_sync_interval; + int8_t log_announce_interval; + int8_t log_pdelay_interval; + uint16_t port_number; +} __attribute__ ((__packed__)); + +#define AVB_AEM_DESC_CLOCK_SOURCE_TYPE_INTERNAL 0x0000 +#define AVB_AEM_DESC_CLOCK_SOURCE_TYPE_EXTERNAL 0x0001 +#define AVB_AEM_DESC_CLOCK_SOURCE_TYPE_INPUT_STREAM 0x0002 +#define AVB_AEM_DESC_CLOCK_SOURCE_TYPE_MEDIA_CLOCK_STREAM 0x0003 +#define AVB_AEM_DESC_CLOCK_SOURCE_TYPE_EXPANSION 0xffff + +struct avb_aem_desc_clock_source { + char object_name[64]; + uint16_t localized_description; + uint16_t clock_source_flags; + uint16_t clock_source_type; + uint64_t clock_source_identifier; + uint16_t clock_source_location_type; + uint16_t clock_source_location_index; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_locale { + char locale_identifier[64]; + uint16_t number_of_strings; + uint16_t base_strings; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_strings { + char string_0[64]; + char string_1[64]; + char string_2[64]; + char string_3[64]; + char string_4[64]; + char string_5[64]; + char string_6[64]; +} __attribute__ ((__packed__)); + +struct avb_aem_desc_stream_port { + uint16_t clock_domain_index; + uint16_t port_flags; + uint16_t number_of_controls; + uint16_t base_control; + uint16_t number_of_clusters; + uint16_t base_cluster; + uint16_t number_of_maps; + uint16_t base_map; +} __attribute__ ((__packed__)); + +#endif /* AVB_AECP_AEM_DESCRIPTORS_H */ diff --git a/src/modules/module-avb/aecp-aem.c b/src/modules/module-avb/aecp-aem.c new file mode 100644 index 0000000..d191330 --- /dev/null +++ b/src/modules/module-avb/aecp-aem.c @@ -0,0 +1,285 @@ +/* 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 "aecp-aem.h" +#include "aecp-aem-descriptors.h" + +static int reply_status(struct aecp *aecp, int status, const void *m, int len) +{ + struct server *server = aecp->server; + uint8_t buf[len]; + struct avb_ethernet_header *h = (void*)buf; + struct avb_packet_aecp_header *reply = SPA_PTROFF(h, sizeof(*h), void); + + memcpy(buf, m, len); + AVB_PACKET_AECP_SET_MESSAGE_TYPE(reply, AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(reply, status); + + return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, len); +} + +static int reply_not_implemented(struct aecp *aecp, const void *m, int len) +{ + return reply_status(aecp, AVB_AECP_AEM_STATUS_NOT_IMPLEMENTED, m, len); +} + +static int reply_success(struct aecp *aecp, const void *m, int len) +{ + return reply_status(aecp, AVB_AECP_AEM_STATUS_SUCCESS, m, len); +} + +/* ACQUIRE_ENTITY */ +static int handle_acquire_entity(struct aecp *aecp, const void *m, int len) +{ + struct server *server = aecp->server; + const struct avb_packet_aecp_aem *p = m; + const struct avb_packet_aecp_aem_acquire *ae; + const struct descriptor *desc; + uint16_t desc_type, desc_id; + + ae = (const struct avb_packet_aecp_aem_acquire*)p->payload; + + desc_type = ntohs(ae->descriptor_type); + desc_id = ntohs(ae->descriptor_id); + + desc = server_find_descriptor(server, desc_type, desc_id); + if (desc == NULL) + return reply_status(aecp, AVB_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, p, len); + + if (desc_type != AVB_AEM_DESC_ENTITY || desc_id != 0) + return reply_not_implemented(aecp, m, len); + + return reply_success(aecp, m, len); +} + +/* LOCK_ENTITY */ +static int handle_lock_entity(struct aecp *aecp, const void *m, int len) +{ + struct server *server = aecp->server; + const struct avb_packet_aecp_aem *p = m; + const struct avb_packet_aecp_aem_acquire *ae; + const struct descriptor *desc; + uint16_t desc_type, desc_id; + + ae = (const struct avb_packet_aecp_aem_acquire*)p->payload; + + desc_type = ntohs(ae->descriptor_type); + desc_id = ntohs(ae->descriptor_id); + + desc = server_find_descriptor(server, desc_type, desc_id); + if (desc == NULL) + return reply_status(aecp, AVB_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, p, len); + + if (desc_type != AVB_AEM_DESC_ENTITY || desc_id != 0) + return reply_not_implemented(aecp, m, len); + + return reply_success(aecp, m, len); +} + +/* READ_DESCRIPTOR */ +static int handle_read_descriptor(struct aecp *aecp, const void *m, int len) +{ + struct server *server = aecp->server; + const struct avb_ethernet_header *h = m; + const struct avb_packet_aecp_aem *p = SPA_PTROFF(h, sizeof(*h), void); + struct avb_packet_aecp_aem *reply; + const struct avb_packet_aecp_aem_read_descriptor *rd; + uint16_t desc_type, desc_id; + const struct descriptor *desc; + uint8_t buf[2048]; + size_t size, psize; + + rd = (struct avb_packet_aecp_aem_read_descriptor*)p->payload; + + desc_type = ntohs(rd->descriptor_type); + desc_id = ntohs(rd->descriptor_id); + + pw_log_info("descriptor type:%04x index:%d", desc_type, desc_id); + + desc = server_find_descriptor(server, desc_type, desc_id); + if (desc == NULL) + return reply_status(aecp, AVB_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, m, len); + + memcpy(buf, m, len); + + psize = sizeof(*rd); + size = sizeof(*h) + sizeof(*reply) + psize; + + memcpy(buf + size, desc->ptr, desc->size); + size += desc->size; + psize += desc->size; + + h = (void*)buf; + reply = SPA_PTROFF(h, sizeof(*h), void); + AVB_PACKET_AECP_SET_MESSAGE_TYPE(&reply->aecp, AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(&reply->aecp, AVB_AECP_AEM_STATUS_SUCCESS); + AVB_PACKET_SET_LENGTH(&reply->aecp.hdr, psize + 12); + + return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, size); +} + +/* GET_AVB_INFO */ +static int handle_get_avb_info(struct aecp *aecp, const void *m, int len) +{ + struct server *server = aecp->server; + const struct avb_ethernet_header *h = m; + const struct avb_packet_aecp_aem *p = SPA_PTROFF(h, sizeof(*h), void); + struct avb_packet_aecp_aem *reply; + struct avb_packet_aecp_aem_get_avb_info *i; + struct avb_aem_desc_avb_interface *avb_interface; + uint16_t desc_type, desc_id; + const struct descriptor *desc; + uint8_t buf[2048]; + size_t size, psize; + + i = (struct avb_packet_aecp_aem_get_avb_info*)p->payload; + + desc_type = ntohs(i->descriptor_type); + desc_id = ntohs(i->descriptor_id); + + desc = server_find_descriptor(server, desc_type, desc_id); + if (desc == NULL) + return reply_status(aecp, AVB_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, m, len); + + if (desc_type != AVB_AEM_DESC_AVB_INTERFACE || desc_id != 0) + return reply_not_implemented(aecp, m, len); + + avb_interface = desc->ptr; + + memcpy(buf, m, len); + + psize = sizeof(*i); + size = sizeof(*h) + sizeof(*reply) + psize; + + h = (void*)buf; + reply = SPA_PTROFF(h, sizeof(*h), void); + AVB_PACKET_AECP_SET_MESSAGE_TYPE(&reply->aecp, AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(&reply->aecp, AVB_AECP_AEM_STATUS_SUCCESS); + AVB_PACKET_SET_LENGTH(&reply->aecp.hdr, psize + 12); + + i = (struct avb_packet_aecp_aem_get_avb_info*)reply->payload; + i->gptp_grandmaster_id = avb_interface->clock_identity; + i->propagation_delay = htonl(0); + i->gptp_domain_number = avb_interface->domain_number; + i->flags = 0; + i->msrp_mappings_count = htons(0); + + return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, size); +} + +/* AEM_COMMAND */ +struct cmd_info { + uint16_t type; + const char *name; + int (*handle) (struct aecp *aecp, const void *p, int len); +}; + +static const struct cmd_info cmd_info[] = { + { AVB_AECP_AEM_CMD_ACQUIRE_ENTITY, "acquire-entity", handle_acquire_entity, }, + { AVB_AECP_AEM_CMD_LOCK_ENTITY, "lock-entity", handle_lock_entity, }, + { AVB_AECP_AEM_CMD_ENTITY_AVAILABLE, "entity-available", NULL, }, + { AVB_AECP_AEM_CMD_CONTROLLER_AVAILABLE, "controller-available", NULL, }, + { AVB_AECP_AEM_CMD_READ_DESCRIPTOR, "read-descriptor", handle_read_descriptor, }, + { AVB_AECP_AEM_CMD_WRITE_DESCRIPTOR, "write-descriptor", NULL, }, + { AVB_AECP_AEM_CMD_SET_CONFIGURATION, "set-configuration", NULL, }, + { AVB_AECP_AEM_CMD_GET_CONFIGURATION, "get-configuration", NULL, }, + { AVB_AECP_AEM_CMD_SET_STREAM_FORMAT, "set-stream-format", NULL, }, + { AVB_AECP_AEM_CMD_GET_STREAM_FORMAT, "get-stream-format", NULL, }, + { AVB_AECP_AEM_CMD_SET_VIDEO_FORMAT, "set-video-format", NULL, }, + { AVB_AECP_AEM_CMD_GET_VIDEO_FORMAT, "get-video-format", NULL, }, + { AVB_AECP_AEM_CMD_SET_SENSOR_FORMAT, "set-sensor-format", NULL, }, + { AVB_AECP_AEM_CMD_GET_SENSOR_FORMAT, "get-sensor-format", NULL, }, + { AVB_AECP_AEM_CMD_SET_STREAM_INFO, "set-stream-info", NULL, }, + { AVB_AECP_AEM_CMD_GET_STREAM_INFO, "get-stream-info", NULL, }, + { AVB_AECP_AEM_CMD_SET_NAME, "set-name", NULL, }, + { AVB_AECP_AEM_CMD_GET_NAME, "get-name", NULL, }, + { AVB_AECP_AEM_CMD_SET_ASSOCIATION_ID, "set-association-id", NULL, }, + { AVB_AECP_AEM_CMD_GET_ASSOCIATION_ID, "get-association-id", NULL, }, + { AVB_AECP_AEM_CMD_SET_SAMPLING_RATE, "set-sampling-rate", NULL, }, + { AVB_AECP_AEM_CMD_GET_SAMPLING_RATE, "get-sampling-rate", NULL, }, + { AVB_AECP_AEM_CMD_SET_CLOCK_SOURCE, "set-clock-source", NULL, }, + { AVB_AECP_AEM_CMD_GET_CLOCK_SOURCE, "get-clock-source", NULL, }, + { AVB_AECP_AEM_CMD_SET_CONTROL, "set-control", NULL, }, + { AVB_AECP_AEM_CMD_GET_CONTROL, "get-control", NULL, }, + { AVB_AECP_AEM_CMD_INCREMENT_CONTROL, "increment-control", NULL, }, + { AVB_AECP_AEM_CMD_DECREMENT_CONTROL, "decrement-control", NULL, }, + { AVB_AECP_AEM_CMD_SET_SIGNAL_SELECTOR, "set-signal-selector", NULL, }, + { AVB_AECP_AEM_CMD_GET_SIGNAL_SELECTOR, "get-signal-selector", NULL, }, + { AVB_AECP_AEM_CMD_SET_MIXER, "set-mixer", NULL, }, + { AVB_AECP_AEM_CMD_GET_MIXER, "get-mixer", NULL, }, + { AVB_AECP_AEM_CMD_SET_MATRIX, "set-matrix", NULL, }, + { AVB_AECP_AEM_CMD_GET_MATRIX, "get-matrix", NULL, }, + { AVB_AECP_AEM_CMD_START_STREAMING, "start-streaming", NULL, }, + { AVB_AECP_AEM_CMD_STOP_STREAMING, "stop-streaming", NULL, }, + { AVB_AECP_AEM_CMD_REGISTER_UNSOLICITED_NOTIFICATION, "register-unsolicited-notification", NULL, }, + { AVB_AECP_AEM_CMD_DEREGISTER_UNSOLICITED_NOTIFICATION, "deregister-unsolicited-notification", NULL, }, + { AVB_AECP_AEM_CMD_IDENTIFY_NOTIFICATION, "identify-notification", NULL, }, + { AVB_AECP_AEM_CMD_GET_AVB_INFO, "get-avb-info", handle_get_avb_info, }, + { AVB_AECP_AEM_CMD_GET_AS_PATH, "get-as-path", NULL, }, + { AVB_AECP_AEM_CMD_GET_COUNTERS, "get-counters", NULL, }, + { AVB_AECP_AEM_CMD_REBOOT, "reboot", NULL, }, + { AVB_AECP_AEM_CMD_GET_AUDIO_MAP, "get-audio-map", NULL, }, + { AVB_AECP_AEM_CMD_ADD_AUDIO_MAPPINGS, "add-audio-mappings", NULL, }, + { AVB_AECP_AEM_CMD_REMOVE_AUDIO_MAPPINGS, "remove-audio-mappings", NULL, }, + { AVB_AECP_AEM_CMD_GET_VIDEO_MAP, "get-video-map", NULL, }, + { AVB_AECP_AEM_CMD_ADD_VIDEO_MAPPINGS, "add-video-mappings", NULL, }, + { AVB_AECP_AEM_CMD_REMOVE_VIDEO_MAPPINGS, "remove-video-mappings", NULL, }, + { AVB_AECP_AEM_CMD_GET_SENSOR_MAP, "get-sensor-map", NULL, } +}; + +static inline const struct cmd_info *find_cmd_info(uint16_t type, const char *name) +{ + SPA_FOR_EACH_ELEMENT_VAR(cmd_info, i) { + if ((name == NULL && type == i->type) || + (name != NULL && spa_streq(name, i->name))) + return i; + } + return NULL; +} + +int avb_aecp_aem_handle_command(struct aecp *aecp, const void *m, int len) +{ + const struct avb_ethernet_header *h = m; + const struct avb_packet_aecp_aem *p = SPA_PTROFF(h, sizeof(*h), void); + uint16_t cmd_type; + const struct cmd_info *info; + + cmd_type = AVB_PACKET_AEM_GET_COMMAND_TYPE(p); + + info = find_cmd_info(cmd_type, NULL); + if (info == NULL) + return reply_not_implemented(aecp, m, len); + + pw_log_info("aem command %s", info->name); + + if (info->handle == NULL) + return reply_not_implemented(aecp, m, len); + + return info->handle(aecp, m, len); +} + +int avb_aecp_aem_handle_response(struct aecp *aecp, const void *m, int len) +{ + return 0; +} diff --git a/src/modules/module-avb/aecp-aem.h b/src/modules/module-avb/aecp-aem.h new file mode 100644 index 0000000..dcf26b5 --- /dev/null +++ b/src/modules/module-avb/aecp-aem.h @@ -0,0 +1,345 @@ +/* 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 AVB_AEM_H +#define AVB_AEM_H + +#include "aecp.h" + +#define AVB_AECP_AEM_STATUS_SUCCESS 0 +#define AVB_AECP_AEM_STATUS_NOT_IMPLEMENTED 1 +#define AVB_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR 2 +#define AVB_AECP_AEM_STATUS_ENTITY_LOCKED 3 +#define AVB_AECP_AEM_STATUS_ENTITY_ACQUIRED 4 +#define AVB_AECP_AEM_STATUS_NOT_AUTHENTICATED 5 +#define AVB_AECP_AEM_STATUS_AUTHENTICATION_DISABLED 6 +#define AVB_AECP_AEM_STATUS_BAD_ARGUMENTS 7 +#define AVB_AECP_AEM_STATUS_NO_RESOURCES 8 +#define AVB_AECP_AEM_STATUS_IN_PROGRESS 9 +#define AVB_AECP_AEM_STATUS_ENTITY_MISBEHAVING 10 +#define AVB_AECP_AEM_STATUS_NOT_SUPPORTED 11 +#define AVB_AECP_AEM_STATUS_STREAM_IS_RUNNING 12 + +#define AVB_AECP_AEM_CMD_ACQUIRE_ENTITY 0x0000 +#define AVB_AECP_AEM_CMD_LOCK_ENTITY 0x0001 +#define AVB_AECP_AEM_CMD_ENTITY_AVAILABLE 0x0002 +#define AVB_AECP_AEM_CMD_CONTROLLER_AVAILABLE 0x0003 +#define AVB_AECP_AEM_CMD_READ_DESCRIPTOR 0x0004 +#define AVB_AECP_AEM_CMD_WRITE_DESCRIPTOR 0x0005 +#define AVB_AECP_AEM_CMD_SET_CONFIGURATION 0x0006 +#define AVB_AECP_AEM_CMD_GET_CONFIGURATION 0x0007 +#define AVB_AECP_AEM_CMD_SET_STREAM_FORMAT 0x0008 +#define AVB_AECP_AEM_CMD_GET_STREAM_FORMAT 0x0009 +#define AVB_AECP_AEM_CMD_SET_VIDEO_FORMAT 0x000a +#define AVB_AECP_AEM_CMD_GET_VIDEO_FORMAT 0x000b +#define AVB_AECP_AEM_CMD_SET_SENSOR_FORMAT 0x000c +#define AVB_AECP_AEM_CMD_GET_SENSOR_FORMAT 0x000d +#define AVB_AECP_AEM_CMD_SET_STREAM_INFO 0x000e +#define AVB_AECP_AEM_CMD_GET_STREAM_INFO 0x000f +#define AVB_AECP_AEM_CMD_SET_NAME 0x0010 +#define AVB_AECP_AEM_CMD_GET_NAME 0x0011 +#define AVB_AECP_AEM_CMD_SET_ASSOCIATION_ID 0x0012 +#define AVB_AECP_AEM_CMD_GET_ASSOCIATION_ID 0x0013 +#define AVB_AECP_AEM_CMD_SET_SAMPLING_RATE 0x0014 +#define AVB_AECP_AEM_CMD_GET_SAMPLING_RATE 0x0015 +#define AVB_AECP_AEM_CMD_SET_CLOCK_SOURCE 0x0016 +#define AVB_AECP_AEM_CMD_GET_CLOCK_SOURCE 0x0017 +#define AVB_AECP_AEM_CMD_SET_CONTROL 0x0018 +#define AVB_AECP_AEM_CMD_GET_CONTROL 0x0019 +#define AVB_AECP_AEM_CMD_INCREMENT_CONTROL 0x001a +#define AVB_AECP_AEM_CMD_DECREMENT_CONTROL 0x001b +#define AVB_AECP_AEM_CMD_SET_SIGNAL_SELECTOR 0x001c +#define AVB_AECP_AEM_CMD_GET_SIGNAL_SELECTOR 0x001d +#define AVB_AECP_AEM_CMD_SET_MIXER 0x001e +#define AVB_AECP_AEM_CMD_GET_MIXER 0x001f +#define AVB_AECP_AEM_CMD_SET_MATRIX 0x0020 +#define AVB_AECP_AEM_CMD_GET_MATRIX 0x0021 +#define AVB_AECP_AEM_CMD_START_STREAMING 0x0022 +#define AVB_AECP_AEM_CMD_STOP_STREAMING 0x0023 +#define AVB_AECP_AEM_CMD_REGISTER_UNSOLICITED_NOTIFICATION 0x0024 +#define AVB_AECP_AEM_CMD_DEREGISTER_UNSOLICITED_NOTIFICATION 0x0025 +#define AVB_AECP_AEM_CMD_IDENTIFY_NOTIFICATION 0x0026 +#define AVB_AECP_AEM_CMD_GET_AVB_INFO 0x0027 +#define AVB_AECP_AEM_CMD_GET_AS_PATH 0x0028 +#define AVB_AECP_AEM_CMD_GET_COUNTERS 0x0029 +#define AVB_AECP_AEM_CMD_REBOOT 0x002a +#define AVB_AECP_AEM_CMD_GET_AUDIO_MAP 0x002b +#define AVB_AECP_AEM_CMD_ADD_AUDIO_MAPPINGS 0x002c +#define AVB_AECP_AEM_CMD_REMOVE_AUDIO_MAPPINGS 0x002d +#define AVB_AECP_AEM_CMD_GET_VIDEO_MAP 0x002e +#define AVB_AECP_AEM_CMD_ADD_VIDEO_MAPPINGS 0x002f +#define AVB_AECP_AEM_CMD_REMOVE_VIDEO_MAPPINGS 0x0030 +#define AVB_AECP_AEM_CMD_GET_SENSOR_MAP 0x0031 +#define AVB_AECP_AEM_CMD_ADD_SENSOR_MAPPINGS 0x0032 +#define AVB_AECP_AEM_CMD_REMOVE_SENSOR_MAPPINGS 0x0033 +#define AVB_AECP_AEM_CMD_START_OPERATION 0x0034 +#define AVB_AECP_AEM_CMD_ABORT_OPERATION 0x0035 +#define AVB_AECP_AEM_CMD_OPERATION_STATUS 0x0036 +#define AVB_AECP_AEM_CMD_AUTH_ADD_KEY 0x0037 +#define AVB_AECP_AEM_CMD_AUTH_DELETE_KEY 0x0038 +#define AVB_AECP_AEM_CMD_AUTH_GET_KEY_LIST 0x0039 +#define AVB_AECP_AEM_CMD_AUTH_GET_KEY 0x003a +#define AVB_AECP_AEM_CMD_AUTH_ADD_KEY_TO_CHAIN 0x003b +#define AVB_AECP_AEM_CMD_AUTH_DELETE_KEY_FROM_CHAIN 0x003c +#define AVB_AECP_AEM_CMD_AUTH_GET_KEYCHAIN_LIST 0x003d +#define AVB_AECP_AEM_CMD_AUTH_GET_IDENTITY 0x003e +#define AVB_AECP_AEM_CMD_AUTH_ADD_TOKEN 0x003f +#define AVB_AECP_AEM_CMD_AUTH_DELETE_TOKEN 0x0040 +#define AVB_AECP_AEM_CMD_AUTHENTICATE 0x0041 +#define AVB_AECP_AEM_CMD_DEAUTHENTICATE 0x0042 +#define AVB_AECP_AEM_CMD_ENABLE_TRANSPORT_SECURITY 0x0043 +#define AVB_AECP_AEM_CMD_DISABLE_TRANSPORT_SECURITY 0x0044 +#define AVB_AECP_AEM_CMD_ENABLE_STREAM_ENCRYPTION 0x0045 +#define AVB_AECP_AEM_CMD_DISABLE_STREAM_ENCRYPTION 0x0046 +#define AVB_AECP_AEM_CMD_SET_MEMORY_OBJECT_LENGTH 0x0047 +#define AVB_AECP_AEM_CMD_GET_MEMORY_OBJECT_LENGTH 0x0048 +#define AVB_AECP_AEM_CMD_SET_STREAM_BACKUP 0x0049 +#define AVB_AECP_AEM_CMD_GET_STREAM_BACKUP 0x004a +#define AVB_AECP_AEM_CMD_EXPANSION 0x7fff + +#define AVB_AEM_ACQUIRE_ENTITY_PERSISTENT_FLAG (1<<0) + +struct avb_packet_aecp_aem_acquire { + uint32_t flags; + uint64_t owner_guid; + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_lock { + uint32_t flags; + uint64_t locked_guid; + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_read_descriptor { + uint16_t configuration; + uint8_t reserved[2]; + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_configuration { + uint16_t reserved; + uint16_t configuration_index; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_stream_format { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint64_t stream_format; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_video_format { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint32_t format_specific; + uint16_t aspect_ratio; + uint16_t color_space; + uint32_t frame_size; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_sensor_format { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint64_t sensor_format; +} __attribute__ ((__packed__)); + + +#define AVB_AEM_STREAM_INFO_FLAG_CLASS_B (1u<<0) +#define AVB_AEM_STREAM_INFO_FLAG_FAST_CONNECT (1u<<1) +#define AVB_AEM_STREAM_INFO_FLAG_SAVED_STATE (1u<<2) +#define AVB_AEM_STREAM_INFO_FLAG_STREAMING_WAIT (1u<<3) +#define AVB_AEM_STREAM_INFO_FLAG_ENCRYPTED_PDU (1u<<4) +#define AVB_AEM_STREAM_INFO_FLAG_STREAM_VLAN_ID_VALID (1u<<25) +#define AVB_AEM_STREAM_INFO_FLAG_CONNECTED (1u<<26) +#define AVB_AEM_STREAM_INFO_FLAG_MSRP_FAILURE_VALID (1u<<27) +#define AVB_AEM_STREAM_INFO_FLAG_STREAM_DEST_MAC_VALID (1u<<28) +#define AVB_AEM_STREAM_INFO_FLAG_MSRP_ACC_LAT_VALID (1u<<29) +#define AVB_AEM_STREAM_INFO_FLAG_STREAM_ID_VALID (1u<<30) +#define AVB_AEM_STREAM_INFO_FLAG_STREAM_FORMAT_VALID (1u<<31) + +struct avb_packet_aecp_aem_setget_stream_info { + uint16_t descriptor_type; + uint16_t descriptor_index; + uint32_t aem_stream_info_flags; + uint64_t stream_format; + uint64_t stream_id; + uint32_t msrp_accumulated_latency; + uint8_t stream_dest_mac[6]; + uint8_t msrp_failure_code; + uint8_t reserved; + uint64_t msrp_failure_bridge_id; + uint16_t stream_vlan_id; + uint16_t reserved2; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_name { + uint16_t descriptor_type; + uint16_t descriptor_index; + uint16_t name_index; + uint16_t configuration_index; + char name[64]; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_association_id { + uint16_t descriptor_type; + uint16_t descriptor_index; + uint64_t association_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_sampling_rate { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint32_t sampling_rate; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_clock_source { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t clock_source_index; + uint16_t reserved; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_control { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_incdec_control { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t index_count; + uint16_t reserved; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_signal_selector { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t signal_type; + uint16_t signal_index; + uint16_t signal_output; + uint16_t reserved; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_mixer { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_setget_matrix { + uint16_t descriptor_type; + uint16_t descriptor_index; + uint16_t matrix_column; + uint16_t matrix_row; + uint16_t region_width; + uint16_t region_height; + uint16_t rep_direction_value_count; + uint16_t item_offset; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_startstop_streaming { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_identify_notification { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_msrp_mapping { + uint8_t traffic_class; + uint8_t priority; + uint16_t vlan_id; +} __attribute__ ((__packed__)); + +#define AVB_AEM_AVB_INFO_FLAG_GPTP_GRANDMASTER_SUPPORTED (1u<<0) +#define AVB_AEM_AVB_INFO_FLAG_GPTP_ENABLED (1u<<1) +#define AVB_AEM_AVB_INFO_FLAG_SRP_ENABLED (1u<<2) + +struct avb_packet_aecp_aem_get_avb_info { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint64_t gptp_grandmaster_id; + uint32_t propagation_delay; + uint8_t gptp_domain_number; + uint8_t flags; + uint16_t msrp_mappings_count; + uint8_t msrp_mappings[0]; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_get_as_path { + uint16_t descriptor_index; + uint16_t reserved; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_get_counters { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint32_t counters_valid; + uint8_t counters_block[0]; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_reboot { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_start_operation { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t operation_id; + uint16_t operation_type; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem_operation_status { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t operation_id; + uint16_t percent_complete; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_aem { + struct avb_packet_aecp_header aecp; +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned u:1; + unsigned cmd1:7; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned cmd1:7; + unsigned u:1; +#endif + uint8_t cmd2; + uint8_t payload[0]; +} __attribute__ ((__packed__)); + +#define AVB_PACKET_AEM_SET_COMMAND_TYPE(p,v) ((p)->cmd1 = ((v) >> 8),(p)->cmd2 = (v)) + +#define AVB_PACKET_AEM_GET_COMMAND_TYPE(p) ((p)->cmd1 << 8 | (p)->cmd2) + +int avb_aecp_aem_handle_command(struct aecp *aecp, const void *m, int len); +int avb_aecp_aem_handle_response(struct aecp *aecp, const void *m, int len); + +#endif /* AVB_AEM_H */ diff --git a/src/modules/module-avb/aecp.c b/src/modules/module-avb/aecp.c new file mode 100644 index 0000000..d581f81 --- /dev/null +++ b/src/modules/module-avb/aecp.c @@ -0,0 +1,168 @@ +/* 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 <spa/utils/json.h> +#include <spa/debug/mem.h> + +#include <pipewire/pipewire.h> + +#include "aecp.h" +#include "aecp-aem.h" +#include "internal.h" + +static const uint8_t mac[6] = AVB_BROADCAST_MAC; + +struct msg_info { + uint16_t type; + const char *name; + int (*handle) (struct aecp *aecp, const void *p, int len); +}; + +static int reply_not_implemented(struct aecp *aecp, const void *p, int len) +{ + struct server *server = aecp->server; + uint8_t buf[len]; + struct avb_ethernet_header *h = (void*)buf; + struct avb_packet_aecp_header *reply = SPA_PTROFF(h, sizeof(*h), void); + + memcpy(h, p, len); + AVB_PACKET_AECP_SET_STATUS(reply, AVB_AECP_STATUS_NOT_IMPLEMENTED); + + return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, len); +} + +static const struct msg_info msg_info[] = { + { AVB_AECP_MESSAGE_TYPE_AEM_COMMAND, "aem-command", avb_aecp_aem_handle_command, }, + { AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE, "aem-response", avb_aecp_aem_handle_response, }, + { AVB_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_COMMAND, "address-access-command", NULL, }, + { AVB_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_RESPONSE, "address-access-response", NULL, }, + { AVB_AECP_MESSAGE_TYPE_AVC_COMMAND, "avc-command", NULL, }, + { AVB_AECP_MESSAGE_TYPE_AVC_RESPONSE, "avc-response", NULL, }, + { AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_COMMAND, "vendor-unique-command", NULL, }, + { AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE, "vendor-unique-response", NULL, }, + { AVB_AECP_MESSAGE_TYPE_EXTENDED_COMMAND, "extended-command", NULL, }, + { AVB_AECP_MESSAGE_TYPE_EXTENDED_RESPONSE, "extended-response", NULL, }, +}; + +static inline const struct msg_info *find_msg_info(uint16_t type, const char *name) +{ + SPA_FOR_EACH_ELEMENT_VAR(msg_info, i) { + if ((name == NULL && type == i->type) || + (name != NULL && spa_streq(name, i->name))) + return i; + } + return NULL; +} + +static int aecp_message(void *data, uint64_t now, const void *message, int len) +{ + struct aecp *aecp = data; + struct server *server = aecp->server; + const struct avb_ethernet_header *h = message; + const struct avb_packet_aecp_header *p = SPA_PTROFF(h, sizeof(*h), void); + const struct msg_info *info; + int message_type; + + if (ntohs(h->type) != AVB_TSN_ETH) + return 0; + if (memcmp(h->dest, mac, 6) != 0 && + memcmp(h->dest, server->mac_addr, 6) != 0) + return 0; + if (AVB_PACKET_GET_SUBTYPE(&p->hdr) != AVB_SUBTYPE_AECP) + return 0; + + message_type = AVB_PACKET_AECP_GET_MESSAGE_TYPE(p); + + info = find_msg_info(message_type, NULL); + if (info == NULL) + return reply_not_implemented(aecp, message, len); + + pw_log_debug("got AECP message %s", info->name); + + if (info->handle == NULL) + return reply_not_implemented(aecp, message, len); + + return info->handle(aecp, message, len); +} + +static void aecp_destroy(void *data) +{ + struct aecp *aecp = data; + spa_hook_remove(&aecp->server_listener); + free(aecp); +} + +static int do_help(struct aecp *aecp, const char *args, FILE *out) +{ + fprintf(out, "{ \"type\": \"help\"," + "\"text\": \"" + "/adp/help: this help \\n" + "\" }"); + return 0; +} + +static int aecp_command(void *data, uint64_t now, const char *command, const char *args, FILE *out) +{ + struct aecp *aecp = data; + int res; + + if (!spa_strstartswith(command, "/aecp/")) + return 0; + + command += strlen("/aecp/"); + + if (spa_streq(command, "help")) + res = do_help(aecp, args, out); + else + res = -ENOTSUP; + + return res; +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = aecp_destroy, + .message = aecp_message, + .command = aecp_command +}; + +struct avb_aecp *avb_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 avb_aecp*)aecp; +} + +void avb_aecp_unregister(struct avb_aecp *aecp) +{ + aecp_destroy(aecp); +} diff --git a/src/modules/module-avb/aecp.h b/src/modules/module-avb/aecp.h new file mode 100644 index 0000000..a3515f0 --- /dev/null +++ b/src/modules/module-avb/aecp.h @@ -0,0 +1,60 @@ +/* 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 AVB_AECP_H +#define AVB_AECP_H + +#include "packets.h" +#include "internal.h" + +#define AVB_AECP_MESSAGE_TYPE_AEM_COMMAND 0 +#define AVB_AECP_MESSAGE_TYPE_AEM_RESPONSE 1 +#define AVB_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_COMMAND 2 +#define AVB_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_RESPONSE 3 +#define AVB_AECP_MESSAGE_TYPE_AVC_COMMAND 4 +#define AVB_AECP_MESSAGE_TYPE_AVC_RESPONSE 5 +#define AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_COMMAND 6 +#define AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE 7 +#define AVB_AECP_MESSAGE_TYPE_EXTENDED_COMMAND 14 +#define AVB_AECP_MESSAGE_TYPE_EXTENDED_RESPONSE 15 + +#define AVB_AECP_STATUS_SUCCESS 0 +#define AVB_AECP_STATUS_NOT_IMPLEMENTED 1 + +struct avb_packet_aecp_header { + struct avb_packet_header hdr; + uint64_t target_guid; + uint64_t controller_guid; + uint16_t sequence_id; +} __attribute__ ((__packed__)); + +#define AVB_PACKET_AECP_SET_MESSAGE_TYPE(p,v) AVB_PACKET_SET_SUB1(&(p)->hdr, v) +#define AVB_PACKET_AECP_SET_STATUS(p,v) AVB_PACKET_SET_SUB2(&(p)->hdr, v) + +#define AVB_PACKET_AECP_GET_MESSAGE_TYPE(p) AVB_PACKET_GET_SUB1(&(p)->hdr) +#define AVB_PACKET_AECP_GET_STATUS(p) AVB_PACKET_GET_SUB2(&(p)->hdr) + +struct avb_aecp *avb_aecp_register(struct server *server); + +#endif /* AVB_AECP_H */ diff --git a/src/modules/module-avb/avb.c b/src/modules/module-avb/avb.c new file mode 100644 index 0000000..2afdc21 --- /dev/null +++ b/src/modules/module-avb/avb.c @@ -0,0 +1,106 @@ +/* PipeWire + * + * 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 "internal.h" + +#include <spa/support/cpu.h> + +struct pw_avb *pw_avb_new(struct pw_context *context, + struct pw_properties *props, size_t user_data_size) +{ + struct impl *impl; + const struct spa_support *support; + uint32_t n_support; + struct spa_cpu *cpu; + const char *str; + int res = 0; + + impl = calloc(1, sizeof(*impl) + user_data_size); + if (impl == NULL) + goto error_exit; + + if (props == NULL) + props = pw_properties_new(NULL, NULL); + if (props == NULL) + goto error_free; + + support = pw_context_get_support(context, &n_support); + cpu = spa_support_find(support, n_support, SPA_TYPE_INTERFACE_CPU); + + pw_context_conf_update_props(context, "avb.properties", props); + + if ((str = pw_properties_get(props, "vm.overrides")) != NULL) { + if (cpu != NULL && spa_cpu_get_vm_type(cpu) != SPA_CPU_VM_NONE) + pw_properties_update_string(props, str, strlen(str)); + pw_properties_set(props, "vm.overrides", NULL); + } + + impl->context = context; + impl->loop = pw_context_get_main_loop(context); + impl->props = props; + impl->core = pw_context_get_object(context, PW_TYPE_INTERFACE_Core); + if (impl->core == NULL) { + str = pw_properties_get(props, PW_KEY_REMOTE_NAME); + impl->core = pw_context_connect(context, + pw_properties_new( + PW_KEY_REMOTE_NAME, str, + NULL), + 0); + impl->do_disconnect = true; + } + if (impl->core == NULL) { + res = -errno; + pw_log_error("can't connect: %m"); + goto error_free; + } + + spa_list_init(&impl->servers); + + avdecc_server_new(impl, &props->dict); + + return (struct pw_avb*)impl; + +error_free: + free(impl); +error_exit: + pw_properties_free(props); + if (res < 0) + errno = -res; + return NULL; +} + +static void impl_free(struct impl *impl) +{ + struct server *s; + + spa_list_consume(s, &impl->servers, link) + avdecc_server_free(s); + free(impl); +} + +void pw_avb_destroy(struct pw_avb *avb) +{ + struct impl *impl = (struct impl*)avb; + impl_free(impl); +} diff --git a/src/modules/module-avb/avb.h b/src/modules/module-avb/avb.h new file mode 100644 index 0000000..cad7dd2 --- /dev/null +++ b/src/modules/module-avb/avb.h @@ -0,0 +1,44 @@ +/* PipeWire + * + * 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 PIPEWIRE_AVB_H +#define PIPEWIRE_AVB_H + +#ifdef __cplusplus +extern "C" { +#endif + +struct pw_context; +struct pw_properties; +struct pw_avb; + +struct pw_avb *pw_avb_new(struct pw_context *context, + struct pw_properties *props, size_t user_data_size); +void pw_avb_destroy(struct pw_avb *avb); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* PIPEWIRE_AVB_H */ diff --git a/src/modules/module-avb/avdecc.c b/src/modules/module-avb/avdecc.c new file mode 100644 index 0000000..308ba48 --- /dev/null +++ b/src/modules/module-avb/avdecc.c @@ -0,0 +1,335 @@ +/* PipeWire + * + * 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 <linux/if_ether.h> +#include <linux/if_packet.h> +#include <linux/filter.h> +#include <linux/net_tstamp.h> +#include <limits.h> +#include <net/if.h> +#include <arpa/inet.h> +#include <sys/ioctl.h> +#include <unistd.h> + +#include <spa/support/cpu.h> +#include <spa/debug/mem.h> + +#include <pipewire/pipewire.h> + +#include "avb.h" +#include "packets.h" +#include "internal.h" +#include "stream.h" +#include "acmp.h" +#include "adp.h" +#include "aecp.h" +#include "maap.h" +#include "mmrp.h" +#include "msrp.h" +#include "mvrp.h" +#include "descriptors.h" +#include "utils.h" + +#define DEFAULT_INTERVAL 1 + +#define server_emit(s,m,v,...) spa_hook_list_call(&s->listener_list, struct server_events, m, v, ##__VA_ARGS__) +#define server_emit_destroy(s) server_emit(s, destroy, 0) +#define server_emit_message(s,n,m,l) server_emit(s, message, 0, n, m, l) +#define server_emit_periodic(s,n) server_emit(s, periodic, 0, n) +#define server_emit_command(s,n,c,a,f) server_emit(s, command, 0, n, c, a, f) + +static void on_timer_event(void *data, uint64_t expirations) +{ + struct server *server = data; + struct timespec now; + clock_gettime(CLOCK_REALTIME, &now); + server_emit_periodic(server, SPA_TIMESPEC_TO_NSEC(&now)); +} + +static void on_socket_data(void *data, int fd, uint32_t mask) +{ + struct server *server = 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); + server_emit_message(server, SPA_TIMESPEC_TO_NSEC(&now), buffer, len); + } + } +} + +int avb_server_send_packet(struct server *server, const uint8_t dest[6], + uint16_t type, void *data, size_t size) +{ + struct avb_ethernet_header *hdr = (struct avb_ethernet_header*)data; + int res = 0; + + memcpy(hdr->dest, dest, ETH_ALEN); + memcpy(hdr->src, server->mac_addr, ETH_ALEN); + hdr->type = htons(type); + + if (send(server->source->fd, data, size, 0) < 0) { + res = -errno; + pw_log_warn("got send error: %m"); + } + 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; +} + +int avb_server_make_socket(struct server *server, uint16_t type, const uint8_t mac[6]) +{ + int fd, res; + struct ifreq req; + struct packet_mreq mreq; + struct sockaddr_ll sll; + + fd = socket(AF_PACKET, SOCK_RAW|SOCK_NONBLOCK, htons(ETH_P_ALL)); + if (fd < 0) { + pw_log_error("socket() failed: %m"); + return -errno; + } + + spa_zero(req); + snprintf(req.ifr_name, sizeof(req.ifr_name), "%s", server->ifname); + if (ioctl(fd, SIOCGIFINDEX, &req) < 0) { + res = -errno; + pw_log_error("SIOCGIFINDEX %s failed: %m", server->ifname); + goto error_close; + } + server->ifindex = req.ifr_ifindex; + + spa_zero(req); + snprintf(req.ifr_name, sizeof(req.ifr_name), "%s", server->ifname); + if (ioctl(fd, SIOCGIFHWADDR, &req) < 0) { + res = -errno; + pw_log_error("SIOCGIFHWADDR %s failed: %m", server->ifname); + goto error_close; + } + 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]; + + 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"); + goto error_close; + } + + spa_zero(mreq); + mreq.mr_ifindex = server->ifindex; + mreq.mr_type = PACKET_MR_MULTICAST; + mreq.mr_alen = ETH_ALEN; + memcpy(mreq.mr_address, mac, ETH_ALEN); + + if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, + &mreq, sizeof(mreq)) < 0) { + res = -errno; + pw_log_error("setsockopt(ADD_MEMBERSHIP) failed: %m"); + goto error_close; + } + + 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("0x%"PRIx64" %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_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_no_timer; + } + value.tv_sec = 0; + value.tv_nsec = 1; + interval.tv_sec = DEFAULT_INTERVAL; + interval.tv_nsec = 0; + pw_loop_update_timer(impl->loop, server->timer, &value, &interval, false); + + return 0; + +error_no_timer: + pw_loop_destroy_source(impl->loop, server->source); + server->source = NULL; +error_no_source: + close(fd); + return res; +} + +struct server *avdecc_server_new(struct impl *impl, struct spa_dict *props) +{ + struct server *server; + int res = 0; + + server = calloc(1, sizeof(*server)); + if (server == NULL) + return NULL; + + server->impl = impl; + spa_list_append(&impl->servers, &server->link); + server->ifname = strdup(spa_dict_lookup(props, "ifname")); + spa_hook_list_init(&server->listener_list); + spa_list_init(&server->descriptors); + spa_list_init(&server->streams); + + server->debug_messages = false; + + if ((res = setup_socket(server)) < 0) + goto error_free; + + init_descriptors(server); + + server->mrp = avb_mrp_new(server); + if (server->mrp == NULL) + goto error_free; + + avb_aecp_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); + avb_adp_register(server); + avb_acmp_register(server); + + server->domain_attr = avb_msrp_attribute_new(server->msrp, + AVB_MSRP_ATTRIBUTE_TYPE_DOMAIN); + server->domain_attr->attr.domain.sr_class_id = AVB_MSRP_CLASS_ID_DEFAULT; + server->domain_attr->attr.domain.sr_class_priority = AVB_MSRP_PRIORITY_DEFAULT; + server->domain_attr->attr.domain.sr_class_vid = htons(AVB_DEFAULT_VLAN); + + avb_mrp_attribute_begin(server->domain_attr->mrp, 0); + avb_mrp_attribute_join(server->domain_attr->mrp, 0, true); + + 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: + free(server); + if (res < 0) + errno = -res; + return NULL; +} + +void avdecc_server_add_listener(struct server *server, struct spa_hook *listener, + const struct server_events *events, void *data) +{ + spa_hook_list_append(&server->listener_list, listener, events, data); +} + +void avdecc_server_free(struct server *server) +{ + struct impl *impl = server->impl; + + spa_list_remove(&server->link); + if (server->source) + pw_loop_destroy_source(impl->loop, server->source); + if (server->timer) + pw_loop_destroy_source(impl->loop, server->source); + spa_hook_list_clean(&server->listener_list); + free(server); +} diff --git a/src/modules/module-avb/descriptors.h b/src/modules/module-avb/descriptors.h new file mode 100644 index 0000000..56397e3 --- /dev/null +++ b/src/modules/module-avb/descriptors.h @@ -0,0 +1,274 @@ +/* PipeWire + * + * 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 "aecp-aem.h" +#include "aecp-aem-descriptors.h" +#include "internal.h" + +void init_descriptors(struct server *server) +{ + server_add_descriptor(server, AVB_AEM_DESC_STRINGS, 0, + sizeof(struct avb_aem_desc_strings), + &(struct avb_aem_desc_strings) + { + .string_0 = "PipeWire", + .string_1 = "Configuration 1", + .string_2 = "Wim Taymans", + }); + server_add_descriptor(server, AVB_AEM_DESC_LOCALE, 0, + sizeof(struct avb_aem_desc_locale), + &(struct avb_aem_desc_locale) + { + .locale_identifier = "en-EN", + .number_of_strings = htons(1), + .base_strings = htons(0) + }); + server_add_descriptor(server, AVB_AEM_DESC_ENTITY, 0, + sizeof(struct avb_aem_desc_entity), + &(struct avb_aem_desc_entity) + { + .entity_id = htobe64(server->entity_id), + .entity_model_id = htobe64(0), + .entity_capabilities = htonl( + AVB_ADP_ENTITY_CAPABILITY_AEM_SUPPORTED | + AVB_ADP_ENTITY_CAPABILITY_CLASS_A_SUPPORTED | + AVB_ADP_ENTITY_CAPABILITY_GPTP_SUPPORTED | + AVB_ADP_ENTITY_CAPABILITY_AEM_IDENTIFY_CONTROL_INDEX_VALID | + AVB_ADP_ENTITY_CAPABILITY_AEM_INTERFACE_INDEX_VALID), + + .talker_stream_sources = htons(8), + .talker_capabilities = htons( + AVB_ADP_TALKER_CAPABILITY_IMPLEMENTED | + AVB_ADP_TALKER_CAPABILITY_AUDIO_SOURCE), + .listener_stream_sinks = htons(8), + .listener_capabilities = htons( + AVB_ADP_LISTENER_CAPABILITY_IMPLEMENTED | + AVB_ADP_LISTENER_CAPABILITY_AUDIO_SINK), + .controller_capabilities = htons(0), + .available_index = htonl(0), + .association_id = htobe64(0), + .entity_name = "PipeWire", + .vendor_name_string = htons(2), + .model_name_string = htons(0), + .firmware_version = "0.3.48", + .group_name = "", + .serial_number = "", + .configurations_count = htons(1), + .current_configuration = htons(0) + }); + struct { + struct avb_aem_desc_configuration desc; + struct avb_aem_desc_descriptor_count descriptor_counts[8]; + } __attribute__ ((__packed__)) config = + { + { + .object_name = "Configuration 1", + .localized_description = htons(1), + .descriptor_counts_count = htons(8), + .descriptor_counts_offset = htons( + 4 + sizeof(struct avb_aem_desc_configuration)), + }, + .descriptor_counts = { + { htons(AVB_AEM_DESC_AUDIO_UNIT), htons(1) }, + { htons(AVB_AEM_DESC_STREAM_INPUT), htons(1) }, + { htons(AVB_AEM_DESC_STREAM_OUTPUT), htons(1) }, + { htons(AVB_AEM_DESC_AVB_INTERFACE), htons(1) }, + { htons(AVB_AEM_DESC_CLOCK_SOURCE), htons(1) }, + { htons(AVB_AEM_DESC_CONTROL), htons(2) }, + { htons(AVB_AEM_DESC_LOCALE), htons(1) }, + { htons(AVB_AEM_DESC_CLOCK_DOMAIN), htons(1) } + } + }; + server_add_descriptor(server, AVB_AEM_DESC_CONFIGURATION, 0, + sizeof(config), &config); + + struct { + struct avb_aem_desc_audio_unit desc; + struct avb_aem_desc_sampling_rate sampling_rates[6]; + } __attribute__ ((__packed__)) audio_unit = + { + { + .object_name = "PipeWire", + .localized_description = htons(0), + .clock_domain_index = htons(0), + .number_of_stream_input_ports = htons(1), + .base_stream_input_port = htons(0), + .number_of_stream_output_ports = htons(1), + .base_stream_output_port = htons(0), + .number_of_external_input_ports = htons(8), + .base_external_input_port = htons(0), + .number_of_external_output_ports = htons(8), + .base_external_output_port = htons(0), + .number_of_internal_input_ports = htons(0), + .base_internal_input_port = htons(0), + .number_of_internal_output_ports = htons(0), + .base_internal_output_port = htons(0), + .number_of_controls = htons(0), + .base_control = htons(0), + .number_of_signal_selectors = htons(0), + .base_signal_selector = htons(0), + .number_of_mixers = htons(0), + .base_mixer = htons(0), + .number_of_matrices = htons(0), + .base_matrix = htons(0), + .number_of_splitters = htons(0), + .base_splitter = htons(0), + .number_of_combiners = htons(0), + .base_combiner = htons(0), + .number_of_demultiplexers = htons(0), + .base_demultiplexer = htons(0), + .number_of_multiplexers = htons(0), + .base_multiplexer = htons(0), + .number_of_transcoders = htons(0), + .base_transcoder = htons(0), + .number_of_control_blocks = htons(0), + .base_control_block = htons(0), + .current_sampling_rate = htonl(48000), + .sampling_rates_offset = htons( + 4 + sizeof(struct avb_aem_desc_audio_unit)), + .sampling_rates_count = htons(6), + }, + .sampling_rates = { + { .pull_frequency = htonl(44100) }, + { .pull_frequency = htonl(48000) }, + { .pull_frequency = htonl(88200) }, + { .pull_frequency = htonl(96000) }, + { .pull_frequency = htonl(176400) }, + { .pull_frequency = htonl(192000) }, + } + }; + server_add_descriptor(server, AVB_AEM_DESC_AUDIO_UNIT, 0, + sizeof(audio_unit), &audio_unit); + + struct { + struct avb_aem_desc_stream desc; + uint64_t stream_formats[6]; + } __attribute__ ((__packed__)) stream_input_0 = + { + { + .object_name = "Stream Input 1", + .localized_description = htons(0xffff), + .clock_domain_index = htons(0), + .stream_flags = htons( + AVB_AEM_DESC_STREAM_FLAG_SYNC_SOURCE | + AVB_AEM_DESC_STREAM_FLAG_CLASS_A), + .current_format = htobe64(0x00a0020840000800ULL), + .formats_offset = htons( + 4 + sizeof(struct avb_aem_desc_stream)), + .number_of_formats = htons(6), + .backup_talker_entity_id_0 = htobe64(0), + .backup_talker_unique_id_0 = htons(0), + .backup_talker_entity_id_1 = htobe64(0), + .backup_talker_unique_id_1 = htons(0), + .backup_talker_entity_id_2 = htobe64(0), + .backup_talker_unique_id_2 = htons(0), + .backedup_talker_entity_id = htobe64(0), + .backedup_talker_unique = htons(0), + .avb_interface_index = htons(0), + .buffer_length = htons(8) + }, + .stream_formats = { + htobe64(0x00a0010860000800ULL), + htobe64(0x00a0020860000800ULL), + htobe64(0x00a0030860000800ULL), + htobe64(0x00a0040860000800ULL), + htobe64(0x00a0050860000800ULL), + htobe64(0x00a0060860000800ULL), + }, + }; + server_add_descriptor(server, AVB_AEM_DESC_STREAM_INPUT, 0, + sizeof(stream_input_0), &stream_input_0); + + struct { + struct avb_aem_desc_stream desc; + uint64_t stream_formats[6]; + } __attribute__ ((__packed__)) stream_output_0 = + { + { + .object_name = "Stream Output 1", + .localized_description = htons(0xffff), + .clock_domain_index = htons(0), + .stream_flags = htons( + AVB_AEM_DESC_STREAM_FLAG_CLASS_A), + .current_format = htobe64(0x00a0020840000800ULL), + .formats_offset = htons( + 4 + sizeof(struct avb_aem_desc_stream)), + .number_of_formats = htons(6), + .backup_talker_entity_id_0 = htobe64(0), + .backup_talker_unique_id_0 = htons(0), + .backup_talker_entity_id_1 = htobe64(0), + .backup_talker_unique_id_1 = htons(0), + .backup_talker_entity_id_2 = htobe64(0), + .backup_talker_unique_id_2 = htons(0), + .backedup_talker_entity_id = htobe64(0), + .backedup_talker_unique = htons(0), + .avb_interface_index = htons(0), + .buffer_length = htons(8) + }, + .stream_formats = { + htobe64(0x00a0010860000800ULL), + htobe64(0x00a0020860000800ULL), + htobe64(0x00a0030860000800ULL), + htobe64(0x00a0040860000800ULL), + htobe64(0x00a0050860000800ULL), + htobe64(0x00a0060860000800ULL), + }, + }; + server_add_descriptor(server, AVB_AEM_DESC_STREAM_OUTPUT, 0, + sizeof(stream_output_0), &stream_output_0); + + struct avb_aem_desc_avb_interface avb_interface = { + .localized_description = htons(0xffff), + .interface_flags = htons( + AVB_AEM_DESC_AVB_INTERFACE_FLAG_GPTP_GRANDMASTER_SUPPORTED), + .clock_identity = htobe64(0), + .priority1 = 0, + .clock_class = 0, + .offset_scaled_log_variance = htons(0), + .clock_accuracy = 0, + .priority2 = 0, + .domain_number = 0, + .log_sync_interval = 0, + .log_announce_interval = 0, + .log_pdelay_interval = 0, + .port_number = 0, + }; + strncpy(avb_interface.object_name, server->ifname, 63); + memcpy(avb_interface.mac_address, server->mac_addr, 6); + server_add_descriptor(server, AVB_AEM_DESC_AVB_INTERFACE, 0, + sizeof(avb_interface), &avb_interface); + + struct avb_aem_desc_clock_source clock_source = { + .object_name = "Stream Clock", + .localized_description = htons(0xffff), + .clock_source_flags = htons(0), + .clock_source_type = htons( + AVB_AEM_DESC_CLOCK_SOURCE_TYPE_INPUT_STREAM), + .clock_source_identifier = htobe64(0), + .clock_source_location_type = htons(AVB_AEM_DESC_STREAM_INPUT), + .clock_source_location_index = htons(0), + }; + server_add_descriptor(server, AVB_AEM_DESC_CLOCK_SOURCE, 0, + sizeof(clock_source), &clock_source); +} diff --git a/src/modules/module-avb/iec61883.h b/src/modules/module-avb/iec61883.h new file mode 100644 index 0000000..6ca8724 --- /dev/null +++ b/src/modules/module-avb/iec61883.h @@ -0,0 +1,110 @@ +/* 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 AVB_IEC61883_H +#define AVB_IEC61883_H + +#include "packets.h" + +struct avb_packet_iec61883 { + uint8_t subtype; +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned sv:1; + unsigned version:3; + unsigned mr:1; + unsigned _r1:1; + unsigned gv:1; + unsigned tv:1; + + uint8_t seq_num; + + unsigned _r2:7; + unsigned tu:1; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned tv:1; + unsigned gv:1; + unsigned _r1:1; + unsigned mr:1; + unsigned version:3; + unsigned sv:1; + + uint8_t seq_num; + + unsigned tu:1; + unsigned _r2:7; +#endif + uint64_t stream_id; + uint32_t timestamp; + uint32_t gateway_info; + uint16_t data_len; +#if __BYTE_ORDER == __BIG_ENDIAN + uint8_t tag:2; + uint8_t channel:6; + + uint8_t tcode:4; + uint8_t app:4; + + uint8_t qi1:2; /* CIP Quadlet Indicator 1 */ + uint8_t sid:6; /* CIP Source ID */ + + uint8_t dbs; /* CIP Data Block Size */ + + uint8_t fn:2; /* CIP Fraction Number */ + uint8_t qpc:3; /* CIP Quadlet Padding Count */ + uint8_t sph:1; /* CIP Source Packet Header */ + uint8_t _r3:2; + + uint8_t dbc; /* CIP Data Block Continuity */ + + uint8_t qi2:2; /* CIP Quadlet Indicator 2 */ + uint8_t format_id:6; /* CIP Format ID */ +#elif __BYTE_ORDER == __LITTLE_ENDIAN + uint8_t channel:6; + uint8_t tag:2; + + uint8_t app:4; + uint8_t tcode:4; + + uint8_t sid:6; /* CIP Source ID */ + uint8_t qi1:2; /* CIP Quadlet Indicator 1 */ + + uint8_t dbs; /* CIP Data Block Size */ + + uint8_t _r3:2; + uint8_t sph:1; /* CIP Source Packet Header */ + uint8_t qpc:3; /* CIP Quadlet Padding Count */ + uint8_t fn:2; /* CIP Fraction Number */ + + uint8_t dbc; /* CIP Data Block Continuity */ + + uint8_t format_id:6; /* CIP Format ID */ + uint8_t qi2:2; /* CIP Quadlet Indicator 2 */ +#endif + uint8_t fdf; /* CIP Format Dependent Field */ + uint16_t syt; + + uint8_t payload[0]; +} __attribute__ ((__packed__)); + +#endif /* AVB_IEC61883_H */ diff --git a/src/modules/module-avb/internal.h b/src/modules/module-avb/internal.h new file mode 100644 index 0000000..9d29d92 --- /dev/null +++ b/src/modules/module-avb/internal.h @@ -0,0 +1,166 @@ +/* PipeWire + * + * 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 AVB_INTERNAL_H +#define AVB_INTERNAL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include <pipewire/pipewire.h> + +struct server; +struct avb_mrp; + +#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; + struct spa_hook context_listener; + struct pw_core *core; + unsigned do_disconnect:1; + + struct pw_properties *props; + + struct spa_list servers; +}; + +struct server_events { +#define AVB_VERSION_SERVER_EVENTS 0 + uint32_t version; + + /** the server is destroyed */ + void (*destroy) (void *data); + + int (*message) (void *data, uint64_t now, const void *message, int len); + + void (*periodic) (void *data, uint64_t now); + + int (*command) (void *data, uint64_t now, const char *command, const char *args, FILE *out); +}; + +struct descriptor { + struct spa_list link; + uint16_t type; + uint16_t index; + uint32_t size; + void *ptr; +}; + +struct server { + struct spa_list link; + struct impl *impl; + + char *ifname; + uint8_t mac_addr[6]; + uint64_t entity_id; + int ifindex; + + struct spa_source *source; + struct spa_source *timer; + + struct spa_hook_list listener_list; + + struct spa_list descriptors; + struct spa_list streams; + + unsigned debug_messages:1; + + struct avb_mrp *mrp; + struct avb_mmrp *mmrp; + struct avb_mvrp *mvrp; + struct avb_msrp *msrp; + struct avb_maap *maap; + + struct avb_msrp_attribute *domain_attr; +}; + +#include "stream.h" + +static inline const struct descriptor *server_find_descriptor(struct server *server, + uint16_t type, uint16_t index) +{ + struct descriptor *d; + spa_list_for_each(d, &server->descriptors, link) { + if (d->type == type && + d->index == index) + return d; + } + return NULL; +} +static inline void *server_add_descriptor(struct server *server, + uint16_t type, uint16_t index, size_t size, void *ptr) +{ + struct descriptor *d; + + if ((d = calloc(1, sizeof(struct descriptor) + size)) == NULL) + return NULL; + + d->type = type; + d->index = index; + d->size = size; + d->ptr = SPA_PTROFF(d, sizeof(struct descriptor), void); + if (ptr) + memcpy(d->ptr, ptr, size); + spa_list_append(&server->descriptors, &d->link); + return d->ptr; +} + +static inline struct stream *server_find_stream(struct server *server, + enum spa_direction direction, uint16_t index) +{ + struct stream *s; + spa_list_for_each(s, &server->streams, link) { + if (s->direction == direction && + s->index == index) + return s; + } + return NULL; +} + +struct server *avdecc_server_new(struct impl *impl, struct spa_dict *props); +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); + +struct aecp { + struct server *server; + struct spa_hook server_listener; +}; + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* AVB_INTERNAL_H */ diff --git a/src/modules/module-avb/maap.c b/src/modules/module-avb/maap.c new file mode 100644 index 0000000..7d195be --- /dev/null +++ b/src/modules/module-avb/maap.c @@ -0,0 +1,469 @@ +/* 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 <unistd.h> + +#include <spa/utils/json.h> + +#include <pipewire/pipewire.h> + +#include "utils.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) +{ + switch (message_type) { + case AVB_MAAP_MESSAGE_TYPE_PROBE: + return "PROBE"; + case AVB_MAAP_MESSAGE_TYPE_DEFEND: + return "DEFEND"; + case AVB_MAAP_MESSAGE_TYPE_ANNOUNCE: + return "ANNOUNCE"; + } + return "INVALID"; +} + +static void maap_message_debug(struct maap *maap, const struct avb_packet_maap *p) +{ + uint32_t v; + const uint8_t *addr; + + v = AVB_PACKET_MAAP_GET_MESSAGE_TYPE(p); + pw_log_info("message-type: %d (%s)", v, message_type_as_string(v)); + pw_log_info(" maap-version: %d", AVB_PACKET_MAAP_GET_MAAP_VERSION(p)); + pw_log_info(" length: %d", AVB_PACKET_GET_LENGTH(&p->hdr)); + + pw_log_info(" stream-id: 0x%"PRIx64, AVB_PACKET_MAAP_GET_STREAM_ID(p)); + addr = AVB_PACKET_MAAP_GET_REQUEST_START(p); + pw_log_info(" request-start: %02x:%02x:%02x:%02x:%02x:%02x", + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); + pw_log_info(" request-count: %d", AVB_PACKET_MAAP_GET_REQUEST_COUNT(p)); + addr = AVB_PACKET_MAAP_GET_CONFLICT_START(p); + pw_log_info(" conflict-start: %02x:%02x:%02x:%02x:%02x:%02x", + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); + 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_ethernet_header *h; + struct avb_packet_maap *p; + uint8_t buf[1024]; + uint8_t bmac[6] = AVB_MAAP_MAC; + int res = 0; + uint8_t start[6]; + + spa_memzero(buf, sizeof(buf)); + h = (void*)buf; + p = SPA_PTROFF(h, sizeof(*h), void); + + memcpy(h->dest, bmac, 6); + memcpy(h->src, maap->server->mac_addr, 6); + h->type = htons(AVB_TSN_ETH); + + p->hdr.subtype = AVB_SUBTYPE_MAAP; + AVB_PACKET_SET_LENGTH(&p->hdr, sizeof(*p)); + + 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); + } + + if (maap->server->debug_messages) { + pw_log_info("send: %d (%s)", type, message_type_as_string(type)); + maap_message_debug(maap, p); + } + + if (send(maap->source->fd, p, sizeof(*h) + 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; + + if (AVB_PACKET_GET_SUBTYPE(&p->hdr) != AVB_SUBTYPE_MAAP) + return 0; + + if (maap->server->debug_messages) + 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; +} + +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 int load_state(struct maap *maap) +{ + const char *str; + char key[512]; + struct spa_json it[3]; + bool have_offset = false; + int count = 0, offset = 0; + + snprintf(key, sizeof(key), "maap.%s", maap->server->ifname); + pw_conf_load_state("module-avb", key, maap->props); + + if ((str = pw_properties_get(maap->props, "maap.addresses")) == NULL) + return 0; + + spa_json_init(&it[0], str, strlen(str)); + if (spa_json_enter_array(&it[0], &it[1]) <= 0) + return 0; + + if (spa_json_enter_object(&it[1], &it[2]) <= 0) + return 0; + + while (spa_json_get_string(&it[2], key, sizeof(key)) > 0) { + const char *val; + int len; + + if ((len = spa_json_next(&it[2], &val)) <= 0) + break; + + if (spa_streq(key, "start")) { + uint8_t addr[6]; + if (avb_utils_parse_addr(val, len, addr) >= 0 && + memcmp(addr, maap_base, 4) == 0) { + offset = addr[4] << 8 | addr[5]; + have_offset = true; + } + } + else if (spa_streq(key, "count")) { + spa_json_parse_int(val, len, &count); + } + } + if (count > 0 && have_offset) { + maap->count = count; + maap->offset = offset; + maap->state = STATE_PROBE; + maap->probe_count = MAAP_PROBE_RETRANSMITS; + maap->timeout = PROBE_TIMEOUT(0); + } + return 0; +} + +static int save_state(struct maap *maap) +{ + char *ptr; + size_t size; + FILE *f; + char key[512]; + uint32_t count; + + if ((f = open_memstream(&ptr, &size)) == NULL) + return -errno; + + fprintf(f, "[ "); + fprintf(f, "{ \"start\": \"%02x:%02x:%02x:%02x:%02x:%02x\", ", + maap_base[0], maap_base[1], maap_base[2], + maap_base[3], (maap->offset >> 8) & 0xff, + maap->offset & 0xff); + fprintf(f, " \"count\": %u } ", maap->count); + fprintf(f, "]"); + fclose(f); + + count = pw_properties_set(maap->props, "maap.addresses", ptr); + free(ptr); + + if (count > 0) { + snprintf(key, sizeof(key), "maap.%s", maap->server->ifname); + pw_conf_save_state("module-avb", key, maap->props); + } + return 0; +} + +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; + save_state(maap); + } + 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; + maap_free(maap); +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = maap_destroy, + .periodic = maap_periodic, +}; + +struct avb_maap *avb_maap_register(struct server *server) +{ + struct maap *maap; + uint8_t bmac[6] = AVB_MAAP_MAC; + int fd, res; + + fd = avb_server_make_socket(server, AVB_TSN_ETH, bmac); + 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; + } + + maap->server = server; + pw_log_info("0x%"PRIx64" %d", server->entity_id, server->ifindex); + + if (pw_getrandom(maap->xsubi, sizeof(maap->xsubi), 0) != sizeof(maap->xsubi)) { + res = -errno; + goto error_free; + } + load_state(maap); + + 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_free; + } + avdecc_server_add_listener(server, &maap->server_listener, &server_events, maap); + + return (struct avb_maap *)maap; + +error_free: + free(maap); +error_close: + close(fd); +error: + errno = -res; + return NULL; +} + +int avb_maap_reserve(struct avb_maap *m, uint32_t count) +{ + struct maap *maap = (struct maap*)m; + if (count > maap->count) + 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; +} diff --git a/src/modules/module-avb/maap.h b/src/modules/module-avb/maap.h new file mode 100644 index 0000000..6e56f8e --- /dev/null +++ b/src/modules/module-avb/maap.h @@ -0,0 +1,70 @@ +/* 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 AVB_MAAP_H +#define AVB_MAAP_H + +#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 + +struct avb_packet_maap { + struct avb_packet_header hdr; + uint64_t stream_id; + uint8_t request_start[6]; + uint16_t request_count; + uint8_t conflict_start[6]; + uint16_t conflict_count; +} __attribute__ ((__packed__)); + +#define AVB_PACKET_MAAP_SET_MESSAGE_TYPE(p,v) AVB_PACKET_SET_SUB1(&(p)->hdr, v) +#define AVB_PACKET_MAAP_SET_MAAP_VERSION(p,v) AVB_PACKET_SET_SUB2(&(p)->hdr, v) +#define AVB_PACKET_MAAP_SET_STREAM_ID(p,v) ((p)->stream_id = htobe64(v)) +#define AVB_PACKET_MAAP_SET_REQUEST_START(p,v) memcpy((p)->request_start, (v), 6) +#define AVB_PACKET_MAAP_SET_REQUEST_COUNT(p,v) ((p)->request_count = htons(v)) +#define AVB_PACKET_MAAP_SET_CONFLICT_START(p,v) memcpy((p)->conflict_start, (v), 6) +#define AVB_PACKET_MAAP_SET_CONFLICT_COUNT(p,v) ((p)->conflict_count = htons(v)) + +#define AVB_PACKET_MAAP_GET_MESSAGE_TYPE(p) AVB_PACKET_GET_SUB1(&(p)->hdr) +#define AVB_PACKET_MAAP_GET_MAAP_VERSION(p) AVB_PACKET_GET_SUB2(&(p)->hdr) +#define AVB_PACKET_MAAP_GET_STREAM_ID(p) be64toh((p)->stream_id) +#define AVB_PACKET_MAAP_GET_REQUEST_START(p) ((p)->request_start) +#define AVB_PACKET_MAAP_GET_REQUEST_COUNT(p) ntohs((p)->request_count) +#define AVB_PACKET_MAAP_GET_CONFLICT_START(p) ((p)->conflict_start) +#define AVB_PACKET_MAAP_GET_CONFLICT_COUNT(p) ntohs((p)->conflict_count) + +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 */ diff --git a/src/modules/module-avb/mmrp.c b/src/modules/module-avb/mmrp.c new file mode 100644 index 0000000..022aea8 --- /dev/null +++ b/src/modules/module-avb/mmrp.c @@ -0,0 +1,233 @@ +/* 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 <unistd.h> + +#include <pipewire/pipewire.h> + +#include "utils.h" +#include "mmrp.h" + +static const uint8_t mmrp_mac[6] = AVB_MMRP_MAC; + +struct attr { + struct avb_mmrp_attribute attr; + struct spa_list link; +}; + +struct mmrp { + struct server *server; + struct spa_hook server_listener; + + struct spa_source *source; + + struct spa_list attributes; +}; + +static bool mmrp_check_header(void *data, const void *hdr, size_t *hdr_size, bool *has_params) +{ + const struct avb_packet_mmrp_msg *msg = hdr; + uint8_t attr_type = msg->attribute_type; + + if (!AVB_MMRP_ATTRIBUTE_TYPE_VALID(attr_type)) + return false; + + *hdr_size = sizeof(*msg); + *has_params = false; + return true; +} + +static int mmrp_attr_event(void *data, uint64_t now, uint8_t attribute_type, uint8_t event) +{ + struct mmrp *mmrp = data; + struct attr *a; + spa_list_for_each(a, &mmrp->attributes, link) + if (a->attr.type == attribute_type) + avb_mrp_attribute_update_state(a->attr.mrp, now, event); + return 0; +} + +static void debug_service_requirement(const struct avb_packet_mmrp_service_requirement *t) +{ + char buf[128]; + pw_log_info("service requirement"); + pw_log_info(" %s", avb_utils_format_addr(buf, sizeof(buf), t->addr)); +} + +static int process_service_requirement(struct mmrp *mmrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avb_packet_mmrp_service_requirement *t = m; + struct attr *a; + + debug_service_requirement(t); + + spa_list_for_each(a, &mmrp->attributes, link) + if (a->attr.type == attr_type && + memcmp(a->attr.attr.service_requirement.addr, t->addr, 6) == 0) + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + return 0; +} + +static void debug_process_mac(const struct avb_packet_mmrp_mac *t) +{ + char buf[128]; + pw_log_info("mac"); + pw_log_info(" %s", avb_utils_format_addr(buf, sizeof(buf), t->addr)); +} + +static int process_mac(struct mmrp *mmrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avb_packet_mmrp_mac *t = m; + struct attr *a; + + debug_process_mac(t); + + spa_list_for_each(a, &mmrp->attributes, link) + if (a->attr.type == attr_type && + memcmp(a->attr.attr.mac.addr, t->addr, 6) == 0) + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + return 0; +} + +static const struct { + int (*dispatch) (struct mmrp *mmrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num); +} dispatch[] = { + [AVB_MMRP_ATTRIBUTE_TYPE_SERVICE_REQUIREMENT] = { process_service_requirement, }, + [AVB_MMRP_ATTRIBUTE_TYPE_MAC] = { process_mac, }, +}; + +static int mmrp_process(void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index) +{ + struct mmrp *mmrp = data; + return dispatch[attribute_type].dispatch(mmrp, now, + attribute_type, value, event, param, index); +} + +static const struct avb_mrp_parse_info info = { + AVB_VERSION_MRP_PARSE_INFO, + .check_header = mmrp_check_header, + .attr_event = mmrp_attr_event, + .process = mmrp_process, +}; + +static int mmrp_message(struct mmrp *mmrp, uint64_t now, const void *message, int len) +{ + 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, +}; + +struct avb_mmrp_attribute *avb_mmrp_attribute_new(struct avb_mmrp *m, + uint8_t type) +{ + struct mmrp *mmrp = (struct mmrp*)m; + struct avb_mrp_attribute *attr; + struct attr *a; + + attr = avb_mrp_attribute_new(mmrp->server->mrp, sizeof(struct attr)); + + a = attr->user_data; + a->attr.mrp = attr; + a->attr.type = type; + spa_list_append(&mmrp->attributes, &a->link); + + return &a->attr; +} + +struct avb_mmrp *avb_mmrp_register(struct server *server) +{ + struct mmrp *mmrp; + int fd, res; + + 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/mmrp.h b/src/modules/module-avb/mmrp.h new file mode 100644 index 0000000..b7bcf8c --- /dev/null +++ b/src/modules/module-avb/mmrp.h @@ -0,0 +1,68 @@ +/* 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 AVB_MMRP_H +#define AVB_MMRP_H + +#include "mrp.h" +#include "internal.h" + +#define AVB_MMRP_ETH 0x88f6 +#define AVB_MMRP_MAC { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x20 } + +#define AVB_MMRP_ATTRIBUTE_TYPE_SERVICE_REQUIREMENT 1 +#define AVB_MMRP_ATTRIBUTE_TYPE_MAC 2 +#define AVB_MMRP_ATTRIBUTE_TYPE_VALID(t) ((t)>=1 && (t)<=2) + +struct avb_packet_mmrp_msg { + uint8_t attribute_type; + uint8_t attribute_length; + uint8_t attribute_list[0]; +} __attribute__ ((__packed__)); + +struct avb_packet_mmrp_service_requirement { + unsigned char addr[6]; +} __attribute__ ((__packed__)); + +struct avb_packet_mmrp_mac { + unsigned char addr[6]; +} __attribute__ ((__packed__)); + +struct avb_mmrp; + +struct avb_mmrp_attribute { + struct avb_mrp_attribute *mrp; + uint8_t type; + union { + struct avb_packet_mmrp_service_requirement service_requirement; + struct avb_packet_mmrp_mac mac; + } attr; +}; + +struct avb_mmrp_attribute *avb_mmrp_attribute_new(struct avb_mmrp *mmrp, + uint8_t type); + +struct avb_mmrp *avb_mmrp_register(struct server *server); + +#endif /* AVB_MMRP_H */ diff --git a/src/modules/module-avb/mrp.c b/src/modules/module-avb/mrp.c new file mode 100644 index 0000000..7b6bc46 --- /dev/null +++ b/src/modules/module-avb/mrp.c @@ -0,0 +1,612 @@ +/* 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 <pipewire/pipewire.h> + +#include "mrp.h" + +#define MRP_JOINTIMER_MS 100 +#define MRP_LVTIMER_MS 1000 +#define MRP_LVATIMER_MS 10000 +#define MRP_PERIODTIMER_MS 1000 + +#define mrp_emit(s,m,v,...) spa_hook_list_call(&s->listener_list, struct avb_mrp_events, m, v, ##__VA_ARGS__) +#define mrp_emit_event(s,n,e) mrp_emit(s,event,0,n,e) +#define mrp_emit_notify(s,n,a,e) mrp_emit(s,notify,0,n,a,e) + +#define mrp_attribute_emit(a,m,v,...) spa_hook_list_call(&a->listener_list, struct avb_mrp_attribute_events, m, v, ##__VA_ARGS__) +#define mrp_attribute_emit_notify(a,n,e) mrp_attribute_emit(a,notify,0,n,e) + + +struct mrp; + +struct attribute { + struct avb_mrp_attribute attr; + struct mrp *mrp; + struct spa_list link; + uint8_t applicant_state; + uint8_t registrar_state; + uint64_t leave_timeout; + unsigned joined:1; + struct spa_hook_list listener_list; +}; + +struct mrp { + struct server *server; + struct spa_hook server_listener; + + struct spa_hook_list listener_list; + + struct spa_list attributes; + + uint64_t periodic_timeout; + uint64_t leave_all_timeout; + uint64_t join_timeout; +}; + +static void mrp_destroy(void *data) +{ + struct mrp *mrp = data; + spa_hook_remove(&mrp->server_listener); + free(mrp); +} + +static void global_event(struct mrp *mrp, uint64_t now, uint8_t event) +{ + struct attribute *a; + spa_list_for_each(a, &mrp->attributes, link) + avb_mrp_attribute_update_state(&a->attr, now, event); + mrp_emit_event(mrp, now, event); +} + +static void mrp_periodic(void *data, uint64_t now) +{ + struct mrp *mrp = data; + bool leave_all = false; + struct attribute *a; + + if (now > mrp->periodic_timeout) { + if (mrp->periodic_timeout > 0) + global_event(mrp, now, AVB_MRP_EVENT_PERIODIC); + mrp->periodic_timeout = now + MRP_PERIODTIMER_MS * SPA_NSEC_PER_MSEC; + } + if (now > mrp->leave_all_timeout) { + if (mrp->leave_all_timeout > 0) { + global_event(mrp, now, AVB_MRP_EVENT_RX_LVA); + leave_all = true; + } + mrp->leave_all_timeout = now + (MRP_LVATIMER_MS + (random() % (MRP_LVATIMER_MS / 2))) + * SPA_NSEC_PER_MSEC; + } + + if (now > mrp->join_timeout) { + if (mrp->join_timeout > 0) { + uint8_t event = leave_all ? AVB_MRP_EVENT_TX_LVA : AVB_MRP_EVENT_TX; + global_event(mrp, now, event); + } + mrp->join_timeout = now + MRP_JOINTIMER_MS * SPA_NSEC_PER_MSEC; + } + + spa_list_for_each(a, &mrp->attributes, link) { + if (a->leave_timeout > 0 && now > a->leave_timeout) { + a->leave_timeout = 0; + avb_mrp_attribute_update_state(&a->attr, now, AVB_MRP_EVENT_LV_TIMER); + } + } +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = mrp_destroy, + .periodic = mrp_periodic, +}; + +int avb_mrp_parse_packet(struct avb_mrp *mrp, uint64_t now, const void *pkt, int len, + const struct avb_mrp_parse_info *info, void *data) +{ + uint8_t *e = SPA_PTROFF(pkt, len, uint8_t); + uint8_t *m = SPA_PTROFF(pkt, sizeof(struct avb_packet_mrp), uint8_t); + + while (m < e && (m[0] != 0 || m[1] != 0)) { + const struct avb_packet_mrp_hdr *hdr = (const struct avb_packet_mrp_hdr*)m; + uint8_t attr_type = hdr->attribute_type; + uint8_t attr_len = hdr->attribute_length; + size_t hdr_size; + bool has_param; + + if (!info->check_header(data, hdr, &hdr_size, &has_param)) + return -EINVAL; + + m += hdr_size; + + while (m < e && (m[0] != 0 || m[1] != 0)) { + const struct avb_packet_mrp_vector *v = + (const struct avb_packet_mrp_vector*)m; + uint16_t i, num_values = AVB_MRP_VECTOR_GET_NUM_VALUES(v); + uint8_t event_len = (num_values+2)/3; + uint8_t param_len = has_param ? (num_values+3)/4 : 0; + int plen = sizeof(*v) + attr_len + event_len + param_len; + const uint8_t *first = v->first_value; + uint8_t event[3], param[4] = { 0, }; + + if (m + plen > e) + return -EPROTO; + + if (v->lva) + info->attr_event(data, now, attr_type, AVB_MRP_EVENT_RX_LVA); + + for (i = 0; i < num_values; i++) { + if (i % 3 == 0) { + uint8_t ep = first[attr_len + i/3]; + event[2] = ep % 6; ep /= 6; + event[1] = ep % 6; ep /= 6; + event[0] = ep % 6; + } + if (has_param && (i % 4 == 0)) { + uint8_t ep = first[attr_len + event_len + i/4]; + param[3] = ep % 4; ep /= 4; + param[2] = ep % 4; ep /= 4; + param[1] = ep % 4; ep /= 4; + param[0] = ep % 4; + } + info->process(data, now, attr_type, first, + event[i%3], param[i%4], i); + } + m += plen; + } + m += 2; + } + return 0; +} + +const char *avb_mrp_notify_name(uint8_t notify) +{ + switch(notify) { + case AVB_MRP_NOTIFY_NEW: + return "new"; + case AVB_MRP_NOTIFY_JOIN: + return "join"; + case AVB_MRP_NOTIFY_LEAVE: + return "leave"; + } + return "unknown"; +} + +const char *avb_mrp_send_name(uint8_t send) +{ + switch(send) { + case AVB_MRP_SEND_NEW: + return "new"; + case AVB_MRP_SEND_JOININ: + return "joinin"; + case AVB_MRP_SEND_IN: + return "in"; + case AVB_MRP_SEND_JOINMT: + return "joinmt"; + case AVB_MRP_SEND_MT: + return "mt"; + case AVB_MRP_SEND_LV: + return "leave"; + } + return "unknown"; +} + +struct avb_mrp_attribute *avb_mrp_attribute_new(struct avb_mrp *m, + size_t user_size) +{ + struct mrp *mrp = (struct mrp*)m; + struct attribute *a; + + a = calloc(1, sizeof(*a) + user_size); + if (a == NULL) + return NULL; + + a->mrp = mrp; + a->attr.user_data = SPA_PTROFF(a, sizeof(*a), void); + spa_hook_list_init(&a->listener_list); + spa_list_append(&mrp->attributes, &a->link); + + return &a->attr; +} + +void avb_mrp_attribute_destroy(struct avb_mrp_attribute *attr) +{ + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + spa_list_remove(&a->link); + free(a); +} + +void avb_mrp_attribute_add_listener(struct avb_mrp_attribute *attr, struct spa_hook *listener, + const struct avb_mrp_attribute_events *events, void *data) +{ + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + spa_hook_list_append(&a->listener_list, listener, events, data); +} + +void avb_mrp_attribute_update_state(struct avb_mrp_attribute *attr, uint64_t now, + int event) +{ + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + struct mrp *mrp = a->mrp; + uint8_t notify = 0, state; + uint8_t send = 0; + + state = a->registrar_state; + + switch (event) { + case AVB_MRP_EVENT_BEGIN: + state = AVB_MRP_MT; + break; + case AVB_MRP_EVENT_RX_NEW: + notify = AVB_MRP_NOTIFY_NEW; + switch (state) { + case AVB_MRP_LV: + a->leave_timeout = 0; + break; + } + state = AVB_MRP_IN; + break; + case AVB_MRP_EVENT_RX_JOININ: + case AVB_MRP_EVENT_RX_JOINMT: + switch (state) { + case AVB_MRP_LV: + a->leave_timeout = 0; + break; + case AVB_MRP_MT: + notify = AVB_MRP_NOTIFY_JOIN; + break; + } + state = AVB_MRP_IN; + break; + case AVB_MRP_EVENT_RX_LV: + case AVB_MRP_EVENT_RX_LVA: + case AVB_MRP_EVENT_TX_LVA: + case AVB_MRP_EVENT_REDECLARE: + switch (state) { + case AVB_MRP_IN: + a->leave_timeout = now + MRP_LVTIMER_MS * SPA_NSEC_PER_MSEC; + //state = AVB_MRP_LV; + break; + } + break; + case AVB_MRP_EVENT_FLUSH: + switch (state) { + case AVB_MRP_LV: + notify = AVB_MRP_NOTIFY_LEAVE; + break; + } + state = AVB_MRP_MT; + break; + case AVB_MRP_EVENT_LV_TIMER: + switch (state) { + case AVB_MRP_LV: + notify = AVB_MRP_NOTIFY_LEAVE; + state = AVB_MRP_MT; + break; + } + break; + default: + break; + } + if (notify) { + mrp_attribute_emit_notify(a, now, notify); + mrp_emit_notify(mrp, now, &a->attr, notify); + } + + if (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; + } + + state = a->applicant_state; + + switch (event) { + case AVB_MRP_EVENT_BEGIN: + state = AVB_MRP_VO; + break; + case AVB_MRP_EVENT_NEW: + switch (state) { + case AVB_MRP_VN: + case AVB_MRP_AN: + break; + default: + state = AVB_MRP_VN; + break; + } + break; + case AVB_MRP_EVENT_JOIN: + switch (state) { + case AVB_MRP_VO: + case AVB_MRP_LO: + state = AVB_MRP_VP; + break; + case AVB_MRP_LA: + state = AVB_MRP_AA; + break; + case AVB_MRP_AO: + state = AVB_MRP_AP; + break; + case AVB_MRP_QO: + state = AVB_MRP_QP; + break; + } + break; + case AVB_MRP_EVENT_LV: + switch (state) { + case AVB_MRP_VP: + state = AVB_MRP_VO; + break; + case AVB_MRP_VN: + case AVB_MRP_AN: + case AVB_MRP_AA: + case AVB_MRP_QA: + state = AVB_MRP_LA; + break; + case AVB_MRP_AP: + state = AVB_MRP_AO; + break; + case AVB_MRP_QP: + state = AVB_MRP_QO; + break; + } + break; + case AVB_MRP_EVENT_RX_JOININ: + switch (state) { + case AVB_MRP_VO: + state = AVB_MRP_AO; + break; + case AVB_MRP_VP: + state = AVB_MRP_AP; + break; + case AVB_MRP_AA: + state = AVB_MRP_QA; + break; + case AVB_MRP_AO: + state = AVB_MRP_QO; + break; + case AVB_MRP_AP: + state = AVB_MRP_QP; + break; + } + SPA_FALLTHROUGH; + case AVB_MRP_EVENT_RX_IN: + switch (state) { + case AVB_MRP_AA: + state = AVB_MRP_QA; + break; + } + break; + case AVB_MRP_EVENT_RX_JOINMT: + case AVB_MRP_EVENT_RX_MT: + switch (state) { + case AVB_MRP_QA: + state = AVB_MRP_AA; + break; + case AVB_MRP_QO: + state = AVB_MRP_AO; + break; + case AVB_MRP_QP: + state = AVB_MRP_AP; + break; + case AVB_MRP_LO: + state = AVB_MRP_VO; + break; + } + break; + case AVB_MRP_EVENT_RX_LV: + case AVB_MRP_EVENT_RX_LVA: + case AVB_MRP_EVENT_REDECLARE: + switch (state) { + case AVB_MRP_VO: + case AVB_MRP_AO: + case AVB_MRP_QO: + state = AVB_MRP_LO; + break; + case AVB_MRP_AN: + state = AVB_MRP_VN; + break; + case AVB_MRP_AA: + case AVB_MRP_QA: + case AVB_MRP_AP: + case AVB_MRP_QP: + state = AVB_MRP_VP; + break; + } + break; + case AVB_MRP_EVENT_PERIODIC: + switch (state) { + case AVB_MRP_QA: + state = AVB_MRP_AA; + break; + case AVB_MRP_QP: + state = AVB_MRP_AP; + break; + } + break; + case AVB_MRP_EVENT_TX: + switch (state) { + case AVB_MRP_VP: + case AVB_MRP_AA: + case AVB_MRP_AP: + if (a->registrar_state == AVB_MRP_IN) + send = AVB_MRP_SEND_JOININ; + else + send = AVB_MRP_SEND_JOINMT; + break; + case AVB_MRP_VN: + case AVB_MRP_AN: + send = AVB_MRP_SEND_NEW; + break; + case AVB_MRP_LA: + send = AVB_MRP_SEND_LV; + break; + case AVB_MRP_LO: + if (a->registrar_state == AVB_MRP_IN) + send = AVB_MRP_SEND_IN; + else + send = AVB_MRP_SEND_MT; + break; + } + switch (state) { + case AVB_MRP_VP: + state = AVB_MRP_AA; + break; + case AVB_MRP_VN: + state = AVB_MRP_AN; + break; + case AVB_MRP_AN: + if(a->registrar_state == AVB_MRP_IN) + state = AVB_MRP_QA; + else + state = AVB_MRP_AA; + break; + case AVB_MRP_AA: + case AVB_MRP_AP: + state = AVB_MRP_QA; + break; + case AVB_MRP_LA: + case AVB_MRP_LO: + state = AVB_MRP_VO; + break; + } + break; + case AVB_MRP_EVENT_TX_LVA: + { + switch (state) { + case AVB_MRP_VP: + if (a->registrar_state == AVB_MRP_IN) + send = AVB_MRP_SEND_IN; + else + send = AVB_MRP_SEND_MT; + break; + case AVB_MRP_VN: + case AVB_MRP_AN: + send = AVB_MRP_SEND_NEW; + break; + case AVB_MRP_AA: + case AVB_MRP_QA: + case AVB_MRP_AP: + case AVB_MRP_QP: + if (a->registrar_state == AVB_MRP_IN) + send = AVB_MRP_SEND_JOININ; + else + send = AVB_MRP_SEND_JOINMT; + break; + } + switch (state) { + case AVB_MRP_VO: + case AVB_MRP_LA: + case AVB_MRP_AO: + case AVB_MRP_QO: + state = AVB_MRP_LO; + break; + case AVB_MRP_VP: + state = AVB_MRP_AA; + break; + case AVB_MRP_VN: + state = AVB_MRP_AN; + break; + case AVB_MRP_AN: + case AVB_MRP_AA: + case AVB_MRP_AP: + case AVB_MRP_QP: + state = AVB_MRP_QA; + break; + } + break; + } + default: + break; + } + if (a->applicant_state != state || send) { + pw_log_debug("attr %p: %d %d -> %d %d", a, event, a->applicant_state, state, send); + a->applicant_state = state; + } + if (a->joined) + a->attr.pending_send = send; +} + +void avb_mrp_attribute_rx_event(struct avb_mrp_attribute *attr, uint64_t now, uint8_t event) +{ + static const int map[] = { + [AVB_MRP_ATTRIBUTE_EVENT_NEW] = AVB_MRP_EVENT_RX_NEW, + [AVB_MRP_ATTRIBUTE_EVENT_JOININ] = AVB_MRP_EVENT_RX_JOININ, + [AVB_MRP_ATTRIBUTE_EVENT_IN] = AVB_MRP_EVENT_RX_IN, + [AVB_MRP_ATTRIBUTE_EVENT_JOINMT] = AVB_MRP_EVENT_RX_JOINMT, + [AVB_MRP_ATTRIBUTE_EVENT_MT] = AVB_MRP_EVENT_RX_MT, + [AVB_MRP_ATTRIBUTE_EVENT_LV] = AVB_MRP_EVENT_RX_LV, + }; + avb_mrp_attribute_update_state(attr, now, map[event]); +} + +void avb_mrp_attribute_begin(struct avb_mrp_attribute *attr, uint64_t now) +{ + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + a->leave_timeout = 0; + avb_mrp_attribute_update_state(attr, now, AVB_MRP_EVENT_BEGIN); +} + +void avb_mrp_attribute_join(struct avb_mrp_attribute *attr, uint64_t now, bool is_new) +{ + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + a->joined = true; + int event = is_new ? AVB_MRP_EVENT_NEW : AVB_MRP_EVENT_JOIN; + avb_mrp_attribute_update_state(attr, now, event); +} + +void avb_mrp_attribute_leave(struct avb_mrp_attribute *attr, uint64_t now) +{ + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + avb_mrp_attribute_update_state(attr, now, AVB_MRP_EVENT_LV); + a->joined = false; +} + +void avb_mrp_destroy(struct avb_mrp *mrp) +{ + mrp_destroy(mrp); +} + +struct avb_mrp *avb_mrp_new(struct server *server) +{ + struct mrp *mrp; + + mrp = calloc(1, sizeof(*mrp)); + if (mrp == NULL) + return NULL; + + mrp->server = server; + spa_list_init(&mrp->attributes); + spa_hook_list_init(&mrp->listener_list); + + avdecc_server_add_listener(server, &mrp->server_listener, &server_events, mrp); + + return (struct avb_mrp*)mrp; +} + +void avb_mrp_add_listener(struct avb_mrp *m, struct spa_hook *listener, + const struct avb_mrp_events *events, void *data) +{ + struct mrp *mrp = (struct mrp*)m; + spa_hook_list_append(&mrp->listener_list, listener, events, data); +} diff --git a/src/modules/module-avb/mrp.h b/src/modules/module-avb/mrp.h new file mode 100644 index 0000000..0a05d4b --- /dev/null +++ b/src/modules/module-avb/mrp.h @@ -0,0 +1,181 @@ +/* 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 AVB_MRP_H +#define AVB_MRP_H + +#include "packets.h" +#include "internal.h" + +#define AVB_MRP_PROTOCOL_VERSION 0 + +struct avb_packet_mrp { + struct avb_ethernet_header eth; + uint8_t version; +} __attribute__ ((__packed__)); + +struct avb_packet_mrp_hdr { + uint8_t attribute_type; + uint8_t attribute_length; +} __attribute__ ((__packed__)); + +struct avb_packet_mrp_vector { +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned lva:3; + unsigned nv1:5; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned nv1:5; + unsigned lva:3; +#endif + uint8_t nv2; + uint8_t first_value[0]; +} __attribute__ ((__packed__)); + +#define AVB_MRP_VECTOR_SET_NUM_VALUES(a,v) ((a)->nv1 = ((v) >> 8),(a)->nv2 = (v)) +#define AVB_MRP_VECTOR_GET_NUM_VALUES(a) ((a)->nv1 << 8 | (a)->nv2) + +struct avb_packet_mrp_footer { + uint16_t end_mark; +} __attribute__ ((__packed__)); + +/* applicant states */ +#define AVB_MRP_VO 0 /* Very anxious Observer */ +#define AVB_MRP_VP 1 /* Very anxious Passive */ +#define AVB_MRP_VN 2 /* Very anxious New */ +#define AVB_MRP_AN 3 /* Anxious New */ +#define AVB_MRP_AA 4 /* Anxious Active */ +#define AVB_MRP_QA 5 /* Quiet Active */ +#define AVB_MRP_LA 6 /* Leaving Active */ +#define AVB_MRP_AO 7 /* Anxious Observer */ +#define AVB_MRP_QO 8 /* Quiet Observer */ +#define AVB_MRP_AP 9 /* Anxious Passive */ +#define AVB_MRP_QP 10 /* Quiet Passive */ +#define AVB_MRP_LO 11 /* Leaving Observer */ + +/* registrar states */ +#define AVB_MRP_IN 16 +#define AVB_MRP_LV 17 +#define AVB_MRP_MT 18 + +/* events */ +#define AVB_MRP_EVENT_BEGIN 0 +#define AVB_MRP_EVENT_NEW 1 +#define AVB_MRP_EVENT_JOIN 2 +#define AVB_MRP_EVENT_LV 3 +#define AVB_MRP_EVENT_TX 4 +#define AVB_MRP_EVENT_TX_LVA 5 +#define AVB_MRP_EVENT_TX_LVAF 6 +#define AVB_MRP_EVENT_RX_NEW 7 +#define AVB_MRP_EVENT_RX_JOININ 8 +#define AVB_MRP_EVENT_RX_IN 9 +#define AVB_MRP_EVENT_RX_JOINMT 10 +#define AVB_MRP_EVENT_RX_MT 11 +#define AVB_MRP_EVENT_RX_LV 12 +#define AVB_MRP_EVENT_RX_LVA 13 +#define AVB_MRP_EVENT_FLUSH 14 +#define AVB_MRP_EVENT_REDECLARE 15 +#define AVB_MRP_EVENT_PERIODIC 16 +#define AVB_MRP_EVENT_LV_TIMER 17 +#define AVB_MRP_EVENT_LVA_TIMER 18 + +/* attribute events */ +#define AVB_MRP_ATTRIBUTE_EVENT_NEW 0 +#define AVB_MRP_ATTRIBUTE_EVENT_JOININ 1 +#define AVB_MRP_ATTRIBUTE_EVENT_IN 2 +#define AVB_MRP_ATTRIBUTE_EVENT_JOINMT 3 +#define AVB_MRP_ATTRIBUTE_EVENT_MT 4 +#define AVB_MRP_ATTRIBUTE_EVENT_LV 5 + +#define AVB_MRP_SEND_NEW 1 +#define AVB_MRP_SEND_JOININ 2 +#define AVB_MRP_SEND_IN 3 +#define AVB_MRP_SEND_JOINMT 4 +#define AVB_MRP_SEND_MT 5 +#define AVB_MRP_SEND_LV 6 + +#define AVB_MRP_NOTIFY_NEW 1 +#define AVB_MRP_NOTIFY_JOIN 2 +#define AVB_MRP_NOTIFY_LEAVE 3 + +const char *avb_mrp_notify_name(uint8_t notify); +const char *avb_mrp_send_name(uint8_t send); + +struct avb_mrp_attribute { + uint8_t pending_send; + void *user_data; +}; + +struct avb_mrp_attribute_events { +#define AVB_VERSION_MRP_ATTRIBUTE_EVENTS 0 + uint32_t version; + + void (*notify) (void *data, uint64_t now, uint8_t notify); +}; + +struct avb_mrp_attribute *avb_mrp_attribute_new(struct avb_mrp *mrp, + size_t user_size); +void avb_mrp_attribute_destroy(struct avb_mrp_attribute *attr); + +void avb_mrp_attribute_update_state(struct avb_mrp_attribute *attr, uint64_t now, int event); + +void avb_mrp_attribute_rx_event(struct avb_mrp_attribute *attr, uint64_t now, uint8_t event); + +void avb_mrp_attribute_begin(struct avb_mrp_attribute *attr, uint64_t now); +void avb_mrp_attribute_join(struct avb_mrp_attribute *attr, uint64_t now, bool is_new); +void avb_mrp_attribute_leave(struct avb_mrp_attribute *attr, uint64_t now); + +void avb_mrp_attribute_add_listener(struct avb_mrp_attribute *attr, struct spa_hook *listener, + const struct avb_mrp_attribute_events *events, void *data); + +struct avb_mrp_parse_info { +#define AVB_VERSION_MRP_PARSE_INFO 0 + uint32_t version; + + bool (*check_header) (void *data, const void *hdr, size_t *hdr_size, bool *has_params); + + int (*attr_event) (void *data, uint64_t now, uint8_t attribute_type, uint8_t event); + + int (*process) (void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index); +}; + +int avb_mrp_parse_packet(struct avb_mrp *mrp, uint64_t now, const void *pkt, int size, + const struct avb_mrp_parse_info *cb, void *data); + +struct avb_mrp_events { +#define AVB_VERSION_MRP_EVENTS 0 + uint32_t version; + + void (*event) (void *data, uint64_t now, uint8_t event); + + void (*notify) (void *data, uint64_t now, struct avb_mrp_attribute *attr, uint8_t notify); +}; + +struct avb_mrp *avb_mrp_new(struct server *server); +void avb_mrp_destroy(struct avb_mrp *mrp); + +void avb_mrp_add_listener(struct avb_mrp *mrp, struct spa_hook *listener, + const struct avb_mrp_events *events, void *data); + +#endif /* AVB_MRP_H */ diff --git a/src/modules/module-avb/msrp.c b/src/modules/module-avb/msrp.c new file mode 100644 index 0000000..85d3ff9 --- /dev/null +++ b/src/modules/module-avb/msrp.c @@ -0,0 +1,459 @@ +/* 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 <unistd.h> + +#include <spa/debug/mem.h> + +#include <pipewire/pipewire.h> + +#include "utils.h" +#include "msrp.h" + +static const uint8_t msrp_mac[6] = AVB_MSRP_MAC; + +struct attr { + struct avb_msrp_attribute attr; + struct msrp *msrp; + struct spa_hook listener; + struct spa_list link; +}; + +struct msrp { + struct server *server; + struct spa_hook server_listener; + struct spa_hook mrp_listener; + + struct spa_source *source; + + struct spa_list attributes; +}; + +static void debug_msrp_talker_common(const struct avb_packet_msrp_talker *t) +{ + char buf[128]; + pw_log_info(" stream-id: %s", avb_utils_format_id(buf, sizeof(buf), be64toh(t->stream_id))); + pw_log_info(" dest-addr: %s", avb_utils_format_addr(buf, sizeof(buf), t->dest_addr)); + pw_log_info(" vlan-id: %d", ntohs(t->vlan_id)); + pw_log_info(" tspec-max-frame-size: %d", ntohs(t->tspec_max_frame_size)); + pw_log_info(" tspec-max-interval-frames: %d", ntohs(t->tspec_max_interval_frames)); + pw_log_info(" priority: %d", t->priority); + pw_log_info(" rank: %d", t->rank); + pw_log_info(" accumulated-latency: %d", ntohl(t->accumulated_latency)); +} + +static void debug_msrp_talker(const struct avb_packet_msrp_talker *t) +{ + pw_log_info("talker"); + debug_msrp_talker_common(t); +} + +static void notify_talker(struct msrp *msrp, uint64_t now, struct attr *attr, uint8_t notify) +{ + pw_log_info("> notify talker: %s", avb_mrp_notify_name(notify)); + debug_msrp_talker(&attr->attr.attr.talker); +} + +static int process_talker(struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avb_packet_msrp_talker *t = m; + struct attr *a; + spa_list_for_each(a, &msrp->attributes, link) + if (a->attr.type == attr_type && + a->attr.attr.talker.stream_id == t->stream_id) { + a->attr.attr.talker = *t; + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + } + return 0; +} +static int encode_talker(struct msrp *msrp, struct attr *a, void *m) +{ + struct avb_packet_msrp_msg *msg = m; + struct avb_packet_mrp_vector *v; + struct avb_packet_msrp_talker *t; + struct avb_packet_mrp_footer *f; + uint8_t *ev; + size_t attr_list_length = sizeof(*v) + sizeof(*t) + sizeof(*f) + 1; + + msg->attribute_type = AVB_MSRP_ATTRIBUTE_TYPE_TALKER_ADVERTISE; + msg->attribute_length = sizeof(*t); + msg->attribute_list_length = htons(attr_list_length); + + v = (struct avb_packet_mrp_vector *)msg->attribute_list; + v->lva = 0; + AVB_MRP_VECTOR_SET_NUM_VALUES(v, 1); + + t = (struct avb_packet_msrp_talker *)v->first_value; + *t = a->attr.attr.talker; + + ev = SPA_PTROFF(t, sizeof(*t), uint8_t); + *ev = a->attr.mrp->pending_send * 6 * 6; + + f = SPA_PTROFF(ev, sizeof(*ev), struct avb_packet_mrp_footer); + f->end_mark = 0; + + return attr_list_length + sizeof(*msg); +} + + +static void debug_msrp_talker_fail(const struct avb_packet_msrp_talker_fail *t) +{ + char buf[128]; + pw_log_info("talker fail"); + debug_msrp_talker_common(&t->talker); + pw_log_info(" bridge-id: %s", avb_utils_format_id(buf, sizeof(buf), be64toh(t->bridge_id))); + pw_log_info(" failure-code: %d", t->failure_code); +} + +static int process_talker_fail(struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avb_packet_msrp_talker_fail *t = m; + struct attr *a; + + debug_msrp_talker_fail(t); + + spa_list_for_each(a, &msrp->attributes, link) + if (a->attr.type == attr_type && + a->attr.attr.talker_fail.talker.stream_id == t->talker.stream_id) + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + return 0; +} + +static void debug_msrp_listener(const struct avb_packet_msrp_listener *l, uint8_t param) +{ + char buf[128]; + pw_log_info("listener"); + pw_log_info(" %s", avb_utils_format_id(buf, sizeof(buf), be64toh(l->stream_id))); + pw_log_info(" %d", param); +} + +static void notify_listener(struct msrp *msrp, uint64_t now, struct attr *attr, uint8_t notify) +{ + pw_log_info("> notify listener: %s", avb_mrp_notify_name(notify)); + debug_msrp_listener(&attr->attr.attr.listener, attr->attr.param); +} + +static int process_listener(struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avb_packet_msrp_listener *l = m; + struct attr *a; + spa_list_for_each(a, &msrp->attributes, link) + if (a->attr.type == attr_type && + a->attr.attr.listener.stream_id == l->stream_id) + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + return 0; +} +static int encode_listener(struct msrp *msrp, struct attr *a, void *m) +{ + struct avb_packet_msrp_msg *msg = m; + struct avb_packet_mrp_vector *v; + struct avb_packet_msrp_listener *l; + struct avb_packet_mrp_footer *f; + uint8_t *ev; + size_t attr_list_length = sizeof(*v) + sizeof(*l) + sizeof(*f) + 1 + 1; + + msg->attribute_type = AVB_MSRP_ATTRIBUTE_TYPE_LISTENER; + msg->attribute_length = sizeof(*l); + msg->attribute_list_length = htons(attr_list_length); + + v = (struct avb_packet_mrp_vector *)msg->attribute_list; + v->lva = 0; + AVB_MRP_VECTOR_SET_NUM_VALUES(v, 1); + + l = (struct avb_packet_msrp_listener *)v->first_value; + *l = a->attr.attr.listener; + + ev = SPA_PTROFF(l, sizeof(*l), uint8_t); + *ev = a->attr.mrp->pending_send * 6 * 6; + + ev = SPA_PTROFF(ev, sizeof(*ev), uint8_t); + *ev = a->attr.param * 4 * 4 * 4; + + f = SPA_PTROFF(ev, sizeof(*ev), struct avb_packet_mrp_footer); + f->end_mark = 0; + + return attr_list_length + sizeof(*msg); +} + +static void debug_msrp_domain(const struct avb_packet_msrp_domain *d) +{ + pw_log_info("domain"); + pw_log_info(" id: %d", d->sr_class_id); + pw_log_info(" prio: %d", d->sr_class_priority); + pw_log_info(" vid: %d", ntohs(d->sr_class_vid)); +} + +static void notify_domain(struct msrp *msrp, uint64_t now, struct attr *attr, uint8_t notify) +{ + pw_log_info("> notify domain: %s", avb_mrp_notify_name(notify)); + debug_msrp_domain(&attr->attr.attr.domain); +} + +static int process_domain(struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + struct attr *a; + spa_list_for_each(a, &msrp->attributes, link) + if (a->attr.type == attr_type) + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + return 0; +} + +static int encode_domain(struct msrp *msrp, struct attr *a, void *m) +{ + struct avb_packet_msrp_msg *msg = m; + struct avb_packet_mrp_vector *v; + struct avb_packet_msrp_domain *d; + struct avb_packet_mrp_footer *f; + uint8_t *ev; + size_t attr_list_length = sizeof(*v) + sizeof(*d) + sizeof(*f) + 1; + + msg->attribute_type = AVB_MSRP_ATTRIBUTE_TYPE_DOMAIN; + msg->attribute_length = sizeof(*d); + msg->attribute_list_length = htons(attr_list_length); + + v = (struct avb_packet_mrp_vector *)msg->attribute_list; + v->lva = 0; + AVB_MRP_VECTOR_SET_NUM_VALUES(v, 1); + + d = (struct avb_packet_msrp_domain *)v->first_value; + *d = a->attr.attr.domain; + + ev = SPA_PTROFF(d, sizeof(*d), uint8_t); + *ev = a->attr.mrp->pending_send * 36; + + f = SPA_PTROFF(ev, sizeof(*ev), struct avb_packet_mrp_footer); + f->end_mark = 0; + + return attr_list_length + sizeof(*msg); +} + +static const struct { + const char *name; + int (*process) (struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num); + int (*encode) (struct msrp *msrp, struct attr *attr, void *m); + void (*notify) (struct msrp *msrp, uint64_t now, struct attr *attr, uint8_t notify); +} dispatch[] = { + [AVB_MSRP_ATTRIBUTE_TYPE_TALKER_ADVERTISE] = { "talker", process_talker, encode_talker, notify_talker, }, + [AVB_MSRP_ATTRIBUTE_TYPE_TALKER_FAILED] = { "talker-fail", process_talker_fail, NULL, NULL }, + [AVB_MSRP_ATTRIBUTE_TYPE_LISTENER] = { "listener", process_listener, encode_listener, notify_listener }, + [AVB_MSRP_ATTRIBUTE_TYPE_DOMAIN] = { "domain", process_domain, encode_domain, notify_domain, }, +}; + +static bool msrp_check_header(void *data, const void *hdr, size_t *hdr_size, bool *has_params) +{ + const struct avb_packet_msrp_msg *msg = hdr; + uint8_t attr_type = msg->attribute_type; + + if (!AVB_MSRP_ATTRIBUTE_TYPE_VALID(attr_type)) + return false; + + *hdr_size = sizeof(*msg); + *has_params = attr_type == AVB_MSRP_ATTRIBUTE_TYPE_LISTENER; + return true; +} + +static int msrp_attr_event(void *data, uint64_t now, uint8_t attribute_type, uint8_t event) +{ + struct msrp *msrp = data; + struct attr *a; + spa_list_for_each(a, &msrp->attributes, link) + if (a->attr.type == attribute_type) + avb_mrp_attribute_update_state(a->attr.mrp, now, event); + return 0; +} + +static int msrp_process(void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index) +{ + struct msrp *msrp = data; + return dispatch[attribute_type].process(msrp, now, + attribute_type, value, event, param, index); +} + +static const struct avb_mrp_parse_info info = { + AVB_VERSION_MRP_PARSE_INFO, + .check_header = msrp_check_header, + .attr_event = msrp_attr_event, + .process = msrp_process, +}; + + +static int msrp_message(struct msrp *msrp, uint64_t now, const void *message, int len) +{ + 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, +}; + +static void msrp_notify(void *data, uint64_t now, uint8_t notify) +{ + struct attr *a = data; + struct msrp *msrp = a->msrp; + return dispatch[a->attr.type].notify(msrp, now, a, notify); +} + +static const struct avb_mrp_attribute_events mrp_attr_events = { + AVB_VERSION_MRP_ATTRIBUTE_EVENTS, + .notify = msrp_notify, +}; + +struct avb_msrp_attribute *avb_msrp_attribute_new(struct avb_msrp *m, + uint8_t type) +{ + struct msrp *msrp = (struct msrp*)m; + struct avb_mrp_attribute *attr; + struct attr *a; + + attr = avb_mrp_attribute_new(msrp->server->mrp, sizeof(struct attr)); + + a = attr->user_data; + a->msrp = msrp; + a->attr.mrp = attr; + a->attr.type = type; + spa_list_append(&msrp->attributes, &a->link); + avb_mrp_attribute_add_listener(attr, &a->listener, &mrp_attr_events, a); + + return &a->attr; +} + +static void msrp_event(void *data, uint64_t now, uint8_t event) +{ + struct msrp *msrp = data; + uint8_t buffer[2048]; + struct avb_packet_mrp *p = (struct avb_packet_mrp*)buffer; + struct avb_packet_mrp_footer *f; + void *msg = SPA_PTROFF(buffer, sizeof(*p), void); + struct attr *a; + int len, count = 0; + size_t total = sizeof(*p) + 2; + + p->version = AVB_MRP_PROTOCOL_VERSION; + + spa_list_for_each(a, &msrp->attributes, link) { + if (!a->attr.mrp->pending_send) + continue; + if (dispatch[a->attr.type].encode == NULL) + continue; + + 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); + if (len < 0) + break; + + count++; + msg = SPA_PTROFF(msg, len, void); + total += len; + } + f = (struct avb_packet_mrp_footer *)msg; + f->end_mark = 0; + + if (count > 0) + avb_server_send_packet(msrp->server, msrp_mac, AVB_MSRP_ETH, + buffer, total); +} + +static const struct avb_mrp_events mrp_events = { + AVB_VERSION_MRP_EVENTS, + .event = msrp_event, +}; + +struct avb_msrp *avb_msrp_register(struct server *server) +{ + struct msrp *msrp; + int fd, res; + + 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/msrp.h b/src/modules/module-avb/msrp.h new file mode 100644 index 0000000..0922e6b --- /dev/null +++ b/src/modules/module-avb/msrp.h @@ -0,0 +1,134 @@ +/* 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 AVB_MSRP_H +#define AVB_MSRP_H + +#include "internal.h" +#include "mrp.h" + +#define AVB_MSRP_ETH 0x22ea +#define AVB_MSRP_MAC { 0x01, 0x80, 0xc2, 0x00, 0x00, 0xe }; + +#define AVB_MSRP_ATTRIBUTE_TYPE_TALKER_ADVERTISE 1 +#define AVB_MSRP_ATTRIBUTE_TYPE_TALKER_FAILED 2 +#define AVB_MSRP_ATTRIBUTE_TYPE_LISTENER 3 +#define AVB_MSRP_ATTRIBUTE_TYPE_DOMAIN 4 +#define AVB_MSRP_ATTRIBUTE_TYPE_VALID(t) ((t)>=1 && (t)<=4) + +struct avb_packet_msrp_msg { + uint8_t attribute_type; + uint8_t attribute_length; + uint16_t attribute_list_length; + uint8_t attribute_list[0]; +} __attribute__ ((__packed__)); + +#define AVB_MSRP_TSPEC_MAX_INTERVAL_FRAMES_DEFAULT 1 +#define AVB_MSRP_RANK_DEFAULT 1 +#define AVB_MSRP_PRIORITY_DEFAULT 3 + +struct avb_packet_msrp_talker { + uint64_t stream_id; + uint8_t dest_addr[6]; + uint16_t vlan_id; + uint16_t tspec_max_frame_size; + uint16_t tspec_max_interval_frames; +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned priority:3; + unsigned rank:1; + unsigned reserved:4; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned reserved:4; + unsigned rank:1; + unsigned priority:3; +#endif + uint32_t accumulated_latency; +} __attribute__ ((__packed__)); + +/* failure codes */ +#define AVB_MRP_FAIL_BANDWIDTH 1 +#define AVB_MRP_FAIL_BRIDGE 2 +#define AVB_MRP_FAIL_TC_BANDWIDTH 3 +#define AVB_MRP_FAIL_ID_BUSY 4 +#define AVB_MRP_FAIL_DSTADDR_BUSY 5 +#define AVB_MRP_FAIL_PREEMPTED 6 +#define AVB_MRP_FAIL_LATENCY_CHNG 7 +#define AVB_MRP_FAIL_PORT_NOT_AVB 8 +#define AVB_MRP_FAIL_DSTADDR_FULL 9 +#define AVB_MRP_FAIL_AVB_MRP_RESOURCE 10 +#define AVB_MRP_FAIL_MMRP_RESOURCE 11 +#define AVB_MRP_FAIL_DSTADDR_FAIL 12 +#define AVB_MRP_FAIL_PRIO_NOT_SR 13 +#define AVB_MRP_FAIL_FRAME_SIZE 14 +#define AVB_MRP_FAIL_FANIN_EXCEED 15 +#define AVB_MRP_FAIL_STREAM_CHANGE 16 +#define AVB_MRP_FAIL_VLAN_BLOCKED 17 +#define AVB_MRP_FAIL_VLAN_DISABLED 18 +#define AVB_MRP_FAIL_SR_PRIO_ERR 19 + +struct avb_packet_msrp_talker_fail { + struct avb_packet_msrp_talker talker; + uint64_t bridge_id; + uint8_t failure_code; +} __attribute__ ((__packed__)); + +struct avb_packet_msrp_listener { + uint64_t stream_id; +} __attribute__ ((__packed__)); + +/* domain discovery */ +#define AVB_MSRP_CLASS_ID_DEFAULT 6 +#define AVB_DEFAULT_VLAN 2 + +struct avb_packet_msrp_domain { + uint8_t sr_class_id; + uint8_t sr_class_priority; + uint16_t sr_class_vid; +} __attribute__ ((__packed__)); + +#define AVB_MSRP_LISTENER_PARAM_IGNORE 0 +#define AVB_MSRP_LISTENER_PARAM_ASKING_FAILED 1 +#define AVB_MSRP_LISTENER_PARAM_READY 2 +#define AVB_MSRP_LISTENER_PARAM_READY_FAILED 3 + +struct avb_msrp_attribute { + struct avb_mrp_attribute *mrp; + uint8_t type; + uint8_t param; + union { + struct avb_packet_msrp_talker talker; + struct avb_packet_msrp_talker_fail talker_fail; + struct avb_packet_msrp_listener listener; + struct avb_packet_msrp_domain domain; + } attr; +}; + +struct avb_msrp; + +struct avb_msrp_attribute *avb_msrp_attribute_new(struct avb_msrp *msrp, + uint8_t type); + +struct avb_msrp *avb_msrp_register(struct server *server); + +#endif /* AVB_MSRP_H */ diff --git a/src/modules/module-avb/mvrp.c b/src/modules/module-avb/mvrp.c new file mode 100644 index 0000000..2f5f6ea --- /dev/null +++ b/src/modules/module-avb/mvrp.c @@ -0,0 +1,297 @@ +/* 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 <unistd.h> + +#include <pipewire/pipewire.h> + +#include "mvrp.h" + +static const uint8_t mvrp_mac[6] = AVB_MVRP_MAC; + +struct attr { + struct avb_mvrp_attribute attr; + struct spa_hook listener; + struct spa_list link; + struct mvrp *mvrp; +}; + +struct mvrp { + struct server *server; + struct spa_hook server_listener; + struct spa_hook mrp_listener; + + struct spa_source *source; + + struct spa_list attributes; +}; + +static bool mvrp_check_header(void *data, const void *hdr, size_t *hdr_size, bool *has_params) +{ + const struct avb_packet_mvrp_msg *msg = hdr; + uint8_t attr_type = msg->attribute_type; + + if (!AVB_MVRP_ATTRIBUTE_TYPE_VALID(attr_type)) + return false; + + *hdr_size = sizeof(*msg); + *has_params = false; + return true; +} + +static int mvrp_attr_event(void *data, uint64_t now, uint8_t attribute_type, uint8_t event) +{ + struct mvrp *mvrp = data; + struct attr *a; + spa_list_for_each(a, &mvrp->attributes, link) + if (a->attr.type == attribute_type) + avb_mrp_attribute_rx_event(a->attr.mrp, now, event); + return 0; +} + +static void debug_vid(const struct avb_packet_mvrp_vid *t) +{ + pw_log_info("vid"); + pw_log_info(" %d", ntohs(t->vlan)); +} + +static int process_vid(struct mvrp *mvrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + return mvrp_attr_event(mvrp, now, attr_type, event); +} + +static int encode_vid(struct mvrp *mvrp, struct attr *a, void *m) +{ + struct avb_packet_mvrp_msg *msg = m; + struct avb_packet_mrp_vector *v; + struct avb_packet_mvrp_vid *d; + struct avb_packet_mrp_footer *f; + uint8_t *ev; + size_t attr_list_length = sizeof(*v) + sizeof(*d) + sizeof(*f) + 1; + + msg->attribute_type = AVB_MVRP_ATTRIBUTE_TYPE_VID; + msg->attribute_length = sizeof(*d); + + v = (struct avb_packet_mrp_vector *)msg->attribute_list; + v->lva = 0; + AVB_MRP_VECTOR_SET_NUM_VALUES(v, 1); + + d = (struct avb_packet_mvrp_vid *)v->first_value; + *d = a->attr.attr.vid; + + ev = SPA_PTROFF(d, sizeof(*d), uint8_t); + *ev = a->attr.mrp->pending_send * 36; + + f = SPA_PTROFF(ev, sizeof(*ev), struct avb_packet_mrp_footer); + f->end_mark = 0; + + return attr_list_length + sizeof(*msg); +} + +static void notify_vid(struct mvrp *mvrp, uint64_t now, struct attr *attr, uint8_t notify) +{ + pw_log_info("> notify vid: %s", avb_mrp_notify_name(notify)); + debug_vid(&attr->attr.attr.vid); +} + +static const struct { + const char *name; + int (*process) (struct mvrp *mvrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num); + int (*encode) (struct mvrp *mvrp, struct attr *attr, void *m); + void (*notify) (struct mvrp *mvrp, uint64_t now, struct attr *attr, uint8_t notify); +} dispatch[] = { + [AVB_MVRP_ATTRIBUTE_TYPE_VID] = { "vid", process_vid, encode_vid, notify_vid }, +}; + +static int mvrp_process(void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index) +{ + struct mvrp *mvrp = data; + return dispatch[attribute_type].process(mvrp, now, + attribute_type, value, event, param, index); +} + +static const struct avb_mrp_parse_info info = { + AVB_VERSION_MRP_PARSE_INFO, + .check_header = mvrp_check_header, + .attr_event = mvrp_attr_event, + .process = mvrp_process, +}; + +static int mvrp_message(struct mvrp *mvrp, uint64_t now, const void *message, int len) +{ + 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, +}; + +static void mvrp_notify(void *data, uint64_t now, uint8_t notify) +{ + struct attr *a = data; + struct mvrp *mvrp = a->mvrp; + return dispatch[a->attr.type].notify(mvrp, now, a, notify); +} + +static const struct avb_mrp_attribute_events mrp_attr_events = { + AVB_VERSION_MRP_ATTRIBUTE_EVENTS, + .notify = mvrp_notify, +}; + +struct avb_mvrp_attribute *avb_mvrp_attribute_new(struct avb_mvrp *m, + uint8_t type) +{ + struct mvrp *mvrp = (struct mvrp*)m; + struct avb_mrp_attribute *attr; + struct attr *a; + + attr = avb_mrp_attribute_new(mvrp->server->mrp, sizeof(struct attr)); + + a = attr->user_data; + a->attr.mrp = attr; + a->attr.type = type; + spa_list_append(&mvrp->attributes, &a->link); + avb_mrp_attribute_add_listener(attr, &a->listener, &mrp_attr_events, a); + + return &a->attr; +} + +static void mvrp_event(void *data, uint64_t now, uint8_t event) +{ + struct mvrp *mvrp = data; + uint8_t buffer[2048]; + struct avb_packet_mrp *p = (struct avb_packet_mrp*)buffer; + struct avb_packet_mrp_footer *f; + void *msg = SPA_PTROFF(buffer, sizeof(*p), void); + struct attr *a; + int len, count = 0; + size_t total = sizeof(*p) + 2; + + p->version = AVB_MRP_PROTOCOL_VERSION; + + spa_list_for_each(a, &mvrp->attributes, link) { + if (!a->attr.mrp->pending_send) + continue; + if (dispatch[a->attr.type].encode == NULL) + continue; + + 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(mvrp, a, msg); + if (len < 0) + break; + + count++; + msg = SPA_PTROFF(msg, len, void); + total += len; + } + f = (struct avb_packet_mrp_footer *)msg; + f->end_mark = 0; + + if (count > 0) + avb_server_send_packet(mvrp->server, mvrp_mac, AVB_MVRP_ETH, + buffer, total); +} + +static const struct avb_mrp_events mrp_events = { + AVB_VERSION_MRP_EVENTS, + .event = mvrp_event, +}; + +struct avb_mvrp *avb_mvrp_register(struct server *server) +{ + struct mvrp *mvrp; + int fd, res; + + 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); + avb_mrp_add_listener(server->mrp, &mvrp->mrp_listener, &mrp_events, mvrp); + + return (struct avb_mvrp*)mvrp; + +error_no_source: + free(mvrp); +error_close: + close(fd); + errno = -res; + return NULL; +} diff --git a/src/modules/module-avb/mvrp.h b/src/modules/module-avb/mvrp.h new file mode 100644 index 0000000..da3d5dc --- /dev/null +++ b/src/modules/module-avb/mvrp.h @@ -0,0 +1,62 @@ +/* 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 AVB_MVRP_H +#define AVB_MVRP_H + +#include "mrp.h" +#include "internal.h" + +#define AVB_MVRP_ETH 0x88f5 +#define AVB_MVRP_MAC { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x21 }; + +struct avb_packet_mvrp_msg { + uint8_t attribute_type; + uint8_t attribute_length; + uint8_t attribute_list[0]; +} __attribute__ ((__packed__)); + +#define AVB_MVRP_ATTRIBUTE_TYPE_VID 1 +#define AVB_MVRP_ATTRIBUTE_TYPE_VALID(t) ((t)==1) + +struct avb_packet_mvrp_vid { + uint16_t vlan; +} __attribute__ ((__packed__)); + +struct avb_mvrp; + +struct avb_mvrp_attribute { + struct avb_mrp_attribute *mrp; + uint8_t type; + union { + struct avb_packet_mvrp_vid vid; + } attr; +}; + +struct avb_mvrp_attribute *avb_mvrp_attribute_new(struct avb_mvrp *mvrp, + uint8_t type); + +struct avb_mvrp *avb_mvrp_register(struct server *server); + +#endif /* AVB_MVRP_H */ diff --git a/src/modules/module-avb/packets.h b/src/modules/module-avb/packets.h new file mode 100644 index 0000000..f35738a --- /dev/null +++ b/src/modules/module-avb/packets.h @@ -0,0 +1,101 @@ +/* Spa 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 AVB_PACKETS_H +#define AVB_PACKETS_H + +#include <arpa/inet.h> + +#define AVB_SUBTYPE_61883_IIDC 0x00 +#define AVB_SUBTYPE_MMA_STREAM 0x01 +#define AVB_SUBTYPE_AAF 0x02 +#define AVB_SUBTYPE_CVF 0x03 +#define AVB_SUBTYPE_CRF 0x04 +#define AVB_SUBTYPE_TSCF 0x05 +#define AVB_SUBTYPE_SVF 0x06 +#define AVB_SUBTYPE_RVF 0x07 +#define AVB_SUBTYPE_AEF_CONTINUOUS 0x6E +#define AVB_SUBTYPE_VSF_STREAM 0x6F +#define AVB_SUBTYPE_EF_STREAM 0x7F +#define AVB_SUBTYPE_NTSCF 0x82 +#define AVB_SUBTYPE_ESCF 0xEC +#define AVB_SUBTYPE_EECF 0xED +#define AVB_SUBTYPE_AEF_DISCRETE 0xEE +#define AVB_SUBTYPE_ADP 0xFA +#define AVB_SUBTYPE_AECP 0xFB +#define AVB_SUBTYPE_ACMP 0xFC +#define AVB_SUBTYPE_MAAP 0xFE +#define AVB_SUBTYPE_EF_CONTROL 0xFF + +struct avb_ethernet_header { + uint8_t dest[6]; + uint8_t src[6]; + uint16_t type; +} __attribute__ ((__packed__)); + +struct avb_frame_header { + uint8_t dest[6]; + uint8_t src[6]; + uint16_t type; /* 802.1Q Virtual Lan 0x8100 */ + uint16_t prio_cfi_id; + uint16_t etype; +} __attribute__ ((__packed__)); + +struct avb_packet_header { + uint8_t subtype; +#if __BYTE_ORDER == __BIG_ENDIAN + unsigned sv:1; /* stream_id valid */ + unsigned version:3; + unsigned subtype_data1:4; + + unsigned subtype_data2:5; + unsigned len1:3; +#elif __BYTE_ORDER == __LITTLE_ENDIAN + unsigned subtype_data1:4; + unsigned version:3; + unsigned sv:1; + + unsigned len1:3; + unsigned subtype_data2:5; +#elif +#error "Unknown byte order" +#endif + uint8_t len2:8; +} __attribute__ ((__packed__)); + +#define AVB_PACKET_SET_SUBTYPE(p,v) ((p)->subtype = (v)) +#define AVB_PACKET_SET_SV(p,v) ((p)->sv = (v)) +#define AVB_PACKET_SET_VERSION(p,v) ((p)->version = (v)) +#define AVB_PACKET_SET_SUB1(p,v) ((p)->subtype_data1 = (v)) +#define AVB_PACKET_SET_SUB2(p,v) ((p)->subtype_data2 = (v)) +#define AVB_PACKET_SET_LENGTH(p,v) ((p)->len1 = ((v) >> 8),(p)->len2 = (v)) + +#define AVB_PACKET_GET_SUBTYPE(p) ((p)->subtype) +#define AVB_PACKET_GET_SV(p) ((p)->sv) +#define AVB_PACKET_GET_VERSION(p) ((p)->version) +#define AVB_PACKET_GET_SUB1(p) ((p)->subtype_data1) +#define AVB_PACKET_GET_SUB2(p) ((p)->subtype_data2) +#define AVB_PACKET_GET_LENGTH(p) ((p)->len1 << 8 | (p)->len2) + +#endif /* AVB_PACKETS_H */ diff --git a/src/modules/module-avb/srp.c b/src/modules/module-avb/srp.c new file mode 100644 index 0000000..89d75f1 --- /dev/null +++ b/src/modules/module-avb/srp.c @@ -0,0 +1,59 @@ +/* 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 <pipewire/pipewire.h> + +#include "srp.h" + +struct srp { + struct server *server; + struct spa_hook server_listener; +}; + +static void srp_destroy(void *data) +{ + struct srp *srp = data; + spa_hook_remove(&srp->server_listener); + free(srp); +} + +static const struct server_events server_events = { + AVB_VERSION_SERVER_EVENTS, + .destroy = srp_destroy, +}; + +int avb_srp_register(struct server *server) +{ + struct srp *srp; + + srp = calloc(1, sizeof(*srp)); + if (srp == NULL) + return -errno; + + srp->server = server; + + avdecc_server_add_listener(server, &srp->server_listener, &server_events, srp); + + return 0; +} diff --git a/src/modules/module-avb/srp.h b/src/modules/module-avb/srp.h new file mode 100644 index 0000000..853321f --- /dev/null +++ b/src/modules/module-avb/srp.h @@ -0,0 +1,32 @@ +/* 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 AVB_SRP_H +#define AVB_SRP_H + +#include "internal.h" + +int avb_srp_register(struct server *server); + +#endif /* AVB_SRP_H */ diff --git a/src/modules/module-avb/stream.c b/src/modules/module-avb/stream.c new file mode 100644 index 0000000..0f5b148 --- /dev/null +++ b/src/modules/module-avb/stream.c @@ -0,0 +1,589 @@ +/* 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 <unistd.h> +#include <linux/if_ether.h> +#include <linux/if_packet.h> +#include <linux/net_tstamp.h> +#include <net/if.h> +#include <sys/ioctl.h> + +#include <spa/debug/mem.h> +#include <spa/pod/builder.h> +#include <spa/param/audio/format-utils.h> + +#include "iec61883.h" +#include "stream.h" +#include "utils.h" +#include "aecp-aem-descriptors.h" + +static void on_stream_destroy(void *d) +{ + struct stream *stream = d; + spa_hook_remove(&stream->stream_listener); + stream->stream = NULL; +} + +static void on_source_stream_process(void *data) +{ + struct stream *stream = data; + struct pw_buffer *buf; + struct spa_data *d; + uint32_t index, n_bytes; + int32_t avail, wanted; + + if ((buf = pw_stream_dequeue_buffer(stream->stream)) == NULL) { + pw_log_debug("out of buffers: %m"); + return; + } + + d = buf->buffer->datas; + + wanted = buf->requested ? buf->requested * stream->stride : d[0].maxsize; + + n_bytes = SPA_MIN(d[0].maxsize, (uint32_t)wanted); + + avail = spa_ringbuffer_get_read_index(&stream->ring, &index); + + if (avail < wanted) { + pw_log_debug("capture underrun %d < %d", avail, wanted); + memset(d[0].data, 0, n_bytes); + } else { + spa_ringbuffer_read_data(&stream->ring, + stream->buffer_data, + stream->buffer_size, + index % stream->buffer_size, + d[0].data, n_bytes); + index += n_bytes; + spa_ringbuffer_read_update(&stream->ring, index); + } + + d[0].chunk->size = n_bytes; + d[0].chunk->stride = stream->stride; + d[0].chunk->offset = 0; + buf->size = n_bytes / stream->stride; + + pw_stream_queue_buffer(stream->stream, buf); +} + +static const struct pw_stream_events source_stream_events = { + PW_VERSION_STREAM_EVENTS, + .destroy = on_stream_destroy, + .process = on_source_stream_process +}; + +static inline void +set_iovec(struct spa_ringbuffer *rbuf, void *buffer, uint32_t size, + uint32_t offset, struct iovec *iov, uint32_t len) +{ + iov[0].iov_len = SPA_MIN(len, size - offset); + iov[0].iov_base = SPA_PTROFF(buffer, offset, void); + iov[1].iov_len = len - iov[0].iov_len; + iov[1].iov_base = buffer; +} + +static int flush_write(struct stream *stream, uint64_t current_time) +{ + int32_t avail; + uint32_t index; + uint64_t ptime, txtime; + int pdu_count; + ssize_t n; + struct avb_frame_header *h = (void*)stream->pdu; + struct avb_packet_iec61883 *p = SPA_PTROFF(h, sizeof(*h), void); + uint8_t dbc; + + avail = spa_ringbuffer_get_read_index(&stream->ring, &index); + + pdu_count = (avail / stream->stride) / stream->frames_per_pdu; + + txtime = current_time + stream->t_uncertainty; + ptime = txtime + stream->mtt; + dbc = stream->dbc; + + while (pdu_count--) { + *(uint64_t*)CMSG_DATA(stream->cmsg) = txtime; + + set_iovec(&stream->ring, + stream->buffer_data, + stream->buffer_size, + index % stream->buffer_size, + &stream->iov[1], stream->payload_size); + + p->seq_num = stream->pdu_seq++; + p->tv = 1; + p->timestamp = ptime; + p->dbc = dbc; + + n = sendmsg(stream->source->fd, &stream->msg, MSG_NOSIGNAL); + if (n < 0 || n != (ssize_t)stream->pdu_size) { + pw_log_error("sendmsg() failed %zd != %zd: %m", + n, stream->pdu_size); + } + txtime += stream->pdu_period; + ptime += stream->pdu_period; + index += stream->payload_size; + dbc += stream->frames_per_pdu; + } + stream->dbc = dbc; + spa_ringbuffer_read_update(&stream->ring, index); + return 0; +} + +static void on_sink_stream_process(void *data) +{ + struct stream *stream = data; + struct pw_buffer *buf; + struct spa_data *d; + int32_t filled; + uint32_t index, offs, avail, size; + struct timespec now; + + if ((buf = pw_stream_dequeue_buffer(stream->stream)) == NULL) { + pw_log_debug("out of buffers: %m"); + return; + } + + d = buf->buffer->datas; + + offs = SPA_MIN(d[0].chunk->offset, d[0].maxsize); + size = SPA_MIN(d[0].chunk->size, d[0].maxsize - offs); + avail = size - offs; + + filled = spa_ringbuffer_get_write_index(&stream->ring, &index); + + if (filled >= (int32_t)stream->buffer_size) { + pw_log_warn("playback overrun %d >= %zd", filled, stream->buffer_size); + } else { + spa_ringbuffer_write_data(&stream->ring, + stream->buffer_data, + stream->buffer_size, + index % stream->buffer_size, + SPA_PTROFF(d[0].data, offs, void), avail); + index += avail; + spa_ringbuffer_write_update(&stream->ring, index); + } + pw_stream_queue_buffer(stream->stream, buf); + + clock_gettime(CLOCK_TAI, &now); + flush_write(stream, SPA_TIMESPEC_TO_NSEC(&now)); +} + +static void setup_pdu(struct stream *stream) +{ + struct avb_frame_header *h; + struct avb_packet_iec61883 *p; + ssize_t payload_size, hdr_size, pdu_size; + + spa_memzero(stream->pdu, sizeof(stream->pdu)); + h = (struct avb_frame_header*)stream->pdu; + p = SPA_PTROFF(h, sizeof(*h), void); + + hdr_size = sizeof(*h) + sizeof(*p); + payload_size = stream->stride * stream->frames_per_pdu; + pdu_size = hdr_size + payload_size; + + h->type = htons(0x8100); + h->prio_cfi_id = htons((stream->prio << 13) | stream->vlan_id); + h->etype = htons(0x22f0); + + if (stream->direction == SPA_DIRECTION_OUTPUT) { + p->subtype = AVB_SUBTYPE_61883_IIDC; + p->sv = 1; + p->stream_id = htobe64(stream->id); + p->data_len = htons(payload_size+8); + p->tag = 0x1; + p->channel = 0x1f; + p->tcode = 0xa; + p->sid = 0x3f; + p->dbs = stream->info.info.raw.channels; + p->qi2 = 0x2; + p->format_id = 0x10; + p->fdf = 0x2; + p->syt = htons(0x0008); + } + stream->hdr_size = hdr_size; + stream->payload_size = payload_size; + stream->pdu_size = pdu_size; +} + +static int setup_msg(struct stream *stream) +{ + stream->iov[0].iov_base = stream->pdu; + stream->iov[0].iov_len = stream->hdr_size; + stream->iov[1].iov_base = SPA_PTROFF(stream->pdu, stream->hdr_size, void); + stream->iov[1].iov_len = stream->payload_size; + stream->iov[2].iov_base = SPA_PTROFF(stream->pdu, stream->hdr_size, void); + stream->iov[2].iov_len = 0; + stream->msg.msg_name = &stream->sock_addr; + stream->msg.msg_namelen = sizeof(stream->sock_addr); + stream->msg.msg_iov = stream->iov; + stream->msg.msg_iovlen = 3; + stream->msg.msg_control = stream->control; + stream->msg.msg_controllen = sizeof(stream->control); + stream->cmsg = CMSG_FIRSTHDR(&stream->msg); + stream->cmsg->cmsg_level = SOL_SOCKET; + stream->cmsg->cmsg_type = SCM_TXTIME; + stream->cmsg->cmsg_len = CMSG_LEN(sizeof(__u64)); + return 0; +} + +static const struct pw_stream_events sink_stream_events = { + PW_VERSION_STREAM_EVENTS, + .destroy = on_stream_destroy, + .process = on_sink_stream_process +}; + +struct stream *server_create_stream(struct server *server, + enum spa_direction direction, uint16_t index) +{ + struct stream *stream; + const struct descriptor *desc; + uint32_t n_params; + const struct spa_pod *params[1]; + uint8_t buffer[1024]; + struct spa_pod_builder b; + int res; + + desc = server_find_descriptor(server, + direction == SPA_DIRECTION_INPUT ? + AVB_AEM_DESC_STREAM_INPUT : + AVB_AEM_DESC_STREAM_OUTPUT, index); + if (desc == NULL) + return NULL; + + stream = calloc(1, sizeof(*stream)); + if (stream == NULL) + return NULL; + + stream->server = server; + stream->direction = direction; + stream->index = index; + stream->desc = desc; + spa_list_append(&server->streams, &stream->link); + + stream->prio = AVB_MSRP_PRIORITY_DEFAULT; + stream->vlan_id = AVB_DEFAULT_VLAN; + + stream->id = (uint64_t)server->mac_addr[0] << 56 | + (uint64_t)server->mac_addr[1] << 48 | + (uint64_t)server->mac_addr[2] << 40 | + (uint64_t)server->mac_addr[3] << 32 | + (uint64_t)server->mac_addr[4] << 24 | + (uint64_t)server->mac_addr[5] << 16 | + htons(index); + + stream->vlan_attr = avb_mvrp_attribute_new(server->mvrp, + AVB_MVRP_ATTRIBUTE_TYPE_VID); + stream->vlan_attr->attr.vid.vlan = htons(stream->vlan_id); + + stream->buffer_data = calloc(1, BUFFER_SIZE); + stream->buffer_size = BUFFER_SIZE; + spa_ringbuffer_init(&stream->ring); + + if (direction == SPA_DIRECTION_INPUT) { + stream->stream = pw_stream_new(server->impl->core, "source", + pw_properties_new( + PW_KEY_MEDIA_CLASS, "Audio/Source", + PW_KEY_NODE_NAME, "avb.source", + PW_KEY_NODE_DESCRIPTION, "AVB Source", + PW_KEY_NODE_WANT_DRIVER, "true", + NULL)); + } else { + stream->stream = pw_stream_new(server->impl->core, "sink", + pw_properties_new( + PW_KEY_MEDIA_CLASS, "Audio/Sink", + PW_KEY_NODE_NAME, "avb.sink", + PW_KEY_NODE_DESCRIPTION, "AVB Sink", + PW_KEY_NODE_WANT_DRIVER, "true", + NULL)); + } + if (stream->stream == NULL) + goto error_free; + + pw_stream_add_listener(stream->stream, + &stream->stream_listener, + direction == SPA_DIRECTION_INPUT ? + &source_stream_events : + &sink_stream_events, + stream); + + stream->info.info.raw.format = SPA_AUDIO_FORMAT_S24_32_BE; + stream->info.info.raw.flags = SPA_AUDIO_FLAG_UNPOSITIONED; + stream->info.info.raw.rate = 48000; + stream->info.info.raw.channels = 8; + stream->stride = stream->info.info.raw.channels * 4; + + n_params = 0; + spa_pod_builder_init(&b, buffer, sizeof(buffer)); + params[n_params++] = spa_format_audio_raw_build(&b, + SPA_PARAM_EnumFormat, &stream->info.info.raw); + + if ((res = pw_stream_connect(stream->stream, + pw_direction_reverse(direction), + PW_ID_ANY, + PW_STREAM_FLAG_MAP_BUFFERS | + PW_STREAM_FLAG_INACTIVE | + PW_STREAM_FLAG_RT_PROCESS, + params, n_params)) < 0) + goto error_free_stream; + + stream->frames_per_pdu = 6; + stream->pdu_period = SPA_NSEC_PER_SEC * stream->frames_per_pdu / + stream->info.info.raw.rate; + + setup_pdu(stream); + setup_msg(stream); + + stream->listener_attr = avb_msrp_attribute_new(server->msrp, + AVB_MSRP_ATTRIBUTE_TYPE_LISTENER); + stream->talker_attr = avb_msrp_attribute_new(server->msrp, + AVB_MSRP_ATTRIBUTE_TYPE_TALKER_ADVERTISE); + stream->talker_attr->attr.talker.vlan_id = htons(stream->vlan_id); + stream->talker_attr->attr.talker.tspec_max_frame_size = htons(32 + stream->frames_per_pdu * stream->stride); + stream->talker_attr->attr.talker.tspec_max_interval_frames = + htons(AVB_MSRP_TSPEC_MAX_INTERVAL_FRAMES_DEFAULT); + stream->talker_attr->attr.talker.priority = stream->prio; + stream->talker_attr->attr.talker.rank = AVB_MSRP_RANK_DEFAULT; + stream->talker_attr->attr.talker.accumulated_latency = htonl(95); + + return stream; + +error_free_stream: + pw_stream_destroy(stream->stream); + errno = -res; +error_free: + free(stream); + return NULL; +} + +void stream_destroy(struct stream *stream) +{ + avb_mrp_attribute_destroy(stream->listener_attr->mrp); + spa_list_remove(&stream->link); + free(stream); +} + +static int setup_socket(struct stream *stream) +{ + struct server *server = stream->server; + int fd, res; + char buf[128]; + struct ifreq req; + + fd = socket(AF_PACKET, SOCK_RAW | SOCK_NONBLOCK, htons(ETH_P_ALL)); + if (fd < 0) { + pw_log_error("socket() failed: %m"); + return -errno; + } + + spa_zero(req); + snprintf(req.ifr_name, sizeof(req.ifr_name), "%s", server->ifname); + res = ioctl(fd, SIOCGIFINDEX, &req); + if (res < 0) { + pw_log_error("SIOCGIFINDEX %s failed: %m", server->ifname); + res = -errno; + goto error_close; + } + + spa_zero(stream->sock_addr); + stream->sock_addr.sll_family = AF_PACKET; + stream->sock_addr.sll_protocol = htons(ETH_P_TSN); + stream->sock_addr.sll_ifindex = req.ifr_ifindex; + + if (stream->direction == SPA_DIRECTION_OUTPUT) { + struct sock_txtime txtime_cfg; + + res = setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &stream->prio, + sizeof(stream->prio)); + if (res < 0) { + pw_log_error("setsockopt(SO_PRIORITY %d) failed: %m", stream->prio); + res = -errno; + goto error_close; + } + + txtime_cfg.clockid = CLOCK_TAI; + txtime_cfg.flags = 0; + res = setsockopt(fd, SOL_SOCKET, SO_TXTIME, &txtime_cfg, + sizeof(txtime_cfg)); + if (res < 0) { + pw_log_error("setsockopt(SO_TXTIME) failed: %m"); + res = -errno; + goto error_close; + } + } else { + struct packet_mreq mreq; + + res = bind(fd, (struct sockaddr *) &stream->sock_addr, sizeof(stream->sock_addr)); + if (res < 0) { + pw_log_error("bind() failed: %m"); + res = -errno; + goto error_close; + } + + spa_zero(mreq); + mreq.mr_ifindex = req.ifr_ifindex; + mreq.mr_type = PACKET_MR_MULTICAST; + mreq.mr_alen = ETH_ALEN; + memcpy(&mreq.mr_address, stream->addr, ETH_ALEN); + res = setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, + &mreq, sizeof(struct packet_mreq)); + + pw_log_info("join %s", avb_utils_format_addr(buf, 128, stream->addr)); + + if (res < 0) { + pw_log_error("setsockopt(ADD_MEMBERSHIP) failed: %m"); + res = -errno; + goto error_close; + } + } + return fd; + +error_close: + close(fd); + return res; +} + +static void handle_iec61883_packet(struct stream *stream, + struct avb_packet_iec61883 *p, int len) +{ + uint32_t index, n_bytes; + int32_t filled; + + filled = spa_ringbuffer_get_write_index(&stream->ring, &index); + n_bytes = ntohs(p->data_len) - 8; + + if (filled + n_bytes > stream->buffer_size) { + pw_log_debug("capture overrun"); + } else { + spa_ringbuffer_write_data(&stream->ring, + stream->buffer_data, + stream->buffer_size, + index % stream->buffer_size, + p->payload, n_bytes); + index += n_bytes; + spa_ringbuffer_write_update(&stream->ring, index); + } +} + +static void on_socket_data(void *data, int fd, uint32_t mask) +{ + struct stream *stream = data; + + 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 { + struct avb_frame_header *h = (void*)buffer; + struct avb_packet_iec61883 *p = SPA_PTROFF(h, sizeof(*h), void); + + if (memcmp(h->dest, stream->addr, 6) != 0 || + p->subtype != AVB_SUBTYPE_61883_IIDC) + return; + + handle_iec61883_packet(stream, p, len - sizeof(*h)); + } + } +} + +int stream_activate(struct stream *stream, uint64_t now) +{ + struct server *server = stream->server; + struct avb_frame_header *h = (void*)stream->pdu; + int fd, res; + + if (stream->source == NULL) { + if ((fd = setup_socket(stream)) < 0) + return fd; + + stream->source = pw_loop_add_io(server->impl->loop, fd, + SPA_IO_IN, true, on_socket_data, stream); + if (stream->source == NULL) { + res = -errno; + pw_log_error("stream %p: can't create source: %m", stream); + close(fd); + return res; + } + } + + avb_mrp_attribute_begin(stream->vlan_attr->mrp, now); + avb_mrp_attribute_join(stream->vlan_attr->mrp, now, true); + + if (stream->direction == SPA_DIRECTION_INPUT) { + stream->listener_attr->attr.listener.stream_id = htobe64(stream->peer_id); + stream->listener_attr->param = AVB_MSRP_LISTENER_PARAM_READY; + avb_mrp_attribute_begin(stream->listener_attr->mrp, now); + avb_mrp_attribute_join(stream->listener_attr->mrp, now, true); + + 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); + + stream->talker_attr->attr.talker.stream_id = htobe64(stream->id); + memcpy(stream->talker_attr->attr.talker.dest_addr, stream->addr, 6); + + stream->sock_addr.sll_halen = ETH_ALEN; + memcpy(&stream->sock_addr.sll_addr, stream->addr, ETH_ALEN); + memcpy(h->dest, stream->addr, 6); + memcpy(h->src, server->mac_addr, 6); + avb_mrp_attribute_begin(stream->talker_attr->mrp, now); + avb_mrp_attribute_join(stream->talker_attr->mrp, now, true); + } + pw_stream_set_active(stream->stream, true); + return 0; +} + +int stream_deactivate(struct stream *stream, uint64_t now) +{ + pw_stream_set_active(stream->stream, false); + + if (stream->source != NULL) { + pw_loop_destroy_source(stream->server->impl->loop, stream->source); + stream->source = NULL; + } + + avb_mrp_attribute_leave(stream->vlan_attr->mrp, now); + + if (stream->direction == SPA_DIRECTION_INPUT) { + avb_mrp_attribute_leave(stream->listener_attr->mrp, now); + } else { + avb_mrp_attribute_leave(stream->talker_attr->mrp, now); + } + return 0; +} diff --git a/src/modules/module-avb/stream.h b/src/modules/module-avb/stream.h new file mode 100644 index 0000000..7062e25 --- /dev/null +++ b/src/modules/module-avb/stream.h @@ -0,0 +1,104 @@ +/* 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 AVB_STREAM_H +#define AVB_STREAM_H + +#include <sys/socket.h> +#include <sys/types.h> +#include <linux/if_packet.h> +#include <net/if.h> + +#include <spa/utils/ringbuffer.h> +#include <spa/param/audio/format.h> + +#include <pipewire/pipewire.h> + +#define BUFFER_SIZE (1u<<16) +#define BUFFER_MASK (BUFFER_SIZE-1) + +struct stream { + struct spa_list link; + + struct server *server; + + uint16_t direction; + uint16_t index; + const struct descriptor *desc; + uint64_t id; + uint64_t peer_id; + + struct pw_stream *stream; + struct spa_hook stream_listener; + + uint8_t addr[6]; + struct spa_source *source; + int prio; + int vlan_id; + int mtt; + int t_uncertainty; + uint32_t frames_per_pdu; + int ptime_tolerance; + + uint8_t pdu[2048]; + size_t hdr_size; + size_t payload_size; + size_t pdu_size; + int64_t pdu_period; + uint8_t pdu_seq; + uint8_t prev_seq; + uint8_t dbc; + + struct iovec iov[3]; + struct sockaddr_ll sock_addr; + struct msghdr msg; + char control[CMSG_SPACE(sizeof(uint64_t))]; + struct cmsghdr *cmsg; + + struct spa_ringbuffer ring; + void *buffer_data; + size_t buffer_size; + + uint64_t format; + uint32_t stride; + struct spa_audio_info info; + + struct avb_msrp_attribute *talker_attr; + struct avb_msrp_attribute *listener_attr; + struct avb_mvrp_attribute *vlan_attr; +}; + +#include "msrp.h" +#include "mvrp.h" +#include "maap.h" + +struct stream *server_create_stream(struct server *server, + enum spa_direction direction, uint16_t index); + +void stream_destroy(struct stream *stream); + +int stream_activate(struct stream *stream, uint64_t now); +int stream_deactivate(struct stream *stream, uint64_t now); + +#endif /* AVB_STREAM_H */ diff --git a/src/modules/module-avb/utils.h b/src/modules/module-avb/utils.h new file mode 100644 index 0000000..f626722 --- /dev/null +++ b/src/modules/module-avb/utils.h @@ -0,0 +1,86 @@ +/* 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 AVB_UTILS_H +#define AVB_UTILS_H + +#include <spa/utils/json.h> + +#include "internal.h" + +static inline char *avb_utils_format_id(char *str, size_t size, const uint64_t id) +{ + snprintf(str, size, "%02x:%02x:%02x:%02x:%02x:%02x:%04x", + (uint8_t)(id >> 56), + (uint8_t)(id >> 48), + (uint8_t)(id >> 40), + (uint8_t)(id >> 32), + (uint8_t)(id >> 24), + (uint8_t)(id >> 16), + (uint16_t)(id)); + return str; +} + +static inline int avb_utils_parse_id(const char *str, int len, uint64_t *id) +{ + char s[64]; + uint8_t v[6]; + uint16_t unique_id; + if (spa_json_parse_stringn(str, len, s, sizeof(s)) <= 0) + return -EINVAL; + if (sscanf(s, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx:%hx", + &v[0], &v[1], &v[2], &v[3], + &v[4], &v[5], &unique_id) == 7) { + *id = (uint64_t) v[0] << 56 | + (uint64_t) v[1] << 48 | + (uint64_t) v[2] << 40 | + (uint64_t) v[3] << 32 | + (uint64_t) v[4] << 24 | + (uint64_t) v[5] << 16 | + unique_id; + } else if (!spa_atou64(str, id, 0)) + return -EINVAL; + return 0; +} + +static inline char *avb_utils_format_addr(char *str, size_t size, const uint8_t addr[6]) +{ + snprintf(str, size, "%02x:%02x:%02x:%02x:%02x:%02x", + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); + return str; +} +static inline int avb_utils_parse_addr(const char *str, int len, uint8_t addr[6]) +{ + char s[64]; + uint8_t v[6]; + if (spa_json_parse_stringn(str, len, s, sizeof(s)) <= 0) + return -EINVAL; + if (sscanf(s, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", + &v[0], &v[1], &v[2], &v[3], &v[4], &v[5]) != 6) + return -EINVAL; + memcpy(addr, v, 6); + return 0; +} + +#endif /* AVB_UTILS_H */ |