diff --git a/src/modules/module-avbtp/adp.c b/src/modules/module-avbtp/adp.c index 416cd6128..f3dbb5c88 100644 --- a/src/modules/module-avbtp/adp.c +++ b/src/modules/module-avbtp/adp.c @@ -166,6 +166,7 @@ static inline char *format_id(char *str, size_t size, const uint64_t id) (uint16_t)(id)); return str; } + static void adp_message_debug(struct adp *adp, const struct avbtp_packet_adp *p) { uint32_t v; @@ -173,7 +174,7 @@ static void adp_message_debug(struct adp *adp, const struct avbtp_packet_adp *p) 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_ADP_GET_LENGTH(p)); + 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))); @@ -203,30 +204,80 @@ static void adp_message_debug(struct adp *adp, const struct avbtp_packet_adp *p) format_id(buf, sizeof(buf), AVBTP_PACKET_ADP_GET_ASSOCIATION_ID(p))); } -static int adp_message(void *data, uint64_t now, const void *message, int len) +static int send_advertise(struct adp *adp, uint64_t now, struct entity *e) +{ + AVBTP_PACKET_ADP_SET_AVAILABLE_INDEX(&e->packet, adp->available_index++); + avbtp_server_broadcast_packet(adp->server, &e->packet, sizeof(e->packet)); + e->last_time = now; + return 0; +} + +static int send_discover(struct adp *adp, uint64_t entity_id) +{ + struct avbtp_packet_adp p; + spa_zero(p); + 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); + avbtp_server_broadcast_packet(adp->server, &p, sizeof(p)); + return 0; +} + +static int adp_message(void *data, uint64_t now, const uint8_t mac[6], const void *message, int len) { struct adp *adp = data; const struct avbtp_packet_adp *p = message; struct entity *e; + int message_type; + char buf[128]; + uint64_t entity_id; - if (AVBTP_PACKET_GET_SUBTYPE(p) != AVBTP_SUBTYPE_ADP || - AVBTP_PACKET_ADP_GET_LENGTH(p) != AVBTP_ADP_DATA_LENGTH) + if (AVBTP_PACKET_GET_SUBTYPE(&p->hdr) != AVBTP_SUBTYPE_ADP || + AVBTP_PACKET_GET_LENGTH(&p->hdr) != AVBTP_ADP_DATA_LENGTH) return 0; - e = find_entity_by_id(adp, AVBTP_PACKET_ADP_GET_ENTITY_ID(p)); - if (e == NULL) { - e = calloc(1, sizeof(*e)); - if (e == NULL) - return -errno; + if (adp->server->debug_messages) + adp_message_debug(adp, p); - e->packet = *p; - spa_list_append(&adp->entities, &e->link); + message_type = AVBTP_PACKET_ADP_GET_MESSAGE_TYPE(p); + entity_id = AVBTP_PACKET_ADP_GET_ENTITY_ID(p); - if (adp->server->debug_messages) - adp_message_debug(adp, p); + e = find_entity_by_id(adp, entity_id); + + switch (message_type) { + case AVBTP_ADP_MESSAGE_TYPE_ENTITY_AVAILABLE: + if (e == NULL) { + e = calloc(1, sizeof(*e)); + if (e == NULL) + return -errno; + + e->packet = *p; + spa_list_append(&adp->entities, &e->link); + pw_log_info("entity %s available", + format_id(buf, sizeof(buf), entity_id)); + } + e->last_time = adp->now = 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)); + 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)) { + pw_log_info("entity %s discover", + format_id(buf, sizeof(buf), entity_id)); + send_discover(adp, entity_id); + } + break; + default: + return -EINVAL; } - e->last_time = adp->now = now; - return 0; } @@ -237,14 +288,6 @@ static void adp_destroy(void *data) free(adp); } -static int send_advertise(struct adp *adp, uint64_t now, struct entity *e) -{ - AVBTP_PACKET_ADP_SET_AVAILABLE_INDEX(&e->packet, adp->available_index++); - avbtp_server_send_packet(adp->server, &e->packet, sizeof(e->packet)); - e->last_time = now; - return 0; -} - static void check_entries(struct adp *adp, uint64_t now) { struct entity *e, *t; @@ -343,8 +386,8 @@ static int do_advertise(struct adp *adp, const char *args) e->last_time = adp->now; p = &e->packet; - AVBTP_PACKET_ADP_SET_LENGTH(p, AVBTP_ADP_DATA_LENGTH); - AVBTP_PACKET_ADP_SET_SUBTYPE(p, AVBTP_SUBTYPE_ADP); + 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); spa_json_init(&it[0], args, strlen(args)); @@ -428,6 +471,7 @@ static int do_depart(struct adp *adp, const char *args) static int do_discover(struct adp *adp, const char *args) { + send_discover(adp, 0UL); return 0; } diff --git a/src/modules/module-avbtp/adp.h b/src/modules/module-avbtp/adp.h index 1dce5eb88..1ef5d0d8a 100644 --- a/src/modules/module-avbtp/adp.h +++ b/src/modules/module-avbtp/adp.h @@ -75,23 +75,7 @@ #define AVBTP_ADP_DATA_LENGTH 56 struct avbtp_packet_adp { - uint8_t subtype; -#if __BYTE_ORDER == __BIG_ENDIAN - unsigned sv:1; - unsigned version:3; - unsigned message_type:4; - - unsigned valid_time:5; - unsigned len1:3; -#elif __BYTE_ORDER == __LITTLE_ENDIAN - unsigned message_type:4; - unsigned version:3; - unsigned sv:1; - - unsigned len1:3; - unsigned valid_time:5; -#endif - uint8_t len2:8; + struct avbtp_packet_header hdr; uint64_t entity_id; uint64_t entity_model_id; uint32_t entity_capabilities; @@ -110,12 +94,8 @@ struct avbtp_packet_adp { uint32_t reserved1; } __attribute__ ((__packed__)); -#define AVBTP_PACKET_ADP_SET_SUBTYPE(p,v) ((p)->subtype = (v)) -#define AVBTP_PACKET_ADP_SET_SV(p,v) ((p)->sv = (v)) -#define AVBTP_PACKET_ADP_SET_VERSION(p,v) ((p)->version = (v)) -#define AVBTP_PACKET_ADP_SET_MESSAGE_TYPE(p,v) ((p)->message_type = (v)) -#define AVBTP_PACKET_ADP_SET_VALID_TIME(p,v) ((p)->valid_time = (v)) -#define AVBTP_PACKET_ADP_SET_LENGTH(p,v) ((p)->len1 = ((v) >> 8),(p)->len2 = (v)) +#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)) @@ -131,12 +111,8 @@ struct avbtp_packet_adp { #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_SUBTYPE(p) ((p)->subtype) -#define AVBTP_PACKET_ADP_GET_SV(p) ((p)->sv) -#define AVBTP_PACKET_ADP_GET_VERSION(p) ((p)->version) -#define AVBTP_PACKET_ADP_GET_MESSAGE_TYPE(p) ((p)->message_type) -#define AVBTP_PACKET_ADP_GET_VALID_TIME(p) ((p)->valid_time) -#define AVBTP_PACKET_ADP_GET_LENGTH(p) (((p)->len1 << 8) | (p)->len2) +#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) diff --git a/src/modules/module-avbtp/aecp.c b/src/modules/module-avbtp/aecp.c index 5bcc960a5..3f214eefa 100644 --- a/src/modules/module-avbtp/aecp.c +++ b/src/modules/module-avbtp/aecp.c @@ -37,20 +37,60 @@ struct aecp { uint64_t now; }; -static void aecp_message_debug(struct aecp *aecp, const struct avbtp_packet_aecp *p) +static void aecp_message_debug(struct aecp *aecp, const struct avbtp_packet_aecp_header *p) { } -static int aecp_message(void *data, uint64_t now, const void *message, int len) +static int reply_not_implemented(struct aecp *aecp, const uint8_t dest[6], + const struct avbtp_packet_aecp_header *p, int len) +{ + struct server *server = aecp->server; + uint8_t buf[len]; + struct avbtp_packet_aecp_header *reply = (struct avbtp_packet_aecp_header*)buf; + + pw_log_info("reply"); + memcpy(reply, p, len); + AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(reply, AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE); + AVBTP_PACKET_AECP_SET_STATUS(reply, AVBTP_AECP_AEM_STATUS_NOT_IMPLEMENTED); + + return avbtp_server_send_packet(server, dest, reply, len); +} + +static int aecp_message(void *data, uint64_t now, const uint8_t src[6], const void *message, int len) { struct aecp *aecp = data; - const struct avbtp_packet_aecp *p = message; + const struct avbtp_packet_aecp_header *p = message; + int message_type; - if (AVBTP_PACKET_GET_SUBTYPE(p) != AVBTP_SUBTYPE_AECP) + if (AVBTP_PACKET_GET_SUBTYPE(&p->hdr) != AVBTP_SUBTYPE_AECP) return 0; - spa_debug_mem(0, p, len); + message_type = AVBTP_PACKET_AECP_GET_MESSAGE_TYPE(p); + pw_log_info("got AECP message %02x", message_type); + switch (message_type) { + case AVBTP_AECP_MESSAGE_TYPE_AEM_COMMAND: + reply_not_implemented(aecp, src, p, len); + break; + case AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE: + break; + case AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_COMMAND: + break; + case AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_RESPONSE: + break; + case AVBTP_AECP_MESSAGE_TYPE_AVC_COMMAND: + break; + case AVBTP_AECP_MESSAGE_TYPE_AVC_RESPONSE: + break; + case AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_COMMAND: + break; + case AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE: + break; + case AVBTP_AECP_MESSAGE_TYPE_EXTENDED_COMMAND: + break; + case AVBTP_AECP_MESSAGE_TYPE_EXTENDED_RESPONSE: + break; + } return 0; } diff --git a/src/modules/module-avbtp/aecp.h b/src/modules/module-avbtp/aecp.h index d41bedb8c..35f514f29 100644 --- a/src/modules/module-avbtp/aecp.h +++ b/src/modules/module-avbtp/aecp.h @@ -28,41 +28,240 @@ #include "packets.h" #include "internal.h" -#define AVBTP_AECP_DATA_LENGTH 56 -struct avbtp_packet_aecp { - uint8_t subtype; -#if __BYTE_ORDER == __BIG_ENDIAN - unsigned sv:1; - unsigned version:3; - unsigned message_type:4; +#define AVBTP_AECP_MESSAGE_TYPE_AEM_COMMAND 0 +#define AVBTP_AECP_MESSAGE_TYPE_AEM_RESPONSE 1 +#define AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_COMMAND 2 +#define AVBTP_AECP_MESSAGE_TYPE_ADDRESS_ACCESS_RESPONSE 3 +#define AVBTP_AECP_MESSAGE_TYPE_AVC_COMMAND 4 +#define AVBTP_AECP_MESSAGE_TYPE_AVC_RESPONSE 5 +#define AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_COMMAND 6 +#define AVBTP_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE 7 +#define AVBTP_AECP_MESSAGE_TYPE_EXTENDED_COMMAND 14 +#define AVBTP_AECP_MESSAGE_TYPE_EXTENDED_RESPONSE 15 - unsigned valid_time:5; - unsigned len1:3; -#elif __BYTE_ORDER == __LITTLE_ENDIAN - unsigned message_type:4; - unsigned version:3; - unsigned sv:1; +#define AVBTP_AECP_AEM_STATUS_SUCCESS 0 +#define AVBTP_AECP_AEM_STATUS_NOT_IMPLEMENTED 1 +#define AVBTP_AECP_AEM_STATUS_NO_SUCH_DESCRIPTOR 2 +#define AVBTP_AECP_AEM_STATUS_ENTITY_LOCKED 3 +#define AVBTP_AECP_AEM_STATUS_ENTITY_ACQUIRED 4 +#define AVBTP_AECP_AEM_STATUS_NOT_AUTHENTICATED 5 +#define AVBTP_AECP_AEM_STATUS_AUTHENTICATION_DISABLED 6 +#define AVBTP_AECP_AEM_STATUS_BAD_ARGUMENTS 7 +#define AVBTP_AECP_AEM_STATUS_NO_RESOURCES 8 +#define AVBTP_AECP_AEM_STATUS_IN_PROGRESS 9 +#define AVBTP_AECP_AEM_STATUS_ENTITY_MISBEHAVING 10 +#define AVBTP_AECP_AEM_STATUS_NOT_SUPPORTED 11 +#define AVBTP_AECP_AEM_STATUS_STREAM_IS_RUNNING 12 - unsigned len1:3; - unsigned valid_time:5; -#endif - uint8_t len2:8; +struct avbtp_packet_aecp_header { + struct avbtp_packet_header hdr; + uint64_t target_guid; + uint64_t controller_guid; + uint16_t sequence_id; } __attribute__ ((__packed__)); -#define AVBTP_PACKET_AECP_SET_SUBTYPE(p,v) ((p)->subtype = (v)) -#define AVBTP_PACKET_AECP_SET_SV(p,v) ((p)->sv = (v)) -#define AVBTP_PACKET_AECP_SET_VERSION(p,v) ((p)->version = (v)) -#define AVBTP_PACKET_AECP_SET_MESSAGE_TYPE(p,v) ((p)->message_type = (v)) -#define AVBTP_PACKET_AECP_SET_VALID_TIME(p,v) ((p)->valid_time = (v)) -#define AVBTP_PACKET_AECP_SET_LENGTH(p,v) ((p)->len1 = ((v) >> 8),(p)->len2 = (v)) +#define AVBTP_PACKET_AECP_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) + +#define AVBTP_AECP_AEM_CMD_ACQUIRE_ENTITY 0 +#define AVBTP_AECP_AEM_CMD_LOCK_ENTITY 1 +#define AVBTP_AECP_AEM_CMD_ENTITY_AVAILABLE 2 +#define AVBTP_AECP_AEM_CMD_CONTROLLER_AVAILABLE 3 +#define AVBTP_AECP_AEM_CMD_READ_DESCRIPTOR 4 +#define AVBTP_AECP_AEM_CMD_WRITE_DESCRIPTOR 5 +#define AVBTP_AECP_AEM_CMD_SET_CONFIGURATION 6 +#define AVBTP_AECP_AEM_CMD_GET_CONFIGURATION 7 +#define AVBTP_AECP_AEM_CMD_SET_STREAM_FORMAT 8 +#define AVBTP_AECP_AEM_CMD_GET_STREAM_FORMAT 9 +#define AVBTP_AECP_AEM_CMD_SET_VIDEO_FORMAT 10 +#define AVBTP_AECP_AEM_CMD_GET_VIDEO_FORMAT 11 +#define AVBTP_AECP_AEM_CMD_SET_SENSOR_FORMAT 12 +#define AVBTP_AECP_AEM_CMD_GET_SENSOR_FORMAT 13 +#define AVBTP_AECP_AEM_CMD_SET_STREAM_INFO 14 +#define AVBTP_AECP_AEM_CMD_GET_STREAM_INFO 15 +#define AVBTP_AECP_AEM_CMD_SET_NAME 16 +#define AVBTP_AECP_AEM_CMD_GET_NAME 17 +#define AVBTP_AECP_AEM_CMD_SET_ASSOCIATION_ID 18 +#define AVBTP_AECP_AEM_CMD_GET_ASSOCIATION_ID 19 +#define AVBTP_AECP_AEM_CMD_SET_SAMPLING_RATE 20 +#define AVBTP_AECP_AEM_CMD_GET_SAMPLING_RATE 21 +#define AVBTP_AECP_AEM_CMD_SET_CLOCK_SOURCE 22 +#define AVBTP_AECP_AEM_CMD_GET_CLOCK_SOURCE 23 +#define AVBTP_AECP_AEM_CMD_SET_CONTROL 24 +#define AVBTP_AECP_AEM_CMD_GET_CONTROL 25 +#define AVBTP_AECP_AEM_CMD_INCREMENT_CONTROL 26 +#define AVBTP_AECP_AEM_CMD_DECREMENT_CONTROL 27 +#define AVBTP_AECP_AEM_CMD_SET_SIGNAL_SELECTOR 28 +#define AVBTP_AECP_AEM_CMD_GET_SIGNAL_SELECTOR 29 +#define AVBTP_AECP_AEM_CMD_SET_MIXER 30 +#define AVBTP_AECP_AEM_CMD_GET_MIXER 31 +#define AVBTP_AECP_AEM_CMD_SET_MATRIX 32 +#define AVBTP_AECP_AEM_CMD_GET_MATRIX 33 +#define AVBTP_AECP_AEM_CMD_START_STREAMING 34 +#define AVBTP_AECP_AEM_CMD_STOP_STREAMING 35 +#define AVBTP_AECP_AEM_CMD_REGISTER_UNSOLICITED_NOTIFICATION 36 +#define AVBTP_AECP_AEM_CMD_DEREGISTER_UNSOLICITED_NOTIFICATION 37 +#define AVBTP_AECP_AEM_CMD_IDENTIFY_NOTIFICATION 38 +#define AVBTP_AECP_AEM_CMD_GET_AVB_INFO 39 +#define AVBTP_AECP_AEM_CMD_GET_AS_PATH 40 +#define AVBTP_AECP_AEM_CMD_GET_COUNTERS 41 +#define AVBTP_AECP_AEM_CMD_REBOOT 42 +#define AVBTP_AECP_AEM_CMD_GET_AUDIO_MAP 43 +#define AVBTP_AECP_AEM_CMD_ADD_AUDIO_MAPPINGS 44 +#define AVBTP_AECP_AEM_CMD_REMOVE_AUDIO_MAPPINGS 45 +#define AVBTP_AECP_AEM_CMD_GET_VIDEO_MAP 46 +#define AVBTP_AECP_AEM_CMD_ADD_VIDEO_MAPPINGS 47 +#define AVBTP_AECP_AEM_CMD_REMOVE_VIDEO_MAPPINGS 48 +#define AVBTP_AECP_AEM_CMD_GET_SENSOR_MAP 49 +#define AVBTP_AECP_AEM_CMD_ADD_SENSOR_MAPPINGS 50 +#define AVBTP_AECP_AEM_CMD_REMOVE_SENSOR_MAPPINGS 51 +#define AVBTP_AECP_AEM_CMD_START_OPERATION 52 +#define AVBTP_AECP_AEM_CMD_ABORT_OPERATION 53 +#define AVBTP_AECP_AEM_CMD_OPERATION_STATUS 54 +#define AVBTP_AECP_AEM_CMD_AUTH_ADD_KEY 55 +#define AVBTP_AECP_AEM_CMD_AUTH_DELETE_KEY 56 +#define AVBTP_AECP_AEM_CMD_AUTH_GET_KEY_COUNT 57 +#define AVBTP_AECP_AEM_CMD_AUTH_GET_KEY 58 +#define AVBTP_AECP_AEM_CMD_AUTHENTICATE 59 +#define AVBTP_AECP_AEM_CMD_DEAUTHENTICATE 60 + +struct avbtp_packet_aecp_aem_acquire { + uint32_t flags; + uint64_t owner_guid; + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_lock { + uint32_t flags; + uint64_t locked_guid; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_read_descriptor_c { + uint16_t configuration; + uint8_t reserved[2]; + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_read_descriptor_r { + uint16_t configuration; + uint8_t reserved[2]; + uint8_t descriptor[512]; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_setget_stream_format { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint64_t stream_format; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_setget_sampling_rate { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint32_t sampling_rate; +} __attribute__ ((__packed__)); + +struct avbtp_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 avbtp_packet_aecp_aem_setget_control { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_startstop_streaming { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_identify_notification { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_get_avb_info_c { + uint16_t descriptor_type; + uint16_t descriptor_id; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_get_avb_info_r { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint64_t as_grandmaster_id; + uint32_t propagation_delay; + uint16_t reserved; + uint16_t msrp_mappings_count; + uint32_t msrp_mappings; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_get_counters { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint32_t counters_valid; + uint8_t counters_block[128]; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_start_operation { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t operation_id; + uint16_t operation_type; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem_operation_status { + uint16_t descriptor_type; + uint16_t descriptor_id; + uint16_t operation_id; + uint16_t percent_complete; +} __attribute__ ((__packed__)); + +struct avbtp_packet_aecp_aem { + struct avbtp_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; + union { + struct avbtp_packet_aecp_aem_acquire acquire_entity; + struct avbtp_packet_aecp_aem_lock lock_entity; + struct avbtp_packet_aecp_aem_read_descriptor_c read_descriptor_command; + struct avbtp_packet_aecp_aem_read_descriptor_r read_descriptor_response; + struct avbtp_packet_aecp_aem_setget_stream_format stream_format; + struct avbtp_packet_aecp_aem_setget_sampling_rate sampling_rate; + struct avbtp_packet_aecp_aem_setget_clock_source clock_source; + struct avbtp_packet_aecp_aem_setget_control control; + struct avbtp_packet_aecp_aem_startstop_streaming streaming; + struct avbtp_packet_aecp_aem_get_avb_info_c avb_info_command; + struct avbtp_packet_aecp_aem_get_avb_info_r avb_info_response; + struct avbtp_packet_aecp_aem_get_counters counters; + struct avbtp_packet_aecp_aem_start_operation start_operation; + struct avbtp_packet_aecp_aem_operation_status operation_status; + }; +} __attribute__ ((__packed__)); + +#define AVBTP_PACKET_AEM_SET_COMMAND_TYPE(p,v) ((p)->cmd1 = ((v) >> 8),(p)->cmd2 = (v)) + +#define AVBTP_PACKET_AEM_GET_COMMAND_TYPE(p) ((p)->cmd1 << 8 | (p)->cmd2) -#define AVBTP_PACKET_AECP_GET_SUBTYPE(p) ((p)->subtype) -#define AVBTP_PACKET_AECP_GET_SV(p) ((p)->sv) -#define AVBTP_PACKET_AECP_GET_VERSION(p) ((p)->version) -#define AVBTP_PACKET_AECP_GET_MESSAGE_TYPE(p) ((p)->message_type) -#define AVBTP_PACKET_AECP_GET_VALID_TIME(p) ((p)->valid_time) -#define AVBTP_PACKET_AECP_GET_LENGTH(p) (((p)->len1 << 8) | (p)->len2) struct avbtp_aecp *avbtp_aecp_register(struct server *server); diff --git a/src/modules/module-avbtp/avdecc.c b/src/modules/module-avbtp/avdecc.c index dde48b9cd..afe0b32bd 100644 --- a/src/modules/module-avbtp/avdecc.c +++ b/src/modules/module-avbtp/avdecc.c @@ -47,7 +47,7 @@ #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_message(s,n,mc,m,l) server_emit(s, message, 0, n, mc, 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) @@ -66,37 +66,55 @@ static void on_socket_data(void *data, int fd, uint32_t mask) if (mask & SPA_IO_IN) { int len, count; + struct sockaddr_ll sll; uint8_t buffer[2048]; + socklen_t sl; + + sl = sizeof(sll); + len = recvfrom(fd, buffer, sizeof(buffer), 0, + (struct sockaddr *)&sll, &sl); + + if (sll.sll_protocol != htons(ETH_P_TSN)) + return; - len = read(fd, buffer, sizeof(buffer)); if (len < 0) { - pw_log_warn("got error: %m"); + pw_log_warn("got recv error: %m"); } - else if (len < (int)sizeof(struct avbtp_packet_common)) { + else if (len < (int)sizeof(struct avbtp_packet_header)) { pw_log_warn("short packet received (%d < %d)", len, - (int)sizeof(struct avbtp_packet_common)); + (int)sizeof(struct avbtp_packet_header)); } else { clock_gettime(CLOCK_REALTIME, &now); server_emit_message(server, SPA_TIMESPEC_TO_NSEC(&now), - buffer, len); + sll.sll_addr, buffer, len); } } } -int avbtp_server_send_packet(struct server *server, void *data, size_t size) +int avbtp_server_send_packet(struct server *server, const uint8_t dest[6], void *data, size_t size) { struct sockaddr_ll sll; - uint8_t dest[ETH_ALEN] = { 0x91, 0xe0, 0xf0, 0x01, 0x00, 0x00 }; + int res = 0; spa_zero(sll); sll.sll_family = AF_PACKET; sll.sll_protocol = htons(ETH_P_TSN); - sll.sll_ifindex = server->ifindex; + sll.sll_ifindex = server->ifindex; sll.sll_halen = ETH_ALEN; memcpy(sll.sll_addr, dest, ETH_ALEN); - return sendto(server->source->fd, data, size, 0, - (struct sockaddr *)&sll, sizeof(sll)); + if ((res = sendto(server->source->fd, data, size, 0, + (struct sockaddr *)&sll, sizeof(sll))) < 0) { + res = -errno; + pw_log_warn("got send error: %m"); + } + return res; +} + +int avbtp_server_broadcast_packet(struct server *server, void *data, size_t size) +{ + static const uint8_t dest[6] = { 0x91, 0xe0, 0xf0, 0x01, 0x00, 0x00 }; + return avbtp_server_send_packet(server, dest, data, size); } static int setup_socket(struct server *server) @@ -108,7 +126,7 @@ static int setup_socket(struct server *server) struct sockaddr_ll sll; struct timespec value, interval; - fd = socket(AF_PACKET, SOCK_DGRAM|SOCK_NONBLOCK, htons(ETH_P_TSN)); + fd = socket(AF_PACKET, SOCK_DGRAM|SOCK_NONBLOCK, htons(ETH_P_ALL)); if (fd < 0) { pw_log_error("socket() failed: %m"); return -errno; @@ -141,11 +159,21 @@ static int setup_socket(struct server *server) (uint64_t)server->mac_addr[4] << 8 | (uint64_t)server->mac_addr[5]; - pw_log_info("%lx", server->entity_id); + pw_log_info("%lx %d", server->entity_id, server->ifindex); + + spa_zero(mreq); + mreq.mr_ifindex = server->ifindex; + mreq.mr_type = PACKET_MR_PROMISC; + 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; + } spa_zero(sll); sll.sll_family = AF_PACKET; - sll.sll_protocol = htons(ETH_P_TSN); + sll.sll_protocol = htons(ETH_P_ALL); sll.sll_ifindex = server->ifindex; if (bind(fd, (struct sockaddr *) &sll, sizeof(sll)) < 0) { res = -errno; @@ -153,16 +181,6 @@ static int setup_socket(struct server *server) goto error_close; } - spa_zero(mreq); - mreq.mr_ifindex = server->ifindex; - mreq.mr_type = PACKET_MR_ALLMULTI; - if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, - &mreq, sizeof(struct packet_mreq)) < 0) { - res = -errno; - pw_log_error("setsockopt(ADD_MEMBERSHIP) failed: %m"); - goto error_close; - } - server->source = pw_loop_add_io(impl->loop, fd, SPA_IO_IN, true, on_socket_data, server); if (server->source == NULL) { res = -errno; diff --git a/src/modules/module-avbtp/internal.h b/src/modules/module-avbtp/internal.h index 3e560bad0..2a0d4d942 100644 --- a/src/modules/module-avbtp/internal.h +++ b/src/modules/module-avbtp/internal.h @@ -49,7 +49,7 @@ struct server_events { /** the server is destroyed */ void (*destroy) (void *data); - int (*message) (void *data, uint64_t now, const void *message, int len); + int (*message) (void *data, uint64_t now, const uint8_t src[6], const void *message, int len); void (*periodic) (void *data, uint64_t now); @@ -78,7 +78,9 @@ void avdecc_server_free(struct server *server); void avdecc_server_add_listener(struct server *server, struct spa_hook *listener, const struct server_events *events, void *data); -int avbtp_server_send_packet(struct server *server, void *data, size_t size); + +int avbtp_server_broadcast_packet(struct server *server, void *data, size_t size); +int avbtp_server_send_packet(struct server *server, const uint8_t dest[6], void *data, size_t size); #ifdef __cplusplus } /* extern "C" */ diff --git a/src/modules/module-avbtp/maap.c b/src/modules/module-avbtp/maap.c index 91902c29c..2203eb79b 100644 --- a/src/modules/module-avbtp/maap.c +++ b/src/modules/module-avbtp/maap.c @@ -65,7 +65,7 @@ static void maap_message_debug(struct maap *maap, const struct avbtp_packet_maap pw_log_info(" conflict-count: %d", AVBTP_PACKET_MAAP_GET_CONFLICT_COUNT(p)); } -static int maap_message(void *data, uint64_t now, const void *message, int len) +static int maap_message(void *data, uint64_t now, const uint8_t src[6], const void *message, int len) { struct maap *maap = data; const struct avbtp_packet_maap *p = message; diff --git a/src/modules/module-avbtp/packets.h b/src/modules/module-avbtp/packets.h index 1bbf12138..c2721b1ab 100644 --- a/src/modules/module-avbtp/packets.h +++ b/src/modules/module-avbtp/packets.h @@ -48,68 +48,46 @@ #define AVBTP_SUBTYPE_MAAP 0xFE #define AVBTP_SUBTYPE_EF_CONTROL 0xFF -struct avbtp_packet_common { +struct avbtp_ehternet_header { + uint8_t dest[6]; + uint8_t src[6]; + uint16_t type; +} __attribute__ ((__packed__)); + +struct avbtp_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 - uint16_t subtype_data2; - uint64_t stream_id; - uint8_t payload[0]; + uint8_t len2:8; } __attribute__ ((__packed__)); #define AVBTP_PACKET_SET_SUBTYPE(p,v) ((p)->subtype = (v)) #define AVBTP_PACKET_SET_SV(p,v) ((p)->sv = (v)) #define AVBTP_PACKET_SET_VERSION(p,v) ((p)->version = (v)) -#define AVBTP_PACKET_SET_STREAM_ID(p,v) ((p)->stream_id = htobe64(v)) +#define AVBTP_PACKET_SET_SUB1(p,v) ((p)->subtype_data1 = (v)) +#define AVBTP_PACKET_SET_SUB2(p,v) ((p)->subtype_data2 = (v)) +#define AVBTP_PACKET_SET_LENGTH(p,v) ((p)->len1 = ((v) >> 8),(p)->len2 = (v)) #define AVBTP_PACKET_GET_SUBTYPE(p) ((p)->subtype) #define AVBTP_PACKET_GET_SV(p) ((p)->sv) #define AVBTP_PACKET_GET_VERSION(p) ((p)->version) -#define AVBTP_PACKET_GET_STREAM_ID(p) be64toh((p)->stream_id) - -struct avbtp_packet_cc { - uint8_t subtype; -#if __BYTE_ORDER == __BIG_ENDIAN - unsigned sv:1; - unsigned version:3; - unsigned control_data1:4; - - unsigned status:5; - unsigned len1:3; -#elif __BYTE_ORDER == __LITTLE_ENDIAN - unsigned control_data1:4; - unsigned version:3; - unsigned sv:1; - - unsigned len1:3; - unsigned status:5; -#endif - uint8_t len2:8; - uint64_t stream_id; - uint8_t payload[0]; -} __attribute__ ((__packed__)); - -#define AVBTP_PACKET_CC_SET_SUBTYPE(p,v) ((p)->subtype = (v)) -#define AVBTP_PACKET_CC_SET_SV(p,v) ((p)->sv = (v)) -#define AVBTP_PACKET_CC_SET_VERSION(p,v) ((p)->version = (v)) -#define AVBTP_PACKET_CC_SET_STREAM_ID(p,v) ((p)->stream_id = htobe64(v)) -#define AVBTP_PACKET_CC_SET_STATUS(p,v) ((p)->status = (v)) -#define AVBTP_PACKET_CC_SET_LENGTH(p,v) ((p)->len1 = ((v) >> 8),(p)->len2 = (v)) - -#define AVBTP_PACKET_CC_GET_SUBTYPE(p) ((p)->subtype) -#define AVBTP_PACKET_CC_GET_SV(p) ((p)->sv) -#define AVBTP_PACKET_CC_GET_VERSION(p) ((p)->version) -#define AVBTP_PACKET_CC_GET_STREAM_ID(p) be64toh((p)->stream_id) -#define AVBTP_PACKET_CC_GET_STATUS(p) ((p)->status) -#define AVBTP_PACKET_CC_GET_LENGTH(p) ((p)->len1 << 8 || (p)->len2) +#define AVBTP_PACKET_GET_SUB1(p) ((p)->subtype_data1) +#define AVBTP_PACKET_GET_SUB2(p) ((p)->subtype_data2) +#define AVBTP_PACKET_GET_LENGTH(p) ((p)->len1 << 8 | (p)->len2) #endif /* AVBTP_PACKETS_H */