diff --git a/src/modules/meson.build b/src/modules/meson.build index 1f15511b1..c3c7aea47 100644 --- a/src/modules/meson.build +++ b/src/modules/meson.build @@ -524,6 +524,7 @@ pipewire_module_avbtp = shared_library('pipewire-module-avbtp', [ 'module-avbtp.c', 'module-avbtp/avb.c', 'module-avbtp/adp.c', + 'module-avbtp/acmp.c', 'module-avbtp/aecp.c', 'module-avbtp/aecp-aem.c', 'module-avbtp/avdecc.c', diff --git a/src/modules/module-avbtp/acmp.c b/src/modules/module-avbtp/acmp.c new file mode 100644 index 000000000..06e3fffe7 --- /dev/null +++ b/src/modules/module-avbtp/acmp.c @@ -0,0 +1,167 @@ +/* AVB support + * + * Copyright © 2022 Wim Taymans + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice (including the next + * paragraph) shall be included in all copies or substantial portions of the + * Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +#include +#include + +#include + +#include "acmp.h" +#include "internal.h" + +struct acmp { + struct server *server; + struct spa_hook server_listener; +}; + +struct msg_info { + uint16_t type; + const char *name; + int (*handle) (struct acmp *acmp, const void *p, int len); +}; + +static int reply_not_supported(struct acmp *acmp, const void *p, int len) +{ + struct server *server = acmp->server; + uint8_t buf[len]; + struct avbtp_packet_acmp *reply = (struct avbtp_packet_acmp*)buf; + + memcpy(reply, p, len); + AVBTP_PACKET_ACMP_SET_STATUS(reply, AVBTP_ACMP_STATUS_NOT_SUPPORTED); + + return avbtp_server_send_packet(server, reply->hdr.eth.src, reply, len); +} + +static const struct msg_info msg_info[] = { + { AVBTP_ACMP_MESSAGE_TYPE_CONNECT_TX_COMMAND, "connect-tx-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE, "connect-tx-response", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_TX_COMMAND, "disconnect-tx-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_TX_RESPONSE, "disconnect-tx-response", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_GET_TX_STATE_COMMAND, "get-tx-state-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE, "get-tx-state-response", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_CONNECT_RX_COMMAND, "connect-rx-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE, "connect-rx-response", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_RX_COMMAND, "disconnect-rx-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE, "disconnect-rx-response", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_GET_RX_STATE_COMMAND, "get-rx-state-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE, "get-rx-state-response", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_COMMAND, "get-tx-connection-command", NULL, }, + { AVBTP_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE, "get-tx-connection-response", NULL, }, +}; + +static inline const struct msg_info *find_msg_info(uint16_t type, const char *name) +{ + uint32_t i; + for (i = 0; i < SPA_N_ELEMENTS(msg_info); i++) { + if ((name == NULL && type == msg_info[i].type) || + (name != NULL && spa_streq(name, msg_info[i].name))) + return &msg_info[i]; + } + return NULL; +} + +static int acmp_message(void *data, uint64_t now, const void *message, int len) +{ + struct acmp *acmp = data; + const struct avbtp_packet_acmp *p = message; + const struct msg_info *info; + int message_type; + + if (AVBTP_PACKET_GET_SUBTYPE(&p->hdr) != AVBTP_SUBTYPE_ACMP) + return 0; + + message_type = AVBTP_PACKET_ACMP_GET_MESSAGE_TYPE(p); + + info = find_msg_info(message_type, NULL); + if (info == NULL) + return reply_not_supported(acmp, p, len); + + pw_log_info("got ACMP message %s", info->name); + + if (info->handle == NULL) + return reply_not_supported(acmp, p, len); + + return info->handle(acmp, p, len); +} + +static void acmp_destroy(void *data) +{ + struct acmp *acmp = data; + spa_hook_remove(&acmp->server_listener); + free(acmp); +} + +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 = { + AVBTP_VERSION_SERVER_EVENTS, + .destroy = acmp_destroy, + .message = acmp_message, + .command = acmp_command +}; + +struct avbtp_acmp *avbtp_acmp_register(struct server *server) +{ + struct acmp *acmp; + + acmp = calloc(1, sizeof(*acmp)); + if (acmp == NULL) + return NULL; + + acmp->server = server; + + avdecc_server_add_listener(server, &acmp->server_listener, &server_events, acmp); + + return (struct avbtp_acmp*)acmp; +} + +void avbtp_acmp_unregister(struct avbtp_acmp *acmp) +{ + acmp_destroy(acmp); +} diff --git a/src/modules/module-avbtp/acmp.h b/src/modules/module-avbtp/acmp.h new file mode 100644 index 000000000..ebef9dbbe --- /dev/null +++ b/src/modules/module-avbtp/acmp.h @@ -0,0 +1,91 @@ +/* AVB support + * + * Copyright © 2022 Wim Taymans + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice (including the next + * paragraph) shall be included in all copies or substantial portions of the + * Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +#ifndef AVBTP_ACMP_H +#define AVBTP_ACMP_H + +#include "packets.h" +#include "internal.h" + +#define AVBTP_ACMP_MESSAGE_TYPE_CONNECT_TX_COMMAND 0 +#define AVBTP_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE 1 +#define AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_TX_COMMAND 2 +#define AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_TX_RESPONSE 3 +#define AVBTP_ACMP_MESSAGE_TYPE_GET_TX_STATE_COMMAND 4 +#define AVBTP_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE 5 +#define AVBTP_ACMP_MESSAGE_TYPE_CONNECT_RX_COMMAND 6 +#define AVBTP_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE 7 +#define AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_RX_COMMAND 8 +#define AVBTP_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE 9 +#define AVBTP_ACMP_MESSAGE_TYPE_GET_RX_STATE_COMMAND 10 +#define AVBTP_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE 11 +#define AVBTP_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_COMMAND 12 +#define AVBTP_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE 13 + +#define AVBTP_ACMP_STATUS_SUCCESS 0 +#define AVBTP_ACMP_STATUS_LISTENER_UNKNOWN_ID 1 +#define AVBTP_ACMP_STATUS_TALKER_UNKNOWN_ID 2 +#define AVBTP_ACMP_STATUS_TALKER_DEST_MAC_FAIL 3 +#define AVBTP_ACMP_STATUS_TALKER_NO_STREAM_INDEX 4 +#define AVBTP_ACMP_STATUS_TALKER_NO_BANDWIDTH 5 +#define AVBTP_ACMP_STATUS_TALKER_EXCLUSIVE 6 +#define AVBTP_ACMP_STATUS_LISTENER_TALKER_TIMEOUT 7 +#define AVBTP_ACMP_STATUS_LISTENER_EXCLUSIVE 8 +#define AVBTP_ACMP_STATUS_STATE_UNAVAILABLE 9 +#define AVBTP_ACMP_STATUS_NOT_CONNECTED 10 +#define AVBTP_ACMP_STATUS_NO_SUCH_CONNECTION 11 +#define AVBTP_ACMP_STATUS_COULD_NOT_SEND_MESSAGE 12 +#define AVBTP_ACMP_STATUS_TALKER_MISBEHAVING 13 +#define AVBTP_ACMP_STATUS_LISTENER_MISBEHAVING 14 +#define AVBTP_ACMP_STATUS_RESERVED 15 +#define AVBTP_ACMP_STATUS_CONTROLLER_NOT_AUTHORIZED 16 +#define AVBTP_ACMP_STATUS_INCOMPATIBLE_REQUEST 17 +#define AVBTP_ACMP_STATUS_LISTENER_INVALID_CONNECTION 18 +#define AVBTP_ACMP_STATUS_NOT_SUPPORTED 31 + +struct avbtp_packet_acmp { + struct avbtp_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 AVBTP_PACKET_ACMP_SET_MESSAGE_TYPE(p,v) AVBTP_PACKET_SET_SUB1(&(p)->hdr, v) +#define AVBTP_PACKET_ACMP_SET_STATUS(p,v) AVBTP_PACKET_SET_SUB2(&(p)->hdr, v) + +#define AVBTP_PACKET_ACMP_GET_MESSAGE_TYPE(p) AVBTP_PACKET_GET_SUB1(&(p)->hdr) +#define AVBTP_PACKET_ACMP_GET_STATUS(p) AVBTP_PACKET_GET_SUB2(&(p)->hdr) + +struct avbtp_acmp *avbtp_acmp_register(struct server *server); + +#endif /* AVBTP_ACMP_H */ diff --git a/src/modules/module-avbtp/adp.c b/src/modules/module-avbtp/adp.c index b99267cc5..68b18e6e9 100644 --- a/src/modules/module-avbtp/adp.c +++ b/src/modules/module-avbtp/adp.c @@ -27,7 +27,9 @@ #include #include "adp.h" +#include "aecp-aem-descriptors.h" #include "internal.h" +#include "utils.h" struct entity { struct spa_list link; @@ -41,7 +43,6 @@ struct adp { struct spa_hook server_listener; struct spa_list entities; - uint64_t now; uint32_t available_index; }; @@ -49,7 +50,7 @@ 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 (AVBTP_PACKET_ADP_GET_ENTITY_ID(&e->packet) == id) + if (be64toh(e->packet.entity_id) == id) return e; return NULL; } @@ -59,154 +60,19 @@ static void entity_free(struct entity *e) free(e); } -struct bit_info { - uint32_t bits; - const char *value; - const char *description; -}; - -static const struct bit_info entity_capabilities_info[] = { - { AVBTP_ADP_ENTITY_CAPABILITY_EFU_MODE, "efu-mode", "EFU Mode" }, - { AVBTP_ADP_ENTITY_CAPABILITY_ADDRESS_ACCESS_SUPPORTED, "address-access-supported", "Address Access Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_GATEWAY_ENTITY, "gateway-entity", "Gateway Entity" }, - { AVBTP_ADP_ENTITY_CAPABILITY_AEM_SUPPORTED, "aem-supported", "AEM Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_LEGACY_AVC, "legacy-avc", "Legacy AVC" }, - { AVBTP_ADP_ENTITY_CAPABILITY_ASSOCIATION_ID_SUPPORTED, "association-id-supported", "Association Id Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_ASSOCIATION_ID_VALID, "association-id-valid", "Association Id Valid" }, - { AVBTP_ADP_ENTITY_CAPABILITY_VENDOR_UNIQUE_SUPPORTED, "vandor-unique-suported", "Vendor Unique Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_CLASS_A_SUPPORTED, "class-a-supported", "Class A Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_CLASS_B_SUPPORTED, "class-b-supported", "Class B Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_GPTP_SUPPORTED, "gptp-supported", "gPTP Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_AEM_AUTHENTICATION_SUPPORTED, "aem-authentication-supported", "AEM Authentication Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_AEM_AUTHENTICATION_REQUIRED, "aem-authentication-required", "AEM Authentication Required" }, - { AVBTP_ADP_ENTITY_CAPABILITY_AEM_PERSISTENT_ACQUIRE_SUPPORTED, "aem-persistent-acquire-supported", "AEM Persisitent Acquire Supported" }, - { AVBTP_ADP_ENTITY_CAPABILITY_AEM_IDENTIFY_CONTROL_INDEX_VALID, "aem-identify-control-index-valid", "AEM Identify Control Index Valid" }, - { AVBTP_ADP_ENTITY_CAPABILITY_AEM_INTERFACE_INDEX_VALID, "aem-interface-index-valid", "AEM Interface Index Valid" }, - { AVBTP_ADP_ENTITY_CAPABILITY_GENERAL_CONTROLLER_IGNORE, "general-controller-ignore", "General Controller Ignore" }, - { AVBTP_ADP_ENTITY_CAPABILITY_ENTITY_NOT_READY, "Entity Not Ready" }, - { 0, NULL }, -}; -static const struct bit_info talker_capabilities_info[] = { - { AVBTP_ADP_TALKER_CAPABILITY_IMPLEMENTED, "implemented", "Implemented" }, - { AVBTP_ADP_TALKER_CAPABILITY_OTHER_SOURCE, "other-source", "Other Source" }, - { AVBTP_ADP_TALKER_CAPABILITY_CONTROL_SOURCE, "control-source", "Control Source" }, - { AVBTP_ADP_TALKER_CAPABILITY_MEDIA_CLOCK_SOURCE, "media-clock-source", "Media Clock Source" }, - { AVBTP_ADP_TALKER_CAPABILITY_SMPTE_SOURCE, "smpte-source", "SMPTE Source" }, - { AVBTP_ADP_TALKER_CAPABILITY_MIDI_SOURCE, "midi-source", "MIDI Source" }, - { AVBTP_ADP_TALKER_CAPABILITY_AUDIO_SOURCE, "audio-source", "Audio Source" }, - { AVBTP_ADP_TALKER_CAPABILITY_VIDEO_SOURCE, "video-source", "Video Source" }, - { 0, NULL }, -}; - -static const struct bit_info listener_capabilities_info[] = { - { AVBTP_ADP_LISTENER_CAPABILITY_IMPLEMENTED, "implemented", "Implemented" }, - { AVBTP_ADP_LISTENER_CAPABILITY_OTHER_SINK, "other-sink", "Other Sink" }, - { AVBTP_ADP_LISTENER_CAPABILITY_CONTROL_SINK, "control-sink", "Control Sink" }, - { AVBTP_ADP_LISTENER_CAPABILITY_MEDIA_CLOCK_SINK, "media-clock-sink", "Media Clock Sink" }, - { AVBTP_ADP_LISTENER_CAPABILITY_SMPTE_SINK, "smpte-sink", "SMPTE Sink" }, - { AVBTP_ADP_LISTENER_CAPABILITY_MIDI_SINK, "midi-sink", "MIDI Sink" }, - { AVBTP_ADP_LISTENER_CAPABILITY_AUDIO_SINK, "audio-sink", "Audio Sink" }, - { AVBTP_ADP_LISTENER_CAPABILITY_VIDEO_SINK, "video-sink", "Video Sink" }, - { 0, NULL }, -}; - -static const struct bit_info controller_capabilities_info[] = { - { AVBTP_ADP_CONTROLLER_CAPABILITY_IMPLEMENTED, "implemented", "Implemented" }, - { AVBTP_ADP_CONTROLLER_CAPABILITY_LAYER3_PROXY, "layer-3-proxy", "Layer 3 Proxy" }, - { 0, NULL }, -}; - -static void print_bit_info(int indent, uint32_t bits, const struct bit_info *info) +static int send_departing(struct adp *adp, uint64_t now, struct entity *e) { - uint32_t i; - for (i = 0; info[i].value; i++) { - if ((info[i].bits & bits) == info[i].bits) - pw_log_info("%*.s%08x %s", indent, "", info[i].bits, info[i].value); - } -} - -static const char *message_type_as_string(uint8_t message_type) -{ - switch (message_type) { - case AVBTP_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE: - return "ENTITY_AVAIALABLE"; - case AVBTP_ADP_MESSAGE_TYPE_ENTITY_DEPARTING: - return "ENTITY_DEPARTING"; - case AVBTP_ADP_MESSAGE_TYPE_ENTITY_DISCOVER: - return "ENTITY_DISCOVER"; - } - return "INVALID"; -} - -#define KEY_VALID_TIME "valid-time" -#define KEY_ENTITY_ID "entity-id" -#define KEY_ENTITY_MODEL_ID "entity-model-id" -#define KEY_ENTITY_CAPABILITIES "entity-capabilities" -#define KEY_TALKER_STREAM_SOURCES "talker-stream-sources" -#define KEY_TALKER_CAPABILITIES "talker-capabilities" -#define KEY_LISTENER_STREAM_SINKS "listener-stream-sinks" -#define KEY_LISTENER_CAPABILITIES "listener-capabilities" -#define KEY_CONTROLLER_CAPABILITIES "controller-capabilities" -#define KEY_AVAILABLE_INDEX "available-index" -#define KEY_GPTP_GRANDMASTER_ID "gptp-grandmaster-id" -#define KEY_GPTP_DOMAIN_NUMBER "gptp-domain-number" -#define KEY_IDENTIFY_CONTROL_INDEX "indentify-control-index" -#define KEY_INTERFACE_INDEX "interface-index" -#define KEY_ASSOCIATION_ID "association-id" - -static inline char *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 void adp_message_debug(struct adp *adp, const struct avbtp_packet_adp *p) -{ - uint32_t v; - char buf[256]; - - v = AVBTP_PACKET_ADP_GET_MESSAGE_TYPE(p); - pw_log_info("message-type: %d (%s)", v, message_type_as_string(v)); - pw_log_info(" length: %d", AVBTP_PACKET_GET_LENGTH(&p->hdr)); - pw_log_info(" "KEY_VALID_TIME": %d", AVBTP_PACKET_ADP_GET_VALID_TIME(p)); - pw_log_info(" "KEY_ENTITY_ID": %s", - format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_ENTITY_ID(p))); - pw_log_info(" "KEY_ENTITY_MODEL_ID": %s", - format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_ENTITY_MODEL_ID(p))); - v = AVBTP_PACKET_ADP_GET_ENTITY_CAPABILITIES(p); - pw_log_info(" "KEY_ENTITY_CAPABILITIES": 0x%08x", v); - print_bit_info(4, v, entity_capabilities_info); - pw_log_info(" "KEY_TALKER_STREAM_SOURCES": %d", AVBTP_PACKET_ADP_GET_TALKER_STREAM_SOURCES(p)); - v = AVBTP_PACKET_ADP_GET_TALKER_CAPABILITIES(p); - pw_log_info(" "KEY_TALKER_CAPABILITIES": %04x", v); - print_bit_info(4, v, talker_capabilities_info); - pw_log_info(" "KEY_LISTENER_STREAM_SINKS": %d", AVBTP_PACKET_ADP_GET_LISTENER_STREAM_SINKS(p)); - v = AVBTP_PACKET_ADP_GET_LISTENER_CAPABILITIES(p); - pw_log_info(" "KEY_LISTENER_CAPABILITIES": %04x", v); - print_bit_info(4, v, listener_capabilities_info); - v = AVBTP_PACKET_ADP_GET_CONTROLLER_CAPABILITIES(p); - pw_log_info(" "KEY_CONTROLLER_CAPABILITIES": %08x", v); - print_bit_info(4, v, controller_capabilities_info); - pw_log_info(" "KEY_AVAILABLE_INDEX": 0x%08x", AVBTP_PACKET_ADP_GET_AVAILABLE_INDEX(p)); - pw_log_info(" "KEY_GPTP_GRANDMASTER_ID": %s", - format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_GPTP_GRANDMASTER_ID(p))); - pw_log_info(" "KEY_GPTP_DOMAIN_NUMBER": %d", AVBTP_PACKET_ADP_GET_GPTP_DOMAIN_NUMBER(p)); - pw_log_info(" "KEY_IDENTIFY_CONTROL_INDEX": %d", AVBTP_PACKET_ADP_GET_IDENTIFY_CONTROL_INDEX(p)); - pw_log_info(" "KEY_INTERFACE_INDEX": %d", AVBTP_PACKET_ADP_GET_INTERFACE_INDEX(p)); - pw_log_info(" "KEY_ASSOCIATION_ID": %s", - format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_ASSOCIATION_ID(p))); + AVBTP_PACKET_ADP_SET_MESSAGE_TYPE(&e->packet, AVBTP_ADP_MESSAGE_TYPE_ENTITY_DEPARTING); + e->packet.available_index = htonl(adp->available_index++); + avbtp_server_broadcast_packet(adp->server, &e->packet, sizeof(e->packet)); + e->last_time = now; + return 0; } static int send_advertise(struct adp *adp, uint64_t now, struct entity *e) { - AVBTP_PACKET_ADP_SET_AVAILABLE_INDEX(&e->packet, adp->available_index++); + AVBTP_PACKET_ADP_SET_MESSAGE_TYPE(&e->packet, AVBTP_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE); + e->packet.available_index = htonl(adp->available_index++); avbtp_server_broadcast_packet(adp->server, &e->packet, sizeof(e->packet)); e->last_time = now; return 0; @@ -219,7 +85,7 @@ static int send_discover(struct adp *adp, uint64_t entity_id) AVBTP_PACKET_SET_SUBTYPE(&p.hdr, AVBTP_SUBTYPE_ADP); AVBTP_PACKET_SET_LENGTH(&p.hdr, AVBTP_ADP_DATA_LENGTH); AVBTP_PACKET_ADP_SET_MESSAGE_TYPE(&p, AVBTP_ADP_MESSAGE_TYPE_ENTITY_DISCOVER); - AVBTP_PACKET_ADP_SET_ENTITY_ID(&p, entity_id); + p.entity_id = htonl(entity_id); avbtp_server_broadcast_packet(adp->server, &p, sizeof(p)); return 0; } @@ -237,11 +103,8 @@ static int adp_message(void *data, uint64_t now, const void *message, int len) AVBTP_PACKET_GET_LENGTH(&p->hdr) != AVBTP_ADP_DATA_LENGTH) return 0; - if (adp->server->debug_messages) - adp_message_debug(adp, p); - message_type = AVBTP_PACKET_ADP_GET_MESSAGE_TYPE(p); - entity_id = AVBTP_PACKET_ADP_GET_ENTITY_ID(p); + entity_id = be64toh(p->entity_id); e = find_entity_by_id(adp, entity_id); @@ -255,23 +118,23 @@ static int adp_message(void *data, uint64_t now, const void *message, int len) e->packet = *p; spa_list_append(&adp->entities, &e->link); pw_log_info("entity %s available", - format_id(buf, sizeof(buf), entity_id)); + avbtp_utils_format_id(buf, sizeof(buf), entity_id)); } - e->last_time = adp->now = now; + e->last_time = now; break; case AVBTP_ADP_MESSAGE_TYPE_ENTITY_DEPARTING: if (e != NULL) { pw_log_info("entity %s departing", - format_id(buf, sizeof(buf), entity_id)); + avbtp_utils_format_id(buf, sizeof(buf), entity_id)); entity_free(e); } break; case AVBTP_ADP_MESSAGE_TYPE_ENTITY_DISCOVER: if (entity_id == 0UL || (e != NULL && e->advertise && - AVBTP_PACKET_ADP_GET_ENTITY_ID(&e->packet) == entity_id)) { + be64toh(e->packet.entity_id) == entity_id)) { pw_log_info("entity %s discover", - format_id(buf, sizeof(buf), entity_id)); + avbtp_utils_format_id(buf, sizeof(buf), entity_id)); send_discover(adp, entity_id); } break; @@ -288,7 +151,7 @@ static void adp_destroy(void *data) free(adp); } -static void check_entries(struct adp *adp, uint64_t now) +static void check_timeout(struct adp *adp, uint64_t now) { struct entity *e, *t; char buf[128]; @@ -296,106 +159,131 @@ static void check_entries(struct adp *adp, uint64_t now) spa_list_for_each_safe(e, t, &adp->entities, link) { int valid_time = AVBTP_PACKET_ADP_GET_VALID_TIME(&e->packet); - if (e->advertise) { - if (e->last_time + (valid_time / 2) * SPA_NSEC_PER_SEC > now) - continue; + if (e->last_time + (valid_time + 2) * SPA_NSEC_PER_SEC > now) + continue; - pw_log_info("entity %s readvertise", - format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_ENTITY_ID(&e->packet))); + pw_log_info("entity %s timeout", + avbtp_utils_format_id(buf, sizeof(buf), + be64toh(e->packet.entity_id))); - send_advertise(adp, now, e); - } else { - if (e->last_time + (valid_time + 2) * SPA_NSEC_PER_SEC > now) - continue; + if (e->advertise) + send_departing(adp, now, e); - pw_log_info("entity %s timeout", - format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_ENTITY_ID(&e->packet))); - entity_free(e); - } + entity_free(e); } } -static void adp_periodic(void *data, uint64_t now) +static void check_readvertize(struct adp *adp, uint64_t now, struct entity *e) { - struct adp *adp = data; - check_entries(adp, now); + int valid_time = AVBTP_PACKET_ADP_GET_VALID_TIME(&e->packet); + char buf[128]; + + if (!e->advertise) + return; + + if (e->last_time + (valid_time / 2) * SPA_NSEC_PER_SEC > now) + return; + + pw_log_debug("entity %s readvertise", + avbtp_utils_format_id(buf, sizeof(buf), + be64toh(e->packet.entity_id))); + + send_advertise(adp, now, e); } -static int parse_id(const char *value, int len, uint64_t *id) -{ - char str[64]; - uint8_t v[6]; - uint16_t unique_id; - if (spa_json_parse_stringn(value, len, str, sizeof(str)) <= 0) - return -EINVAL; - if (sscanf(str, "%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 1; -} -static int parse_bits(struct spa_json *it, const char *value, int len, const struct bit_info *info, int *bits) -{ - uint32_t i; - int b = 0; - struct spa_json sub; - char val[256]; - - if (spa_json_is_array(value, len)) { - spa_json_enter(it, &sub); - while (spa_json_get_string(&sub, val, sizeof(val)) > 0) { - for (i = 0; info[i].value; i++) { - if (spa_streq(val, info[i].value)) { - b |= info[i].bits; - break; - } - } - } - } else if (!spa_json_parse_int(value, len, &b)) { - return -EINVAL; - } - *bits = b; - return 1; -} - -static int do_help(struct adp *adp, const char *args) -{ - return 0; -} - -static int do_advertise(struct adp *adp, const char *args) +static int check_advertise(struct adp *adp, uint64_t now) { struct server *server = adp->server; + const struct descriptor *d; + struct avbtp_aem_desc_entity *entity; + struct avbtp_aem_desc_avb_interface *avb_interface; struct entity *e; - struct spa_json it[2]; - char key[128]; + uint64_t entity_id; struct avbtp_packet_adp *p; + char buf[128]; + + d = server_find_descriptor(server, AVBTP_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, AVBTP_AEM_DESC_AVB_INTERFACE, 0); + avb_interface = d ? d->ptr : NULL; + + pw_log_info("entity %s advertise", + avbtp_utils_format_id(buf, sizeof(buf), entity_id)); e = calloc(1, sizeof(*e)); if (e == NULL) return -errno; e->advertise = true; - e->last_time = adp->now; + e->last_time = now; p = &e->packet; AVBTP_PACKET_SET_LENGTH(&p->hdr, AVBTP_ADP_DATA_LENGTH); AVBTP_PACKET_SET_SUBTYPE(&p->hdr, AVBTP_SUBTYPE_ADP); - AVBTP_PACKET_ADP_SET_ENTITY_ID(p, server->entity_id); + AVBTP_PACKET_ADP_SET_MESSAGE_TYPE(p, AVBTP_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE); + AVBTP_PACKET_ADP_SET_VALID_TIME(p, 10); + + 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\": }] : 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, int_val; + int len; const char *value; uint64_t id_val; @@ -405,77 +293,16 @@ static int do_advertise(struct adp *adp, const char *args) if (spa_json_is_null(value, len)) continue; - if (spa_streq(key, KEY_VALID_TIME)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_VALID_TIME(p, int_val); - } else if (spa_streq(key, KEY_ENTITY_ID)) { - if (parse_id(value, len, &id_val) > 0) - AVBTP_PACKET_ADP_SET_ENTITY_ID(p, id_val); - } else if (spa_streq(key, KEY_ENTITY_MODEL_ID)) { - if (parse_id(value, len, &id_val) > 0) - AVBTP_PACKET_ADP_SET_ENTITY_MODEL_ID(p, id_val); - } else if (spa_streq(key, KEY_ENTITY_CAPABILITIES)) { - if (parse_bits(&it[1], value, len, entity_capabilities_info, &int_val)) - AVBTP_PACKET_ADP_SET_ENTITY_CAPABILITIES(p, int_val); - } else if (spa_streq(key, KEY_TALKER_STREAM_SOURCES)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_TALKER_STREAM_SOURCES(p, int_val); - } else if (spa_streq(key, KEY_TALKER_CAPABILITIES)) { - if (parse_bits(&it[1], value, len, talker_capabilities_info, &int_val)) - AVBTP_PACKET_ADP_SET_TALKER_CAPABILITIES(p, int_val); - } else if (spa_streq(key, KEY_LISTENER_STREAM_SINKS)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_LISTENER_STREAM_SINKS(p, int_val); - } else if (spa_streq(key, KEY_LISTENER_CAPABILITIES)) { - if (parse_bits(&it[1], value, len, listener_capabilities_info, &int_val)) - AVBTP_PACKET_ADP_SET_LISTENER_CAPABILITIES(p, int_val); - } else if (spa_streq(key, KEY_CONTROLLER_CAPABILITIES)) { - if (parse_bits(&it[1], value, len, controller_capabilities_info, &int_val)) - AVBTP_PACKET_ADP_SET_CONTROLLER_CAPABILITIES(p, int_val); - } else if (spa_streq(key, KEY_AVAILABLE_INDEX)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_AVAILABLE_INDEX(p, int_val); - } else if (spa_streq(key, KEY_GPTP_GRANDMASTER_ID)) { - if (parse_id(value, len, &id_val) > 0) - AVBTP_PACKET_ADP_SET_GPTP_GRANDMASTER_ID(p, id_val); - } else if (spa_streq(key, KEY_GPTP_DOMAIN_NUMBER)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_GPTP_DOMAIN_NUMBER(p, int_val); - } else if (spa_streq(key, KEY_IDENTIFY_CONTROL_INDEX)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_IDENTIFY_CONTROL_INDEX(p, int_val); - } else if (spa_streq(key, KEY_INTERFACE_INDEX)) { - if (spa_json_parse_int(value, len, &int_val)) - AVBTP_PACKET_ADP_SET_INTERFACE_INDEX(p, int_val); - } else if (spa_streq(key, KEY_ASSOCIATION_ID)) { - if (parse_id(value, len, &id_val)) - AVBTP_PACKET_ADP_SET_ASSOCIATION_ID(p, id_val); + if (spa_streq(key, "entity-id")) { + if (avbtp_utils_parse_id(value, len, &id_val) >= 0) + entity_id = id_val; } } - if (find_entity_by_id(adp, AVBTP_PACKET_ADP_GET_ENTITY_ID(p))) { - free(e); - return -EEXIST; - } - spa_list_append(&adp->entities, &e->link); - - if (adp->server->debug_messages) - adp_message_debug(adp, p); - + send_discover(adp, entity_id); return 0; } -static int do_depart(struct adp *adp, const char *args) -{ - return 0; -} - -static int do_discover(struct adp *adp, const char *args) -{ - send_discover(adp, 0UL); - return 0; -} - -static int adp_command(void *data, uint64_t now, const char *command, const char *args) +static int adp_command(void *data, uint64_t now, const char *command, const char *args, FILE *out) { struct adp *adp = data; int res; @@ -484,16 +311,11 @@ static int adp_command(void *data, uint64_t now, const char *command, const char return 0; command += strlen("/adp/"); - adp->now = now; if (spa_streq(command, "help")) - res = do_help(adp, args); - else if (spa_streq(command, "advertise")) - res = do_advertise(adp, args); - else if (spa_streq(command, "depart")) - res = do_depart(adp, args); + res = do_help(adp, args, out); else if (spa_streq(command, "discover")) - res = do_discover(adp, args); + res = do_discover(adp, args, out); else res = -ENOTSUP; diff --git a/src/modules/module-avbtp/adp.h b/src/modules/module-avbtp/adp.h index 1ef5d0d8a..62f5436d9 100644 --- a/src/modules/module-avbtp/adp.h +++ b/src/modules/module-avbtp/adp.h @@ -96,37 +96,9 @@ struct avbtp_packet_adp { #define AVBTP_PACKET_ADP_SET_MESSAGE_TYPE(p,v) AVBTP_PACKET_SET_SUB1(&(p)->hdr, v) #define AVBTP_PACKET_ADP_SET_VALID_TIME(p,v) AVBTP_PACKET_SET_SUB2(&(p)->hdr, v) -#define AVBTP_PACKET_ADP_SET_ENTITY_ID(p,v) ((p)->entity_id = htobe64(v)) -#define AVBTP_PACKET_ADP_SET_ENTITY_MODEL_ID(p,v) ((p)->entity_model_id = htobe64(v)) -#define AVBTP_PACKET_ADP_SET_ENTITY_CAPABILITIES(p,v) ((p)->entity_capabilities = htonl(v)) -#define AVBTP_PACKET_ADP_SET_TALKER_STREAM_SOURCES(p,v) ((p)->talker_stream_sources = htons(v)) -#define AVBTP_PACKET_ADP_SET_TALKER_CAPABILITIES(p,v) ((p)->talker_capabilities = htons(v)) -#define AVBTP_PACKET_ADP_SET_LISTENER_STREAM_SINKS(p,v) ((p)->listener_stream_sinks = htons(v)) -#define AVBTP_PACKET_ADP_SET_LISTENER_CAPABILITIES(p,v) ((p)->listener_capabilities = htons(v)) -#define AVBTP_PACKET_ADP_SET_CONTROLLER_CAPABILITIES(p,v) ((p)->controller_capabilities = htonl(v)) -#define AVBTP_PACKET_ADP_SET_AVAILABLE_INDEX(p,v) ((p)->available_index = htonl(v)) -#define AVBTP_PACKET_ADP_SET_GPTP_GRANDMASTER_ID(p,v) ((p)->gptp_grandmaster_id = htobe64(v)) -#define AVBTP_PACKET_ADP_SET_GPTP_DOMAIN_NUMBER(p,v) ((p)->gptp_domain_number = (v)) -#define AVBTP_PACKET_ADP_SET_IDENTIFY_CONTROL_INDEX(p,v) ((p)->identify_control_index = htons(v)) -#define AVBTP_PACKET_ADP_SET_INTERFACE_INDEX(p,v) ((p)->interface_index = htons(v)) -#define AVBTP_PACKET_ADP_SET_ASSOCIATION_ID(p,v) ((p)->association_id = htobe64(v)) #define AVBTP_PACKET_ADP_GET_MESSAGE_TYPE(p) AVBTP_PACKET_GET_SUB1(&(p)->hdr) #define AVBTP_PACKET_ADP_GET_VALID_TIME(p) AVBTP_PACKET_GET_SUB2(&(p)->hdr) -#define AVBTP_PACKET_ADP_GET_ENTITY_ID(p) be64toh((p)->entity_id) -#define AVBTP_PACKET_ADP_GET_ENTITY_MODEL_ID(p) be64toh((p)->entity_model_id) -#define AVBTP_PACKET_ADP_GET_ENTITY_CAPABILITIES(p) ntohl((p)->entity_capabilities) -#define AVBTP_PACKET_ADP_GET_TALKER_STREAM_SOURCES(p) ntohs((p)->talker_stream_sources) -#define AVBTP_PACKET_ADP_GET_TALKER_CAPABILITIES(p) ntohs((p)->talker_capabilities) -#define AVBTP_PACKET_ADP_GET_LISTENER_STREAM_SINKS(p) ntohs((p)->listener_stream_sinks) -#define AVBTP_PACKET_ADP_GET_LISTENER_CAPABILITIES(p) ntohs((p)->listener_capabilities) -#define AVBTP_PACKET_ADP_GET_CONTROLLER_CAPABILITIES(p) ntohl((p)->controller_capabilities) -#define AVBTP_PACKET_ADP_GET_AVAILABLE_INDEX(p) ntohl((p)->available_index) -#define AVBTP_PACKET_ADP_GET_GPTP_GRANDMASTER_ID(p) be64toh((p)->gptp_grandmaster_id) -#define AVBTP_PACKET_ADP_GET_GPTP_DOMAIN_NUMBER(p) ((p)->gptp_domain_number) -#define AVBTP_PACKET_ADP_GET_IDENTIFY_CONTROL_INDEX(p) ntohs((p)->identify_control_index) -#define AVBTP_PACKET_ADP_GET_INTERFACE_INDEX(p) ntohs((p)->interface_index) -#define AVBTP_PACKET_ADP_GET_ASSOCIATION_ID(p) be64toh((p)->association_id) struct avbtp_adp *avbtp_adp_register(struct server *server); diff --git a/src/modules/module-avbtp/aecp-aem-descriptors.h b/src/modules/module-avbtp/aecp-aem-descriptors.h index 949f7ccaa..e513a353b 100644 --- a/src/modules/module-avbtp/aecp-aem-descriptors.h +++ b/src/modules/module-avbtp/aecp-aem-descriptors.h @@ -243,4 +243,5 @@ struct avbtp_aem_desc_stream_port { uint16_t number_of_maps; uint16_t base_map; } __attribute__ ((__packed__)); + #endif /* AVBTP_AECP_AEM_DESCRIPTORS_H */ diff --git a/src/modules/module-avbtp/aecp-aem.c b/src/modules/module-avbtp/aecp-aem.c index 94aee223c..947c5d409 100644 --- a/src/modules/module-avbtp/aecp-aem.c +++ b/src/modules/module-avbtp/aecp-aem.c @@ -62,7 +62,7 @@ static int handle_acquire_entity(struct aecp *aecp, const void *m, int len) desc_type = ntohs(ae->descriptor_type); desc_id = ntohs(ae->descriptor_id); - desc = find_descriptor(server, desc_type, desc_id); + desc = server_find_descriptor(server, desc_type, desc_id); if (desc == NULL) return reply_status(aecp, AVBTP_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, p, len); @@ -86,7 +86,7 @@ static int handle_lock_entity(struct aecp *aecp, const void *m, int len) desc_type = ntohs(ae->descriptor_type); desc_id = ntohs(ae->descriptor_id); - desc = find_descriptor(server, desc_type, desc_id); + desc = server_find_descriptor(server, desc_type, desc_id); if (desc == NULL) return reply_status(aecp, AVBTP_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, p, len); @@ -97,16 +97,16 @@ static int handle_lock_entity(struct aecp *aecp, const void *m, int len) } /* READ_DESCRIPTOR */ -static int handle_read_descriptor(struct aecp *aecp, const void *h, int len) +static int handle_read_descriptor(struct aecp *aecp, const void *m, int len) { struct server *server = aecp->server; - const struct avbtp_packet_aecp_aem *p = h; - struct avbtp_packet_aecp_header *reply; + const struct avbtp_packet_aecp_aem *p = m; + struct avbtp_packet_aecp_aem *reply; + const struct avbtp_packet_aecp_aem_read_descriptor *rd; uint16_t desc_type, desc_id; const struct descriptor *desc; - const struct avbtp_packet_aecp_aem_read_descriptor *rd; uint8_t buf[2048]; - size_t size; + size_t size, psize; rd = (struct avbtp_packet_aecp_aem_read_descriptor*)p->payload; @@ -115,22 +115,72 @@ static int handle_read_descriptor(struct aecp *aecp, const void *h, int len) pw_log_info("descriptor type:%04x index:%d", desc_type, desc_id); - desc = find_descriptor(server, desc_type, desc_id); + desc = server_find_descriptor(server, desc_type, desc_id); if (desc == NULL) return reply_status(aecp, AVBTP_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, p, len); - size = sizeof(struct avbtp_packet_aecp_aem) + sizeof(*rd); + memcpy(buf, p, len); + + psize = sizeof(*rd); + size = sizeof(*reply) + psize; - memcpy(buf, p, size); memcpy(buf + size, desc->ptr, desc->size); size += desc->size; + psize += desc->size; - reply = (struct avbtp_packet_aecp_header *)buf; - AVBTP_PACKET_SET_LENGTH(&reply->hdr, size - 26 ); - AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(reply, AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE); - AVBTP_PACKET_AECP_SET_STATUS(reply, AVBTP_AECP_AEM_STATUS_SUCCESS); + reply = (struct avbtp_packet_aecp_aem*)buf; + AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(&reply->aecp, AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVBTP_PACKET_AECP_SET_STATUS(&reply->aecp, AVBTP_AECP_AEM_STATUS_SUCCESS); + AVBTP_PACKET_SET_LENGTH(&reply->aecp.hdr, psize + 12); - return avbtp_server_send_packet(server, reply->hdr.eth.src, reply, size); + return avbtp_server_send_packet(server, reply->aecp.hdr.eth.src, reply, 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 avbtp_packet_aecp_aem *p = m; + struct avbtp_packet_aecp_aem *reply; + struct avbtp_packet_aecp_aem_get_avb_info *i; + struct avbtp_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 avbtp_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, AVBTP_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR, p, len); + + if (desc_type != AVBTP_AEM_DESC_AVB_INTERFACE || desc_id != 0) + return reply_not_implemented(aecp, m, len); + + avb_interface = desc->ptr; + + memcpy(buf, p, len); + + psize = sizeof(*i); + size = sizeof(*reply) + psize; + + reply = (struct avbtp_packet_aecp_aem *)buf; + AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(&reply->aecp, AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVBTP_PACKET_AECP_SET_STATUS(&reply->aecp, AVBTP_AECP_AEM_STATUS_SUCCESS); + AVBTP_PACKET_SET_LENGTH(&reply->aecp.hdr, psize + 12); + + i = (struct avbtp_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 avbtp_server_send_packet(server, reply->aecp.hdr.eth.src, reply, size); } /* AEM_COMMAND */ @@ -180,7 +230,7 @@ static const struct cmd_info cmd_info[] = { { AVBTP_AECP_AEM_CMD_REGISTER_UNSOLICITED_NOTIFICATION, "register-unsolicited-notification", NULL, }, { AVBTP_AECP_AEM_CMD_DEREGISTER_UNSOLICITED_NOTIFICATION, "deregister-unsolicited-notification", NULL, }, { AVBTP_AECP_AEM_CMD_IDENTIFY_NOTIFICATION, "identify-notification", NULL, }, - { AVBTP_AECP_AEM_CMD_GET_AVB_INFO, "get-avb-info", NULL, }, + { AVBTP_AECP_AEM_CMD_GET_AVB_INFO, "get-avb-info", handle_get_avb_info, }, { AVBTP_AECP_AEM_CMD_GET_AS_PATH, "get-as-path", NULL, }, { AVBTP_AECP_AEM_CMD_GET_COUNTERS, "get-counters", NULL, }, { AVBTP_AECP_AEM_CMD_REBOOT, "reboot", NULL, }, diff --git a/src/modules/module-avbtp/aecp-aem.h b/src/modules/module-avbtp/aecp-aem.h index 6b10f4a13..2ddb8470e 100644 --- a/src/modules/module-avbtp/aecp-aem.h +++ b/src/modules/module-avbtp/aecp-aem.h @@ -270,6 +270,16 @@ struct avbtp_packet_aecp_aem_identify_notification { uint16_t descriptor_id; } __attribute__ ((__packed__)); +struct avbtp_packet_aecp_aem_msrp_mapping { + uint8_t traffic_class; + uint8_t priority; + uint16_t vlan_id; +} __attribute__ ((__packed__)); + +#define AVBTP_AEM_AVB_INFO_FLAG_GPTP_GRANDMASTER_SUPPORTED (1u<<0) +#define AVBTP_AEM_AVB_INFO_FLAG_GPTP_ENABLED (1u<<1) +#define AVBTP_AEM_AVB_INFO_FLAG_SRP_ENABLED (1u<<2) + struct avbtp_packet_aecp_aem_get_avb_info { uint16_t descriptor_type; uint16_t descriptor_id; @@ -278,7 +288,7 @@ struct avbtp_packet_aecp_aem_get_avb_info { uint8_t gptp_domain_number; uint8_t flags; uint16_t msrp_mappings_count; - uint32_t msrp_mappings; + uint8_t msrp_mappings[0]; } __attribute__ ((__packed__)); struct avbtp_packet_aecp_aem_get_as_path { diff --git a/src/modules/module-avbtp/aecp.c b/src/modules/module-avbtp/aecp.c index 7a4e56309..4a1df3534 100644 --- a/src/modules/module-avbtp/aecp.c +++ b/src/modules/module-avbtp/aecp.c @@ -34,7 +34,6 @@ struct msg_info { uint16_t type; const char *name; - const char *description; int (*handle) (struct aecp *aecp, const void *p, int len); }; @@ -51,16 +50,16 @@ static int reply_not_implemented(struct aecp *aecp, const void *p, int len) } static const struct msg_info msg_info[] = { - { AVBTP_AECP_MESSAGE_TYPE_AEM_COMMAND, "aem-command", "AEM Command", avbtp_aecp_aem_handle_command, }, - { AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE, "aem-response", "AEM Response", avbtp_aecp_aem_handle_response, }, - { AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_COMMAND, "address-access-command", "Address Access Command", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_RESPONSE, "address-access-response", "Address Access Response", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_AVC_COMMAND, "avc-command", "AVC Command", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_AVC_RESPONSE, "avc-response", "AVC Response", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_COMMAND, "vendor-unique-command", "Vendor Unique Command", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE, "vendor-unique-response", "Vendor Unique Response", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_EXTENDED_COMMAND, "extended-command", "Extended Command", NULL, }, - { AVBTP_AECP_MESSAGE_TYPE_EXTENDED_RESPONSE, "extended-response", "Extended Response", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_AEM_COMMAND, "aem-command", avbtp_aecp_aem_handle_command, }, + { AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE, "aem-response", avbtp_aecp_aem_handle_response, }, + { AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_COMMAND, "address-access-command", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_RESPONSE, "address-access-response", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_AVC_COMMAND, "avc-command", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_AVC_RESPONSE, "avc-response", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_COMMAND, "vendor-unique-command", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE, "vendor-unique-response", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_EXTENDED_COMMAND, "extended-command", NULL, }, + { AVBTP_AECP_MESSAGE_TYPE_EXTENDED_RESPONSE, "extended-response", NULL, }, }; static inline const struct msg_info *find_msg_info(uint16_t type, const char *name) @@ -90,7 +89,7 @@ static int aecp_message(void *data, uint64_t now, const void *message, int len) if (info == NULL) return reply_not_implemented(aecp, p, len); - pw_log_info("got AECP message %s", info->name); + pw_log_debug("got AECP message %s", info->name); if (info->handle == NULL) return reply_not_implemented(aecp, p, len); @@ -105,16 +104,16 @@ static void aecp_destroy(void *data) free(aecp); } -static void aecp_periodic(void *data, uint64_t now) -{ -} - -static int do_help(struct aecp *aecp, const char *args) +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) +static int aecp_command(void *data, uint64_t now, const char *command, const char *args, FILE *out) { struct aecp *aecp = data; int res; @@ -123,10 +122,9 @@ static int aecp_command(void *data, uint64_t now, const char *command, const cha return 0; command += strlen("/aecp/"); - aecp->now = now; if (spa_streq(command, "help")) - res = do_help(aecp, args); + res = do_help(aecp, args, out); else res = -ENOTSUP; @@ -137,7 +135,6 @@ static const struct server_events server_events = { AVBTP_VERSION_SERVER_EVENTS, .destroy = aecp_destroy, .message = aecp_message, - .periodic = aecp_periodic, .command = aecp_command }; diff --git a/src/modules/module-avbtp/aecp.h b/src/modules/module-avbtp/aecp.h index 0fca3e33a..0e8732c47 100644 --- a/src/modules/module-avbtp/aecp.h +++ b/src/modules/module-avbtp/aecp.h @@ -51,15 +51,9 @@ struct avbtp_packet_aecp_header { #define AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(p,v) AVBTP_PACKET_SET_SUB1(&(p)->hdr, v) #define AVBTP_PACKET_AECP_SET_STATUS(p,v) AVBTP_PACKET_SET_SUB2(&(p)->hdr, v) -#define AVBTP_PACKET_AECP_SET_TARGET_GUID(p,v) ((p)->target_guid = htobe64(v)) -#define AVBTP_PACKET_AECP_SET_CONTROLLER_GUID(p,v) ((p)->controller_guid = htobe64(v)) -#define AVBTP_PACKET_AECP_SET_SEQUENCE_ID(p,v) ((p)->sequence_id = htons(v)) #define AVBTP_PACKET_AECP_GET_MESSAGE_TYPE(p) AVBTP_PACKET_GET_SUB1(&(p)->hdr) #define AVBTP_PACKET_AECP_GET_STATUS(p) AVBTP_PACKET_GET_SUB2(&(p)->hdr) -#define AVBTP_PACKET_AECP_GET_TARGET_GUID(p,v) be64toh((p)->target_guid) -#define AVBTP_PACKET_AECP_GET_CONTROLLER_GUID(p,v) be64toh((p)->controller_guid) -#define AVBTP_PACKET_AECP_GET_SEQUENCE_ID(p,v) ntohs((p)->sequence_id) struct avbtp_aecp *avbtp_aecp_register(struct server *server); diff --git a/src/modules/module-avbtp/avdecc.c b/src/modules/module-avbtp/avdecc.c index 1377cad6c..1f008e28d 100644 --- a/src/modules/module-avbtp/avdecc.c +++ b/src/modules/module-avbtp/avdecc.c @@ -52,7 +52,7 @@ static const uint8_t AVB_MAC_BROADCAST[6] = { 0x91, 0xe0, 0xf0, 0x01, 0x00, 0x00 #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) server_emit(s, command, 0, n, c, a) +#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) { @@ -208,7 +208,6 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s { struct server *server; int res = 0; - struct timespec now; server = calloc(1, sizeof(*server)); if (server == NULL) @@ -218,40 +217,20 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s spa_list_append(&impl->servers, &server->link); server->ifname = strdup(ifname); spa_hook_list_init(&server->listener_list); + spa_list_init(&server->descriptors); - server->debug_messages = true; + server->debug_messages = false; if ((res = setup_socket(server)) < 0) goto error_free; init_descriptors(server); - avbtp_adp_register(server); avbtp_aecp_register(server); avbtp_maap_register(server); + avbtp_adp_register(server); + avbtp_acmp_register(server); - - clock_gettime(CLOCK_REALTIME, &now); - server_emit_command(server, SPA_TIMESPEC_TO_NSEC(&now), - "/adp/advertise", - "{" - " valid-time = 10 " - " entity-model-id = \"00:01:02:03:04:05:0600\" " - " entity-capabilities = [ " - " aem-supported " - " class-a-supported " - " gptp-supported " - " aem-identify-control-index-valid " - " aem-interface-index-valid ] " - " talker-stream-sources = 8 " - " talker-capabilities = [ implemented audio-source ] " - " listener-stream-sinks = 8 " - " listener-capabilities = [ implemented audio-sink ] " - " controller-capabilities = [ ] " - " gptp-grandmaster-id = \"10:20:30:40:50:60:0001\" " - " gptp-domain-number = 6 " - " association-id = 0 " - "}"); return server; error_free: diff --git a/src/modules/module-avbtp/descriptors.h b/src/modules/module-avbtp/descriptors.h index 8feaa3bc2..82377117f 100644 --- a/src/modules/module-avbtp/descriptors.h +++ b/src/modules/module-avbtp/descriptors.h @@ -26,24 +26,9 @@ #include "aecp-aem-descriptors.h" #include "internal.h" -static void 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; - - d->type = type; - d->index = index; - d->size = size; - d->ptr = SPA_PTROFF(d, sizeof(struct descriptor), void); - memcpy(d->ptr, ptr, size); - server->descriptors[server->n_descriptors++] = d; -} - void init_descriptors(struct server *server) { - add_descriptor(server, AVBTP_AEM_DESC_STRINGS, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_STRINGS, 0, sizeof(struct avbtp_aem_desc_strings), &(struct avbtp_aem_desc_strings) { @@ -51,7 +36,7 @@ void init_descriptors(struct server *server) .string_1 = "Configuration 1", .string_2 = "Wim Taymans", }); - add_descriptor(server, AVBTP_AEM_DESC_LOCALE, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_LOCALE, 0, sizeof(struct avbtp_aem_desc_locale), &(struct avbtp_aem_desc_locale) { @@ -59,7 +44,7 @@ void init_descriptors(struct server *server) .number_of_strings = htons(1), .base_strings = htons(0) }); - add_descriptor(server, AVBTP_AEM_DESC_ENTITY, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_ENTITY, 0, sizeof(struct avbtp_aem_desc_entity), &(struct avbtp_aem_desc_entity) { @@ -115,7 +100,7 @@ void init_descriptors(struct server *server) { htons(AVBTP_AEM_DESC_CLOCK_DOMAIN), htons(1) } } }; - add_descriptor(server, AVBTP_AEM_DESC_CONFIGURATION, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_CONFIGURATION, 0, sizeof(config), &config); struct { @@ -173,7 +158,7 @@ void init_descriptors(struct server *server) { .pull_frequency = htonl(192000) }, } }; - add_descriptor(server, AVBTP_AEM_DESC_AUDIO_UNIT, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_AUDIO_UNIT, 0, sizeof(audio_unit), &audio_unit); struct { @@ -212,7 +197,7 @@ void init_descriptors(struct server *server) htobe64(0x00a0060860000800ULL), }, }; - add_descriptor(server, AVBTP_AEM_DESC_STREAM_INPUT, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_STREAM_INPUT, 0, sizeof(stream_input_0), &stream_input_0); struct { @@ -250,7 +235,7 @@ void init_descriptors(struct server *server) htobe64(0x00a0060860000800ULL), }, }; - add_descriptor(server, AVBTP_AEM_DESC_STREAM_OUTPUT, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_STREAM_OUTPUT, 0, sizeof(stream_output_0), &stream_output_0); struct avbtp_aem_desc_avb_interface avb_interface = { @@ -271,7 +256,7 @@ void init_descriptors(struct server *server) }; strncpy(avb_interface.object_name, server->ifname, 63); memcpy(avb_interface.mac_address, server->mac_addr, 6); - add_descriptor(server, AVBTP_AEM_DESC_AVB_INTERFACE, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_AVB_INTERFACE, 0, sizeof(avb_interface), &avb_interface); struct avbtp_aem_desc_clock_source clock_source = { @@ -284,6 +269,6 @@ void init_descriptors(struct server *server) .clock_source_location_type = htons(AVBTP_AEM_DESC_STREAM_INPUT), .clock_source_location_index = htons(0), }; - add_descriptor(server, AVBTP_AEM_DESC_CLOCK_SOURCE, 0, + server_add_descriptor(server, AVBTP_AEM_DESC_CLOCK_SOURCE, 0, sizeof(clock_source), &clock_source); } diff --git a/src/modules/module-avbtp/internal.h b/src/modules/module-avbtp/internal.h index 2b004b3db..9bb67e210 100644 --- a/src/modules/module-avbtp/internal.h +++ b/src/modules/module-avbtp/internal.h @@ -53,10 +53,11 @@ struct server_events { void (*periodic) (void *data, uint64_t now); - int (*command) (void *data, uint64_t now, const char *command, const char *args); + 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; @@ -77,22 +78,38 @@ struct server { struct spa_hook_list listener_list; - const struct descriptor *descriptors[512]; - uint32_t n_descriptors; + struct spa_list descriptors; unsigned debug_messages:1; }; -static inline const struct descriptor *find_descriptor(struct server *server, uint16_t type, uint16_t index) +static inline const struct descriptor *server_find_descriptor(struct server *server, + uint16_t type, uint16_t index) { - uint32_t i; - for (i = 0; i < server->n_descriptors; i++) { - if (server->descriptors[i]->type == type && - server->descriptors[i]->index == index) - return server->descriptors[i]; + struct descriptor *d; + spa_list_for_each(d, &server->descriptors, link) { + if (d->type == type && + d->index == index) + return d; } return NULL; } +static inline const struct descriptor *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); + memcpy(d->ptr, ptr, size); + spa_list_append(&server->descriptors, &d->link); + return d; +} struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct spa_dict *props); void avdecc_server_free(struct server *server); @@ -106,8 +123,6 @@ int avbtp_server_send_packet(struct server *server, const uint8_t dest[6], void struct aecp { struct server *server; struct spa_hook server_listener; - - uint64_t now; }; diff --git a/src/modules/module-avbtp/utils.h b/src/modules/module-avbtp/utils.h new file mode 100644 index 000000000..ba6fde7ec --- /dev/null +++ b/src/modules/module-avbtp/utils.h @@ -0,0 +1,66 @@ +/* AVB support + * + * Copyright © 2022 Wim Taymans + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice (including the next + * paragraph) shall be included in all copies or substantial portions of the + * Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +#ifndef AVBTP_UTILS_H +#define AVBTP_UTILS_H + +#include "internal.h" + +static inline char *avbtp_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 avbtp_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; +} + + +#endif /* AVBTP_UTILS_H */