From 0e1608747d6641c0cc73cffd5e44bf61641990a8 Mon Sep 17 00:00:00 2001 From: Milanka Ringwald Date: Sat, 26 Nov 2016 10:49:06 +0100 Subject: [PATCH] separate acceptor and initiator role --- test/avdtp/avdtp.h | 65 +-- test/avdtp/avdtp_acceptor.c | 183 ++++---- test/avdtp/avdtp_acceptor.h | 7 +- test/avdtp/avdtp_initiator.c | 163 +++---- test/avdtp/avdtp_initiator.h | 6 +- test/avdtp/avdtp_sink.c | 332 +++++++------- test/avdtp/avdtp_sink.h | 2 - test/avdtp/avdtp_sink_working.c | 741 ++++++++++++++++++++++++++++++++ 8 files changed, 1083 insertions(+), 416 deletions(-) create mode 100644 test/avdtp/avdtp_sink_working.c diff --git a/test/avdtp/avdtp.h b/test/avdtp/avdtp.h index 2e7bd2d2a..68508c250 100644 --- a/test/avdtp/avdtp.h +++ b/test/avdtp/avdtp.h @@ -115,8 +115,8 @@ typedef enum { AVDTP_SI_SET_CONFIGURATION, AVDTP_SI_GET_CONFIGURATION, AVDTP_SI_RECONFIGURE, //5 - AVDTP_SI_OPEN, - AVDTP_SI_START, + AVDTP_SI_OPEN, //6 + AVDTP_SI_START, //7 AVDTP_SI_CLOSE, AVDTP_SI_SUSPEND, AVDTP_SI_ABORT, //10 @@ -280,13 +280,11 @@ typedef enum { typedef enum { AVDTP_STREAM_ENDPOINT_IDLE, + AVDTP_STREAM_ENDPOINT_CONFIGURATION_SUBSTATEMACHINE, AVDTP_STREAM_ENDPOINT_CONFIGURED, - AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM, AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED, - AVDTP_STREAM_ENDPOINT_OPENED, // 5 - AVDTP_STREAM_ENDPOINT_W2_ANSWER_START_STREAM, - AVDTP_STREAM_ENDPOINT_W4_STREAMING_CONNECTION_OPEN, - AVDTP_STREAM_ENDPOINT_STREAMING, // 8 + AVDTP_STREAM_ENDPOINT_OPENED, + AVDTP_STREAM_ENDPOINT_STREAMING, AVDTP_STREAM_ENDPOINT_CLOSING, AVDTP_STREAM_ENDPOINT_ABORTING, AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED @@ -294,8 +292,6 @@ typedef enum { typedef enum { AVDTP_INITIATOR_STREAM_CONFIG_IDLE, - AVDTP_INITIATOR_W2_DISCOVER_SEPS, - AVDTP_INITIATOR_W4_SEPS_DISCOVERED, AVDTP_INITIATOR_W2_GET_CAPABILITIES, AVDTP_INITIATOR_W4_CAPABILITIES, AVDTP_INITIATOR_W2_GET_ALL_CAPABILITIES, @@ -304,18 +300,21 @@ typedef enum { AVDTP_INITIATOR_W4_CONFIGURATION_SET, AVDTP_INITIATOR_W2_GET_CONFIGURATION, AVDTP_INITIATOR_W4_CONFIGURATION_RECEIVED, - AVDTP_INITIATOR_STREAM_CONFIG_DONE -} avdtp_initiator_stream_config_state_t; + AVDTP_INITIATOR_STREAM_CONFIGURED +} avdtp_initiator_stream_endpoint_state_t; typedef enum { AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE, - AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS, + AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD, AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES, AVDTP_ACCEPTOR_W2_ANSWER_GET_ALL_CAPABILITIES, AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION, - AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD, - AVDTP_ACCEPTOR_STREAM_CONFIG_DONE -} avdtp_acceptor_stream_config_state_t; + AVDTP_ACCEPTOR_STREAM_CONFIGURED, + AVDTP_ACCEPTOR_W2_ANSWER_OPEN_STREAM, + AVDTP_ACCEPTOR_W4_L2CAP_FOR_MEDIA_CONNECTED, + AVDTP_ACCEPTOR_W2_ANSWER_START_STREAM, + AVDTP_ACCEPTOR_STREAMING +} avdtp_acceptor_stream_endpoint_state_t; typedef struct { uint8_t seid; // 0x01 – 0x3E, 6bit @@ -331,12 +330,23 @@ typedef struct { typedef enum { AVDTP_SIGNALING_CONNECTION_IDLE, AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED, - AVDTP_SIGNALING_CONNECTION_CONFIGURATION_SUBSTATEMACHINE, - AVDTP_SIGNALING_CONNECTION_CONFIGURED, - AVDTP_SIGNALING_CONNECTION_STREAM_ENPOINT_IN_USE, + AVDTP_SIGNALING_CONNECTION_OPENED, + // AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS, + //AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS, + //AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED, AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED } avdtp_connection_state_t; +typedef enum { + AVDTP_SIGNALING_CONNECTION_ACCEPTOR_IDLE, + AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS +} avdtp_acceptor_connection_state_t; + +typedef enum { + AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED +} avdtp_initiator_connection_state_t; typedef struct { btstack_linked_item_t item; @@ -344,22 +354,20 @@ typedef struct { bd_addr_t remote_addr; hci_con_handle_t con_handle; uint16_t l2cap_signaling_cid; - avdtp_connection_state_t state; avdtp_service_mode_t service_mode; - - avdtp_initiator_stream_config_state_t initiator_config_state; - avdtp_acceptor_stream_config_state_t acceptor_config_state; - + + avdtp_connection_state_t state; + avdtp_acceptor_connection_state_t acceptor_connection_state; + avdtp_initiator_connection_state_t initiator_connection_state; + uint8_t disconnect; uint8_t initiator_transaction_label; uint8_t acceptor_transaction_label; uint8_t query_seid; - avdtp_signal_identifier_t unknown_signal_identifier; uint8_t wait_to_send_acceptor; uint8_t wait_to_send_initiator; uint8_t wait_to_send_self; - } avdtp_connection_t; @@ -371,7 +379,12 @@ typedef struct avdtp_stream_endpoint { uint16_t l2cap_reporting_cid; uint16_t l2cap_recovery_cid; - avdtp_stream_endpoint_state_t state; + avdtp_stream_endpoint_state_t state; + avdtp_acceptor_stream_endpoint_state_t acceptor_config_state; + avdtp_initiator_stream_endpoint_state_t initiator_config_state; + + avdtp_signal_identifier_t unknown_signal_identifier; + // active connection avdtp_connection_t * connection; // store remote seps diff --git a/test/avdtp/avdtp_acceptor.c b/test/avdtp/avdtp_acceptor.c index 2e3f2d4c4..66b189516 100644 --- a/test/avdtp/avdtp_acceptor.c +++ b/test/avdtp/avdtp_acceptor.c @@ -150,21 +150,6 @@ static uint16_t avdtp_unpack_service_capabilities(avdtp_capabilities_t * caps, u } return registered_service_categories; } -static int avdtp_acceptor_send_seps_response(uint16_t cid, uint8_t transaction_label, avdtp_stream_endpoint_t * endpoints){ - uint8_t command[2+2*MAX_NUM_SEPS]; - int pos = 0; - command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); - command[pos++] = (uint8_t)AVDTP_SI_DISCOVER; - - btstack_linked_list_iterator_t it; - btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) endpoints); - while (btstack_linked_list_iterator_has_next(&it)){ - avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); - command[pos++] = (stream_endpoint->sep.seid << 2) | (stream_endpoint->sep.in_use<<1); - command[pos++] = (stream_endpoint->sep.media_type << 4) | (stream_endpoint->sep.type << 3); - } - return l2cap_send(cid, command, pos); -} static inline int avdtp_acceptor_send_capabilities(uint16_t cid, uint8_t transaction_label, avdtp_sep_t sep, uint8_t pack_all_capabilities){ uint8_t command[100]; @@ -202,73 +187,34 @@ static int avdtp_acceptor_send_all_capabilities_response(uint16_t cid, uint8_t t return avdtp_acceptor_send_capabilities(cid, transaction_label, sep, 1); } -int avdtp_acceptor_send_accept_response(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ +static int avdtp_acceptor_send_accept_response(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ uint8_t command[2]; command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); command[1] = (uint8_t)identifier; return l2cap_send(cid, command, sizeof(command)); } -void avdtp_acceptor_stream_config_subsm_init(avdtp_connection_t * connection){ - connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; -} - -int avdtp_acceptor_stream_config_subsm_is_configured(avdtp_connection_t * connection){ - return connection->acceptor_config_state == AVDTP_ACCEPTOR_STREAM_CONFIG_DONE; -} - -static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_active_seid(uint8_t seid){ - btstack_linked_list_iterator_t it; - btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); - while (btstack_linked_list_iterator_has_next(&it)){ - avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); - if (stream_endpoint->sep.seid == seid){ - return stream_endpoint; - } - } - return NULL; -} - -int avdtp_acceptor_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size){ - if (!connection) return 0; - - avdtp_signaling_packet_header_t signaling_header; - avdtp_read_signaling_header(&signaling_header, packet, size); - - // printf("SIGNALING HEADER: tr_label %d, packet_type %d, msg_type %d, signal_identifier %02x\n", - // signaling_header.transaction_label, signaling_header.packet_type, signaling_header.message_type, signaling_header.signal_identifier); - // printf("acceptor sm: state %d, si %d\n", connection->acceptor_config_state, signaling_header.signal_identifier); - - int i = 2; - avdtp_sep_t sep; - - connection->acceptor_transaction_label = signaling_header.transaction_label; - avdtp_stream_endpoint_t * stream_endpoint = NULL; +int avdtp_acceptor_stream_config_subsm(avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t *packet, uint16_t size){ + if (!stream_endpoint) return 0; int request_to_send = 1; - switch (connection->acceptor_config_state){ + + switch (stream_endpoint->acceptor_config_state){ case AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE: - switch (signaling_header.signal_identifier){ - case AVDTP_SI_DISCOVER: - printf(" ACP SM: AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS\n"); - connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS; - break; + switch (signaling_header->signal_identifier){ case AVDTP_SI_GET_ALL_CAPABILITIES: - printf(" ACP SM: AVDTP_SI_GET_ALL_CAPABILITIES\n"); - connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_ALL_CAPABILITIES; + printf(" ACP: AVDTP_SI_GET_ALL_CAPABILITIES\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_ALL_CAPABILITIES; break; case AVDTP_SI_GET_CAPABILITIES: - printf(" ACP SM: AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES\n"); - connection->query_seid = packet[2] >> 2; - connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES; + printf(" ACP: AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES; break; - case AVDTP_SI_SET_CONFIGURATION: - printf(" ACP SM: AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION \n"); - connection->query_seid = packet[2] >> 2; + case AVDTP_SI_SET_CONFIGURATION:{ + printf(" ACP: AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION \n"); + avdtp_sep_t sep; sep.seid = packet[3] >> 2; - - stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); - if (!stream_endpoint) return 0; // find or add sep + int i; for (i=0; i < stream_endpoint->remote_seps_num; i++){ if (stream_endpoint->remote_seps[i].seid == sep.seid){ stream_endpoint->remote_sep_index = i; @@ -276,25 +222,55 @@ int avdtp_acceptor_stream_config_subsm(avdtp_connection_t * connection, uint8_t } sep.registered_service_categories = avdtp_unpack_service_capabilities(&sep.capabilities, packet+4, size-4); stream_endpoint->remote_seps[stream_endpoint->remote_sep_index] = sep; - connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION; + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION; break; - + } default: - printf(" ACP SM: NOT IMPLEMENTED, Reject signal_identifier %02x\n", signaling_header.signal_identifier); - connection->acceptor_config_state = AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD; - connection->unknown_signal_identifier = signaling_header.signal_identifier; + printf(" ACP: NOT IMPLEMENTED, Reject signal_identifier %02x\n", signaling_header->signal_identifier); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD; + stream_endpoint->unknown_signal_identifier = signaling_header->signal_identifier; break; } break; - case AVDTP_ACCEPTOR_STREAM_CONFIG_DONE: - request_to_send = 0; - printf(" ACP SM: AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION\n"); + case AVDTP_ACCEPTOR_STREAM_CONFIGURED: + switch (signaling_header->signal_identifier){ + case AVDTP_SI_OPEN: + if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_CONFIGURED) return 0; + printf(" ACP: AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_OPEN_STREAM; + stream_endpoint->connection->acceptor_transaction_label = signaling_header->transaction_label; + break; + default: + printf(" ACP: AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM unhandled signal %d ", signaling_header->signal_identifier); + return 0; + } break; + case AVDTP_ACCEPTOR_W2_ANSWER_OPEN_STREAM: + switch (signaling_header->signal_identifier){ + case AVDTP_SI_START: + if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_OPENED) return 0; + printf(" ACP: AVDTP_ACCEPTOR_W2_ANSWER_START_STREAM\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_W2_ANSWER_START_STREAM; + stream_endpoint->connection->acceptor_transaction_label = signaling_header->transaction_label; + break; + default: + printf(" ACP: AVDTP_ACCEPTOR_W2_ANSWER_OPEN_STREAM unhandled signal %d ", signaling_header->signal_identifier); + return 0; + } + break; + + case AVDTP_ACCEPTOR_W2_ANSWER_START_STREAM: + printf(" AVDTP_ACCEPTOR_W2_ANSWER_START_STREAM state %d, si %d \n", stream_endpoint->state, signaling_header->signal_identifier); + break; + default: request_to_send = 0; - printf(" ACP SM: NOT IMPLEMENTED\n"); break; } + + if (!request_to_send){ + printf(" ACP: NOT IMPLEMENTED\n"); + } return request_to_send; } @@ -306,48 +282,49 @@ static int avdtp_acceptor_send_response_reject(uint16_t cid, avdtp_signal_ident return l2cap_send(cid, command, sizeof(command)); } -int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection){ +int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint){ + if (!stream_endpoint) return 0; + int sent = 1; - avdtp_stream_endpoint_t * stream_endpoint = NULL; - switch (connection->acceptor_config_state){ - case AVDTP_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS: - printf(" ACP RUN: AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE\n"); - connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; - avdtp_acceptor_send_seps_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, (avdtp_stream_endpoint_t *)&stream_endpoints); + switch (stream_endpoint->acceptor_config_state){ + case AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE: break; case AVDTP_ACCEPTOR_W2_ANSWER_GET_CAPABILITIES: - stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); - if (!stream_endpoint) return 0; - printf(" ACP RUN: AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE\n"); - connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; + printf(" ACP: DONE\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; avdtp_acceptor_send_capabilities_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, stream_endpoint->sep); break; case AVDTP_ACCEPTOR_W2_ANSWER_GET_ALL_CAPABILITIES: - stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); - if (!stream_endpoint) return 0;connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; - printf(" ACP RUN: AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; + printf(" ACP: DONE\n"); avdtp_acceptor_send_all_capabilities_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, stream_endpoint->sep); break; - case AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD: - connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; - printf(" ACP RUN: AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE\n"); - avdtp_acceptor_send_response_reject(connection->l2cap_signaling_cid, connection->unknown_signal_identifier, connection->acceptor_transaction_label); - break; case AVDTP_ACCEPTOR_W2_ANSWER_SET_CONFIGURATION: - stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); - if (!stream_endpoint) return 0; - printf(" ACP RUN: AVDTP_ACCEPTOR_STREAM_CONFIG_DONE\n"); - connection->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_DONE; + printf(" ACP: DONE\n"); + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIGURED; stream_endpoint->connection = connection; stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; avdtp_acceptor_send_accept_response(connection->l2cap_signaling_cid, AVDTP_SI_SET_CONFIGURATION, connection->acceptor_transaction_label); break; - case AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE: + case AVDTP_ACCEPTOR_W2_REJECT_UNKNOWN_CMD: + stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; + printf(" ACP: DONE\n"); + avdtp_acceptor_send_response_reject(connection->l2cap_signaling_cid, stream_endpoint->unknown_signal_identifier, connection->acceptor_transaction_label); break; + case AVDTP_ACCEPTOR_W2_ANSWER_OPEN_STREAM: + printf(" ACP: AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED\n"); + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED; + avdtp_acceptor_send_accept_response(stream_endpoint->connection->l2cap_signaling_cid, AVDTP_SI_OPEN, stream_endpoint->connection->acceptor_transaction_label); + break; + case AVDTP_ACCEPTOR_W2_ANSWER_START_STREAM: + printf(" ACP: AVDTP_STREAM_ENDPOINT_STREAMING \n"); + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; + avdtp_acceptor_send_accept_response(stream_endpoint->connection->l2cap_signaling_cid, AVDTP_SI_START, stream_endpoint->connection->acceptor_transaction_label); + break; + default: - printf(" ACP RUN: NOT IMPLEMENTED\n"); + printf(" ACP: NOT IMPLEMENTED\n"); return 0; } return sent; } - diff --git a/test/avdtp/avdtp_acceptor.h b/test/avdtp/avdtp_acceptor.h index afac57cfa..bf00393ff 100644 --- a/test/avdtp/avdtp_acceptor.h +++ b/test/avdtp/avdtp_acceptor.h @@ -51,11 +51,8 @@ extern "C" { #endif -void avdtp_acceptor_stream_config_subsm_init(avdtp_connection_t * connection); -int avdtp_acceptor_stream_config_subsm_is_configured(avdtp_connection_t * connection); -int avdtp_acceptor_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size); -int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection); -int avdtp_acceptor_send_accept_response(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label); +int avdtp_acceptor_stream_config_subsm(avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t *packet, uint16_t size); +int avdtp_acceptor_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint); #if defined __cplusplus } diff --git a/test/avdtp/avdtp_initiator.c b/test/avdtp/avdtp_initiator.c index 03c9940c5..fbdbd1d0e 100644 --- a/test/avdtp/avdtp_initiator.c +++ b/test/avdtp/avdtp_initiator.c @@ -47,125 +47,60 @@ #include "avdtp_util.h" #include "avdtp_initiator.h" -// static int avdtp_initiator_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ -// uint8_t command[2]; -// command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); -// command[1] = (uint8_t)identifier; -// return l2cap_send(cid, command, sizeof(command)); -// } - -// static int avdtp_initiator_send_get_all_capabilities_cmd(uint16_t cid, uint8_t transaction_label, uint8_t sep_id){ -// uint8_t command[3]; -// command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); -// command[1] = AVDTP_SI_GET_ALL_CAPABILITIES; -// command[2] = sep_id << 2; -// return l2cap_send(cid, command, sizeof(command)); -// } - -// static int avdtp_initiator_send_get_capabilities_cmd(uint16_t cid, uint8_t transaction_label, uint8_t sep_id){ -// uint8_t command[3]; -// command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); -// command[1] = AVDTP_SI_GET_CAPABILITIES; -// command[2] = sep_id << 2; -// return l2cap_send(cid, command, sizeof(command)); -// } - -void avdtp_initiator_stream_config_subsm_init(avdtp_connection_t * connection){ - connection->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; +static int avdtp_initiator_send_get_all_capabilities_cmd(uint16_t cid, uint8_t transaction_label, uint8_t sep_id){ + uint8_t command[3]; + command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); + command[1] = AVDTP_SI_GET_ALL_CAPABILITIES; + command[2] = sep_id << 2; + return l2cap_send(cid, command, sizeof(command)); } -int avdtp_initiator_stream_config_subsm_is_configured(avdtp_connection_t * connection){ - return 1; //connection->initiator_config_state == AVDTP_INITIATOR_STREAM_CONFIG_DONE; +static int avdtp_initiator_send_get_capabilities_cmd(uint16_t cid, uint8_t transaction_label, uint8_t sep_id){ + uint8_t command[3]; + command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); + command[1] = AVDTP_SI_GET_CAPABILITIES; + command[2] = sep_id << 2; + return l2cap_send(cid, command, sizeof(command)); } -int avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size){ - return 0; - // if (avdtp_initiator_stream_config_subsm_run(connection, stream_endpoint)) return 1; - // int i; - // int responded = 1; - // avdtp_sep_t sep; + +int avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t *packet, uint16_t size){ + //if (!stream_endpoint) return 0; + if (avdtp_initiator_stream_config_subsm_run(connection, stream_endpoint)) return 1; + + int responded = 1; - // avdtp_signaling_packet_header_t signaling_header; - // avdtp_read_signaling_header(&signaling_header, packet, size); - - // switch (connection->initiator_config_state){ - // case AVDTP_INITIATOR_W4_SEPS_DISCOVERED: - // printf(" AVDTP_INITIATOR_W4_SEPS_DISCOVERED -> AVDTP_INITIATOR_W2_GET_CAPABILITIES\n"); - - // if (signaling_header.transaction_label != connection->initiator_transaction_label){ - // printf("unexpected transaction label, got %d, expected %d\n", signaling_header.transaction_label, connection->initiator_transaction_label); - // return 0; - // } - // if (signaling_header.signal_identifier != AVDTP_SI_DISCOVER) { - // printf("unexpected signal identifier ...\n"); - // return 0; - // } - // if (signaling_header.message_type != AVDTP_RESPONSE_ACCEPT_MSG){ - // printf("request rejected...\n"); - // return 0; - // } - // if (size == 3){ - // printf("ERROR code %02x\n", packet[2]); - // return 0; - // } - // for (i = 2; i> 2; - // if (sep.seid < 0x01 || sep.seid > 0x3E){ - // printf("invalid sep id\n"); - // return 0; - // } - // sep.in_use = (packet[i] >> 1) & 0x01; - // sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); - // sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); - // stream_endpoint->remote_seps[stream_endpoint->remote_seps_num++] = sep; - // // printf("found sep: seid %u, in_use %d, media type %d, sep type %d (1-SNK)\n", - // // sep.seid, sep.in_use, sep.media_type, sep.type); - // } - // connection->initiator_config_state = AVDTP_INITIATOR_W2_GET_CAPABILITIES; - // connection->initiator_transaction_label++; - // l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); - // responded = 1; - // break; - // case AVDTP_INITIATOR_W4_CAPABILITIES: - // printf(" Received basic capabilities -> NOT IMPLEMENTED\n"); - // responded = 0; - // break; - // case AVDTP_INITIATOR_W4_ALL_CAPABILITIES: - // printf(" Received all capabilities -> NOT IMPLEMENTED\n"); - // responded = 0; - // break; - // default: - // printf(" INT : NOT IMPLEMENTED sig. ID %02x\n", signaling_header.signal_identifier); - // //printf_hexdump( packet, size ); - // responded = 0; - // break; - // } - // return responded; + switch (stream_endpoint->initiator_config_state){ + case AVDTP_INITIATOR_W4_CAPABILITIES: + printf(" INT : AVDTP_INITIATOR_W4_CAPABILITIES, Received basic capabilities -> NOT IMPLEMENTED\n"); + return 0; + case AVDTP_INITIATOR_W4_ALL_CAPABILITIES: + printf(" INT : AVDTP_INITIATOR_W4_ALL_CAPABILITIES -> NOT IMPLEMENTED\n"); + return 0; + default: + printf(" INT : NOT IMPLEMENTED sig. ID %02x\n", signaling_header->signal_identifier); + //printf_hexdump( packet, size ); + return 0; + } + return responded; } -int avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection){ - return 0; - // int sent = 1; - // switch (connection->initiator_config_state){ - // case AVDTP_INITIATOR_STREAM_CONFIG_IDLE: - // case AVDTP_INITIATOR_W2_DISCOVER_SEPS: - // printf(" AVDTP_INITIATOR_STREAM_CONFIG_IDLE | AVDTP_INITIATOR_W2_DISCOVER_SEPS -> AVDTP_INITIATOR_W4_SEPS_DISCOVERED\n"); - // connection->initiator_config_state = AVDTP_INITIATOR_W4_SEPS_DISCOVERED; - // avdtp_initiator_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_SI_DISCOVER, connection->initiator_transaction_label); - // break; - // case AVDTP_INITIATOR_W2_GET_CAPABILITIES: - // printf(" AVDTP_INITIATOR_W2_GET_CAPABILITIES -> AVDTP_INITIATOR_W4_CAPABILITIES\n"); - // connection->initiator_config_state = AVDTP_INITIATOR_W4_CAPABILITIES; - // avdtp_initiator_send_get_capabilities_cmd(connection->l2cap_signaling_cid, connection->initiator_transaction_label, connection->query_seid); - // break; - // case AVDTP_INITIATOR_W2_GET_ALL_CAPABILITIES: - // printf(" AVDTP_INITIATOR_W2_GET_ALL_CAPABILITIES -> AVDTP_INITIATOR_W4_ALL_CAPABILITIES\n"); - // connection->initiator_config_state = AVDTP_INITIATOR_W4_ALL_CAPABILITIES; - // avdtp_initiator_send_get_all_capabilities_cmd(connection->l2cap_signaling_cid, connection->initiator_transaction_label, connection->query_seid); - // break; - // default: - // sent = 0; - // break; - // } - // return sent; +int avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint){ + int sent = 1; + switch (stream_endpoint->initiator_config_state){ + case AVDTP_INITIATOR_W2_GET_CAPABILITIES: + printf(" INT: AVDTP_INITIATOR_W2_GET_CAPABILITIES -> AVDTP_INITIATOR_W4_CAPABILITIES\n"); + stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W4_CAPABILITIES; + avdtp_initiator_send_get_capabilities_cmd(connection->l2cap_signaling_cid, connection->initiator_transaction_label, connection->query_seid); + break; + case AVDTP_INITIATOR_W2_GET_ALL_CAPABILITIES: + printf(" INT: AVDTP_INITIATOR_W2_GET_ALL_CAPABILITIES -> AVDTP_INITIATOR_W4_ALL_CAPABILITIES\n"); + stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W4_ALL_CAPABILITIES; + avdtp_initiator_send_get_all_capabilities_cmd(connection->l2cap_signaling_cid, connection->initiator_transaction_label, connection->query_seid); + break; + default: + sent = 0; + break; + } + return sent; } diff --git a/test/avdtp/avdtp_initiator.h b/test/avdtp/avdtp_initiator.h index 829015d34..73d9d4d5c 100644 --- a/test/avdtp/avdtp_initiator.h +++ b/test/avdtp/avdtp_initiator.h @@ -51,10 +51,8 @@ extern "C" { #endif -void avdtp_initiator_stream_config_subsm_init(avdtp_connection_t * connection); -int avdtp_initiator_stream_config_subsm_is_configured(avdtp_connection_t * connection); -int avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, uint8_t *packet, uint16_t size); -int avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection); +int avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, avdtp_signaling_packet_header_t * signaling_header, uint8_t *packet, uint16_t size); +int avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint); #if defined __cplusplus } diff --git a/test/avdtp/avdtp_sink.c b/test/avdtp/avdtp_sink.c index a9c54de78..eb33e4f16 100644 --- a/test/avdtp/avdtp_sink.c +++ b/test/avdtp/avdtp_sink.c @@ -56,6 +56,7 @@ static const char * default_avdtp_sink_service_provider_name = "BTstack AVDTP Si static btstack_linked_list_t avdtp_connections; static uint16_t stream_endpoints_id_counter; +static btstack_linked_list_t stream_endpoints; static btstack_packet_handler_t avdtp_sink_callback; static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); @@ -301,6 +302,18 @@ static avdtp_connection_t * get_avdtp_connection_for_con_handle(hci_con_handle_t return NULL; } +static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_active_seid(uint8_t seid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + if (stream_endpoint->sep.seid == seid){ + return stream_endpoint; + } + } + return NULL; +} + static avdtp_connection_t * get_avdtp_connection_for_l2cap_signaling_cid(uint16_t l2cap_cid){ btstack_linked_list_iterator_t it; btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &avdtp_connections); @@ -341,76 +354,66 @@ static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_connection(avdtp_ return NULL; } +static int avdtp_send_seps_response(uint16_t cid, uint8_t transaction_label, avdtp_stream_endpoint_t * endpoints){ + uint8_t command[2+2*MAX_NUM_SEPS]; + int pos = 0; + command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); + command[pos++] = (uint8_t)AVDTP_SI_DISCOVER; + + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + command[pos++] = (stream_endpoint->sep.seid << 2) | (stream_endpoint->sep.in_use<<1); + command[pos++] = (stream_endpoint->sep.media_type << 4) | (stream_endpoint->sep.type << 3); + } + return l2cap_send(cid, command, pos); +} + +static int avdtp_initiator_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ + uint8_t command[2]; + command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); + command[1] = (uint8_t)identifier; + return l2cap_send(cid, command, sizeof(command)); +} /* START: tracking can send now requests pro l2cap cid */ -#if 0 -static void avdtp_sink_run_for_stream_endpoints(void){ - - // btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); - // while (btstack_linked_list_iterator_has_next(&it)){ - // avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); - - // if (stream_endpoint->disconnect){ - // switch (stream_endpoint->state){ - // case AVDTP_STREAM_ENDPOINT_IDLE: - // case AVDTP_STREAM_ENDPOINT_CONFIGURED: - // case AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED: - // stream_endpoint->disconnect = 0; - // break; - // case AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM: - // stream_endpoint->disconnect = 0; - // stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; - // break; - // case AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED: - // break; - // default: - // stream_endpoint->disconnect = 0; - // stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED; - // l2cap_disconnect(stream_endpoint->l2cap_media_cid, 0); - // return; - // } - // } - // } -} -#endif - static void avdtp_sink_handle_can_send_now(avdtp_connection_t * connection, uint16_t l2cap_cid){ if (connection->wait_to_send_acceptor){ connection->wait_to_send_acceptor = 0; - avdtp_acceptor_stream_config_subsm_run(connection); - if (avdtp_acceptor_stream_config_subsm_is_configured(connection)){ - printf(" -> RUN: AVDTP_SIGNALING_CONNECTION_CONFIGURED\n"); - connection->state = AVDTP_SIGNALING_CONNECTION_CONFIGURED; - } - } else if (connection->wait_to_send_initiator){ - connection->wait_to_send_initiator = 0; - avdtp_initiator_stream_config_subsm_run(connection); - if (avdtp_initiator_stream_config_subsm_is_configured(connection)){ - printf(" -> RUN: AVDTP_SIGNALING_CONNECTION_CONFIGURED\n"); - connection->state = AVDTP_SIGNALING_CONNECTION_CONFIGURED; - } - } else if (connection->wait_to_send_self){ - printf(" -> RUN: wait_to_send_self for seid %d\n", connection->query_seid); - connection->wait_to_send_self = 0; - avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); if (!stream_endpoint) return; + if (avdtp_acceptor_stream_config_subsm_run(connection, stream_endpoint)) return; - switch (stream_endpoint->state){ - case AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM: - printf(" -> RUN: AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED\n"); - stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED; - avdtp_acceptor_send_accept_response(stream_endpoint->connection->l2cap_signaling_cid, AVDTP_SI_OPEN, stream_endpoint->connection->acceptor_transaction_label); - break; - case AVDTP_STREAM_ENDPOINT_W2_ANSWER_START_STREAM: - printf(" -> RUN: AVDTP_STREAM_ENDPOINT_W4_STREAMING_CONNECTION_OPEN \n"); - stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_STREAMING_CONNECTION_OPEN; - avdtp_acceptor_send_accept_response(stream_endpoint->connection->l2cap_signaling_cid, AVDTP_SI_START, stream_endpoint->connection->acceptor_transaction_label); - break; + } else if (connection->wait_to_send_initiator){ + connection->wait_to_send_initiator = 0; + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) return; + if (avdtp_initiator_stream_config_subsm_run(connection, stream_endpoint)) return; + + } else if (connection->wait_to_send_self){ + connection->wait_to_send_self = 0; + + switch (connection->acceptor_connection_state){ + case AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS: + printf(" -> AVDTP_SIGNALING_CONNECTION_OPENED\n"); + connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; + connection->acceptor_connection_state = AVDTP_SIGNALING_CONNECTION_ACCEPTOR_IDLE; + avdtp_send_seps_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, (avdtp_stream_endpoint_t *)&stream_endpoints); + return; default: - printf(" -> RUN: NOT IMPLEMENTED, state %d\n", stream_endpoint->state); break; } + switch (connection->initiator_connection_state){ + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED; + avdtp_initiator_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_SI_DISCOVER, connection->initiator_transaction_label); + return; + default: + break; + } + printf("wait_to_send_self not handled \n"); } // re-register @@ -434,101 +437,105 @@ static void avdtp_sink_request_can_send_now_self(avdtp_connection_t * connection } /* END: tracking can send now requests pro l2cap cid */ - -static void handle_l2cap_signaling_data_packet_for_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size){ - if (!connection) return; - if (size < 2) { - log_error("l2cap data packet too small"); - return; - } - avdtp_stream_endpoint_t * stream_endpoint = NULL; - +static int handle_l2cap_data_packet_for_stream_endpoint(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size){ avdtp_signaling_packet_header_t signaling_header; avdtp_read_signaling_header(&signaling_header, packet, size); - - switch (connection->state){ - case AVDTP_SIGNALING_CONNECTION_CONFIGURATION_SUBSTATEMACHINE: - if (signaling_header.message_type == AVDTP_CMD_MSG){ - if (avdtp_acceptor_stream_config_subsm(connection, packet, size)){ - avdtp_sink_request_can_send_now_acceptor(connection, connection->l2cap_signaling_cid); - } - } else { - if (avdtp_initiator_stream_config_subsm(connection, packet, size)){ - avdtp_sink_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); - } - } - break; - case AVDTP_SIGNALING_CONNECTION_CONFIGURED: - connection->query_seid = packet[2] >> 2; - stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); - if (!stream_endpoint) return; - printf(" -> SM: AVDTP_STREAM_ENDPOINT_CONFIGURED "); - - switch (stream_endpoint->state){ - case AVDTP_STREAM_ENDPOINT_CONFIGURED: - if (signaling_header.signal_identifier != AVDTP_SI_OPEN) break; - printf(" -> stream endpoint: AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM, seid %d \n", connection->query_seid ); - stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W2_ANSWER_OPEN_STREAM; - stream_endpoint->connection->acceptor_transaction_label = signaling_header.transaction_label; - avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); - break; - case AVDTP_STREAM_ENDPOINT_OPENED: - if (signaling_header.signal_identifier != AVDTP_SI_START) break; - printf(" -> stream endpoint: AVDTP_STREAM_ENDPOINT_W2_ANSWER_START_STREAM, seid %d \n", connection->query_seid ); - stream_endpoint->sep.in_use = 1; - stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W2_ANSWER_START_STREAM; - stream_endpoint->connection->acceptor_transaction_label = signaling_header.transaction_label; - avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); - break; - default: - printf(" -> stream endpoint: NOT IMPLEMENTED\n"); - break; - } - break; - default: - break; - } + + if (signaling_header.message_type == AVDTP_CMD_MSG){ + if (avdtp_acceptor_stream_config_subsm(stream_endpoint, &signaling_header, packet, size)){ + avdtp_sink_request_can_send_now_acceptor(connection, connection->l2cap_signaling_cid); + return 1; + } + } else { + if (avdtp_initiator_stream_config_subsm(connection, stream_endpoint, &signaling_header, packet, size)){ + avdtp_sink_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); + return 1; + } + } + return 0; } - -static void handle_l2cap_signaling_data_packet_for_stream_endpoint(avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size){ - log_error("handle_l2cap_signaling_data_packet_for_stream_endpoint called for endpoint %p", stream_endpoint); -#if 0 - if (!stream_endpoint) return; - if (!stream_endpoint->connection) return; - - int request_to_send = 0; +static int handle_l2cap_data_packet_for_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size){ + if (size < 2) { + log_error("l2cap data packet too small"); + return 0; + } + + avdtp_stream_endpoint_t * stream_endpoint = NULL; avdtp_signaling_packet_header_t signaling_header; avdtp_read_signaling_header(&signaling_header, packet, size); - printf(" handle_l2cap_signaling_data_packet_for_stream_endpoint %d \n", stream_endpoint->sep.seid); - - switch (stream_endpoint->state){ - - case AVDTP_STREAM_ENDPOINT_OPENED: + + switch (signaling_header.message_type){ + case AVDTP_CMD_MSG: + connection->acceptor_transaction_label = signaling_header.transaction_label; switch (signaling_header.signal_identifier){ + case AVDTP_SI_DISCOVER: + printf(" -> AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS\n"); + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return 0; + connection->acceptor_connection_state = AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); + return 1; + case AVDTP_SI_GET_CAPABILITIES: + case AVDTP_SI_SET_CONFIGURATION: + case AVDTP_SI_OPEN: case AVDTP_SI_START: - printf("AVDTP_OPEN -> AVDTP_W2_ANSWER_START_SINGLE_STREAM\n"); - if (stream_endpoint->sep.seid != packet[2] >> 2) return; - stream_endpoint->sep.in_use = 1; - stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W2_ANSWER_START_STREAM; - stream_endpoint->connection->acceptor_transaction_label = signaling_header.transaction_label; - request_to_send = 1; - break; + connection->query_seid = packet[2] >> 2; + stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); + // printf(" handle_l2cap_data_packet_for_connection 2\n"); + return handle_l2cap_data_packet_for_stream_endpoint(connection, stream_endpoint, packet, size); default: - printf("AVDTP_OPEN -> NOT IMPLEMENTED signal_identifier %d\n", signaling_header.signal_identifier); + printf("AVDTP_CMD_MSG signal %d not implemented\n", signaling_header.signal_identifier); break; } break; - default: - printf("handle_l2cap_signaling_data_packet: state %d -> NOT IMPLEMENTED signal_identifier %d\n", stream_endpoint->state, signaling_header.signal_identifier); - // printf_hexdump( packet, size ); + case AVDTP_RESPONSE_ACCEPT_MSG: + switch (signaling_header.signal_identifier){ + case AVDTP_SI_DISCOVER: + if (signaling_header.transaction_label != connection->initiator_transaction_label){ + printf("unexpected transaction label, got %d, expected %d\n", signaling_header.transaction_label, connection->initiator_transaction_label); + return 0; + } + + if (size == 3){ + printf("ERROR code %02x\n", packet[2]); + return 0; + } + + avdtp_sep_t sep; + int i; + for (i = 2; i> 2; + if (sep.seid < 0x01 || sep.seid > 0x3E){ + printf("invalid sep id\n"); + return 0; + } + sep.in_use = (packet[i] >> 1) & 0x01; + sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); + sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); + stream_endpoint->remote_seps[stream_endpoint->remote_seps_num++] = sep; + // printf("found sep: seid %u, in_use %d, media type %d, sep type %d (1-SNK)\n", + // sep.seid, sep.in_use, sep.media_type, sep.type); + } + connection->initiator_transaction_label++; + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); + break; + default: + printf("AVDTP_RESPONSE_ACCEPT_MSG signal %d not implemented\n", signaling_header.signal_identifier); + break; + } + break; + case AVDTP_RESPONSE_REJECT_MSG: + printf("AVDTP_RESPONSE_REJECT_MSG signal %d not implemented\n", signaling_header.signal_identifier); + break; + case AVDTP_GENERAL_REJECT_MSG: + printf("AVDTP_GENERAL_REJECT_MSG signal %d not implemented\n", signaling_header.signal_identifier); break; } - if (request_to_send){ - l2cap_request_can_send_now_event(stream_endpoint->connection->l2cap_signaling_cid); - return; - } -#endif + + printf(" handle_l2cap_data_packet_for_connection 3, signal %d\n", signaling_header.signal_identifier); + return 0; } static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ @@ -538,30 +545,32 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe uint16_t local_cid; avdtp_stream_endpoint_t * stream_endpoint = NULL; avdtp_connection_t * connection = NULL; - + switch (packet_type) { case L2CAP_DATA_PACKET: connection = get_avdtp_connection_for_l2cap_signaling_cid(channel); if (connection){ - if (connection->state <= AVDTP_SIGNALING_CONNECTION_CONFIGURED){ - handle_l2cap_signaling_data_packet_for_connection(connection, packet, size); - } - break; + if (handle_l2cap_data_packet_for_connection(connection, packet, size)) return; } - + stream_endpoint = get_avdtp_stream_endpoint_for_l2cap_cid(channel); if (!stream_endpoint){ printf("avdtp L2CAP_DATA_PACKET: no stream enpoint for local cid 0x%02x found", channel); break; } + if (channel == stream_endpoint->connection->l2cap_signaling_cid){ - handle_l2cap_signaling_data_packet_for_stream_endpoint(stream_endpoint, packet, size); + printf(" handle_l2cap_data_packet_for_connection 1\n"); + handle_l2cap_data_packet_for_stream_endpoint(stream_endpoint->connection, stream_endpoint, packet, size); break; } if (channel == stream_endpoint->l2cap_media_cid){ (*handle_media_data)(stream_endpoint, packet, size); - } else if (channel == stream_endpoint->l2cap_reporting_cid){ + break; + } + + if (channel == stream_endpoint->l2cap_reporting_cid){ // TODO printf("L2CAP_DATA_PACKET for reporting: NOT IMPLEMENTED\n"); } else if (channel == stream_endpoint->l2cap_recovery_cid){ @@ -581,18 +590,17 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe connection = get_avdtp_connection_for_bd_addr(event_addr); if (!connection){ connection = avdtp_sink_create_connection(event_addr); - } - - if (connection && connection->l2cap_signaling_cid == 0){ - if (connection->state != AVDTP_SIGNALING_CONNECTION_IDLE) break; connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; l2cap_accept_connection(local_cid); break; } stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); - if (!stream_endpoint) break; - + if (!stream_endpoint) { + printf("L2CAP_EVENT_INCOMING_CONNECTION no streamendpoint found for seid %d\n", connection->query_seid); + break; + } + if (stream_endpoint->l2cap_media_cid == 0){ if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED) break; l2cap_accept_connection(local_cid); @@ -618,8 +626,8 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe con_handle = l2cap_event_channel_opened_get_handle(packet); local_cid = l2cap_event_channel_opened_get_local_cid(packet); - printf("L2CAP_EVENT_CHANNEL_OPENED: Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x\n", - bd_addr_to_str(event_addr), con_handle, psm, local_cid, l2cap_event_channel_opened_get_remote_cid(packet)); + // printf("L2CAP_EVENT_CHANNEL_OPENED: Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x\n", + // bd_addr_to_str(event_addr), con_handle, psm, local_cid, l2cap_event_channel_opened_get_remote_cid(packet)); if (psm != PSM_AVDTP) break; @@ -631,11 +639,11 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe connection->l2cap_signaling_cid = local_cid; connection->con_handle = con_handle; connection->query_seid = 0; - connection->state = AVDTP_SIGNALING_CONNECTION_CONFIGURATION_SUBSTATEMACHINE; - avdtp_acceptor_stream_config_subsm_init(connection); - printf(" -> AVDTP_SIGNALING_CONNECTION_CONFIGURATION_SUBSTATEMACHINE\n"); + connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; + printf(" -> AVDTP_SIGNALING_CONNECTION_OPENED\n"); break; } + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); if (!stream_endpoint){ printf("L2CAP_EVENT_CHANNEL_OPENED: stream_endpoint not found"); @@ -656,7 +664,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe local_cid = l2cap_event_channel_closed_get_local_cid(packet); connection = get_avdtp_connection_for_l2cap_signaling_cid(local_cid); if (connection){ - log_info("L2CAP_EVENT_CHANNEL_CLOSED signaling cid 0x%0x", local_cid); + log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED signaling cid 0x%0x", local_cid); stream_endpoint = get_avdtp_stream_endpoint_for_connection(connection); stream_endpoint->connection = NULL; connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; @@ -668,14 +676,14 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe if (!stream_endpoint) return; if (stream_endpoint->l2cap_media_cid == local_cid){ - log_info("L2CAP_EVENT_CHANNEL_CLOSED media cid 0x%0x", local_cid); + log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED media cid 0x%0x", local_cid); stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; stream_endpoint->l2cap_media_cid = 0; break; } if (stream_endpoint->l2cap_recovery_cid == local_cid){ - log_info("L2CAP_EVENT_CHANNEL_CLOSED recovery cid 0x%0x", local_cid); + log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED recovery cid 0x%0x", local_cid); stream_endpoint->l2cap_recovery_cid = 0; break; } diff --git a/test/avdtp/avdtp_sink.h b/test/avdtp/avdtp_sink.h index 5506401c9..4860a82af 100644 --- a/test/avdtp/avdtp_sink.h +++ b/test/avdtp/avdtp_sink.h @@ -54,8 +54,6 @@ extern "C" { #endif -btstack_linked_list_t stream_endpoints; - /* API_START */ /** * @brief AVDTP Sink service record. diff --git a/test/avdtp/avdtp_sink_working.c b/test/avdtp/avdtp_sink_working.c new file mode 100644 index 000000000..717ee7b9f --- /dev/null +++ b/test/avdtp/avdtp_sink_working.c @@ -0,0 +1,741 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS + * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + + +#include +#include +#include +#include +#include + +#include "btstack.h" +#include "avdtp.h" +#include "avdtp_sink.h" +#include "avdtp_util.h" +#include "avdtp_initiator.h" +#include "avdtp_acceptor.h" + +static const char * default_avdtp_sink_service_name = "BTstack AVDTP Sink Service"; +static const char * default_avdtp_sink_service_provider_name = "BTstack AVDTP Sink Service Provider"; + +// TODO list of devices +static btstack_linked_list_t avdtp_connections; +static uint16_t stream_endpoints_id_counter; + +static btstack_linked_list_t stream_endpoints; + +static btstack_packet_handler_t avdtp_sink_callback; +static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); + +static void (*handle_media_data)(avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size); + +void a2dp_sink_create_sdp_record(uint8_t * service, uint32_t service_record_handle, uint16_t supported_features, const char * service_name, const char * service_provider_name){ + uint8_t* attribute; + de_create_sequence(service); + + // 0x0000 "Service Record Handle" + de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle); + de_add_number(service, DE_UINT, DE_SIZE_32, service_record_handle); + + // 0x0001 "Service Class ID List" + de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList); + attribute = de_push_sequence(service); + { + de_add_number(attribute, DE_UUID, DE_SIZE_16, AUDIO_SINK_GROUP); + } + de_pop_sequence(service, attribute); + + // 0x0004 "Protocol Descriptor List" + de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ProtocolDescriptorList); + attribute = de_push_sequence(service); + { + uint8_t* l2cpProtocol = de_push_sequence(attribute); + { + de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, SDP_L2CAPProtocol); + de_add_number(l2cpProtocol, DE_UINT, DE_SIZE_16, PSM_AVDTP); + } + de_pop_sequence(attribute, l2cpProtocol); + + uint8_t* avProtocol = de_push_sequence(attribute); + { + de_add_number(avProtocol, DE_UUID, DE_SIZE_16, PSM_AVDTP); // avProtocol_service + de_add_number(avProtocol, DE_UINT, DE_SIZE_16, 0x0103); // version + } + de_pop_sequence(attribute, avProtocol); + } + de_pop_sequence(service, attribute); + + // 0x0005 "Public Browse Group" + de_add_number(service, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList); // public browse group + attribute = de_push_sequence(service); + { + de_add_number(attribute, DE_UUID, DE_SIZE_16, SDP_PublicBrowseGroup); + } + de_pop_sequence(service, attribute); + + // 0x0009 "Bluetooth Profile Descriptor List" + de_add_number(service, DE_UINT, DE_SIZE_16, SDP_BluetoothProfileDescriptorList); + attribute = de_push_sequence(service); + { + uint8_t *a2dProfile = de_push_sequence(attribute); + { + de_add_number(a2dProfile, DE_UUID, DE_SIZE_16, ADVANCED_AUDIO_DISTRIBUTION); + de_add_number(a2dProfile, DE_UINT, DE_SIZE_16, 0x0103); + } + de_pop_sequence(attribute, a2dProfile); + } + de_pop_sequence(service, attribute); + + + // 0x0100 "Service Name" + de_add_number(service, DE_UINT, DE_SIZE_16, 0x0100); + if (service_name){ + de_add_data(service, DE_STRING, strlen(service_name), (uint8_t *) service_name); + } else { + de_add_data(service, DE_STRING, strlen(default_avdtp_sink_service_name), (uint8_t *) default_avdtp_sink_service_name); + } + + // 0x0100 "Provider Name" + de_add_number(service, DE_UINT, DE_SIZE_16, 0x0102); + if (service_provider_name){ + de_add_data(service, DE_STRING, strlen(service_provider_name), (uint8_t *) service_provider_name); + } else { + de_add_data(service, DE_STRING, strlen(default_avdtp_sink_service_provider_name), (uint8_t *) default_avdtp_sink_service_provider_name); + } + + // 0x0311 "Supported Features" + de_add_number(service, DE_UINT, DE_SIZE_16, 0x0311); + de_add_number(service, DE_UINT, DE_SIZE_16, supported_features); +} + +static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_seid(uint16_t seid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + if (stream_endpoint->sep.seid == seid){ + return stream_endpoint; + } + } + return NULL; +} + + + +static avdtp_connection_t * avdtp_sink_create_connection(bd_addr_t remote_addr){ + avdtp_connection_t * connection = btstack_memory_avdtp_connection_get(); + memset(connection, 0, sizeof(avdtp_connection_t)); + connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; + connection->initiator_transaction_label++; + memcpy(connection->remote_addr, remote_addr, 6); + btstack_linked_list_add(&avdtp_connections, (btstack_linked_item_t *) connection); + return connection; +} + +uint8_t avdtp_sink_create_stream_endpoint(avdtp_sep_type_t sep_type, avdtp_media_type_t media_type){ + avdtp_stream_endpoint_t * stream_endpoint = btstack_memory_avdtp_stream_endpoint_get(); + memset(stream_endpoint, 0, sizeof(avdtp_stream_endpoint_t)); + stream_endpoints_id_counter++; + stream_endpoint->sep.seid = stream_endpoints_id_counter; + stream_endpoint->sep.media_type = media_type; + stream_endpoint->sep.type = sep_type; + btstack_linked_list_add(&stream_endpoints, (btstack_linked_item_t *) stream_endpoint); + return stream_endpoint->sep.seid; +} + +void avdtp_sink_register_media_transport_category(uint8_t seid){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_TRANSPORT, 1); + stream_endpoint->sep.registered_service_categories = bitmap; + printf("registered services AVDTP_MEDIA_TRANSPORT(%d) %02x\n", AVDTP_MEDIA_TRANSPORT, stream_endpoint->sep.registered_service_categories); +} + +void avdtp_sink_register_reporting_category(uint8_t seid){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_REPORTING, 1); + stream_endpoint->sep.registered_service_categories = bitmap; +} + +void avdtp_sink_register_delay_reporting_category(uint8_t seid){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_DELAY_REPORTING, 1); + stream_endpoint->sep.registered_service_categories = bitmap; +} + +void avdtp_sink_register_recovery_category(uint8_t seid, uint8_t maximum_recovery_window_size, uint8_t maximum_number_media_packets){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_RECOVERY, 1); + stream_endpoint->sep.registered_service_categories = bitmap; + stream_endpoint->sep.capabilities.recovery.recovery_type = 0x01; // 0x01 = RFC2733 + stream_endpoint->sep.capabilities.recovery.maximum_recovery_window_size = maximum_recovery_window_size; + stream_endpoint->sep.capabilities.recovery.maximum_number_media_packets = maximum_number_media_packets; +} + +void avdtp_sink_register_content_protection_category(uint8_t seid, uint8_t cp_type_lsb, uint8_t cp_type_msb, const uint8_t * cp_type_value, uint8_t cp_type_value_len){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_CONTENT_PROTECTION, 1); + stream_endpoint->sep.registered_service_categories = bitmap; + stream_endpoint->sep.capabilities.content_protection.cp_type_lsb = cp_type_lsb; + stream_endpoint->sep.capabilities.content_protection.cp_type_msb = cp_type_msb; + stream_endpoint->sep.capabilities.content_protection.cp_type_value = cp_type_value; + stream_endpoint->sep.capabilities.content_protection.cp_type_value_len = cp_type_value_len; +} + +void avdtp_sink_register_header_compression_category(uint8_t seid, uint8_t back_ch, uint8_t media, uint8_t recovery){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_HEADER_COMPRESSION, 1); + stream_endpoint->sep.registered_service_categories = bitmap; + stream_endpoint->sep.capabilities.header_compression.back_ch = back_ch; + stream_endpoint->sep.capabilities.header_compression.media = media; + stream_endpoint->sep.capabilities.header_compression.recovery = recovery; +} + +void avdtp_sink_register_media_codec_category(uint8_t seid, avdtp_media_type_t media_type, avdtp_media_codec_type_t media_codec_type, const uint8_t * media_codec_info, uint16_t media_codec_info_len){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_CODEC, 1); + stream_endpoint->sep.registered_service_categories = bitmap; + printf("registered services AVDTP_MEDIA_CODEC(%d) %02x\n", AVDTP_MEDIA_CODEC, stream_endpoint->sep.registered_service_categories); + stream_endpoint->sep.capabilities.media_codec.media_type = media_type; + stream_endpoint->sep.capabilities.media_codec.media_codec_type = media_codec_type; + stream_endpoint->sep.capabilities.media_codec.media_codec_information = media_codec_info; + stream_endpoint->sep.capabilities.media_codec.media_codec_information_len = media_codec_info_len; +} + +void avdtp_sink_register_multiplexing_category(uint8_t seid, uint8_t fragmentation){ + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint){ + log_error("avdtp_sink_register_media_transport_category: stream endpoint with seid %d is not registered", seid); + return; + } + uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MULTIPLEXING, 1); + stream_endpoint->sep.registered_service_categories = bitmap; + stream_endpoint->sep.capabilities.multiplexing_mode.fragmentation = fragmentation; +} + +// // media, reporting. recovery +// void avdtp_sink_register_media_transport_identifier_for_multiplexing_category(uint8_t seid, uint8_t fragmentation){ + +// } + + +static avdtp_connection_t * get_avdtp_connection_for_bd_addr(bd_addr_t addr){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &avdtp_connections); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_connection_t * connection = (avdtp_connection_t *)btstack_linked_list_iterator_next(&it); + if (memcmp(addr, connection->remote_addr, 6) != 0) continue; + return connection; + } + return NULL; +} + +static avdtp_connection_t * get_avdtp_connection_for_con_handle(hci_con_handle_t con_handle){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &avdtp_connections); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_connection_t * connection = (avdtp_connection_t *)btstack_linked_list_iterator_next(&it); + if (connection->con_handle != con_handle) continue; + return connection; + } + return NULL; +} + +static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_active_seid(uint8_t seid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + if (stream_endpoint->sep.seid == seid){ + return stream_endpoint; + } + } + return NULL; +} + +static avdtp_connection_t * get_avdtp_connection_for_l2cap_signaling_cid(uint16_t l2cap_cid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &avdtp_connections); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_connection_t * connection = (avdtp_connection_t *)btstack_linked_list_iterator_next(&it); + if (connection->l2cap_signaling_cid != l2cap_cid) continue; + return connection; + } + return NULL; +} + +static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_l2cap_cid(uint16_t l2cap_cid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + if (stream_endpoint->l2cap_media_cid == l2cap_cid){ + return stream_endpoint; + } + if (stream_endpoint->l2cap_reporting_cid == l2cap_cid){ + return stream_endpoint; + } + if (stream_endpoint->l2cap_recovery_cid == l2cap_cid){ + return stream_endpoint; + } + } + return NULL; +} + +static avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_connection(avdtp_connection_t * connection){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + if (stream_endpoint->connection != connection) continue; + return stream_endpoint; + } + return NULL; +} + +static int avdtp_send_seps_response(uint16_t cid, uint8_t transaction_label, avdtp_stream_endpoint_t * endpoints){ + uint8_t command[2+2*MAX_NUM_SEPS]; + int pos = 0; + command[pos++] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_RESPONSE_ACCEPT_MSG); + command[pos++] = (uint8_t)AVDTP_SI_DISCOVER; + + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + command[pos++] = (stream_endpoint->sep.seid << 2) | (stream_endpoint->sep.in_use<<1); + command[pos++] = (stream_endpoint->sep.media_type << 4) | (stream_endpoint->sep.type << 3); + } + return l2cap_send(cid, command, pos); +} + + +static int avdtp_initiator_send_signaling_cmd(uint16_t cid, avdtp_signal_identifier_t identifier, uint8_t transaction_label){ + uint8_t command[2]; + command[0] = avdtp_header(transaction_label, AVDTP_SINGLE_PACKET, AVDTP_CMD_MSG); + command[1] = (uint8_t)identifier; + return l2cap_send(cid, command, sizeof(command)); +} + +/* START: tracking can send now requests pro l2cap cid */ +static void avdtp_sink_handle_can_send_now(avdtp_connection_t * connection, uint16_t l2cap_cid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + + if (connection->wait_to_send_acceptor){ + connection->wait_to_send_acceptor = 0; + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) return; + if (avdtp_acceptor_stream_config_subsm_run(connection, stream_endpoint)) return; + + } else if (connection->wait_to_send_initiator){ + connection->wait_to_send_initiator = 0; + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) return; + if (avdtp_initiator_stream_config_subsm_run(connection, stream_endpoint)) return; + + } else if (connection->wait_to_send_self){ + connection->wait_to_send_self = 0; + switch (connection->state){ + case AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS: + printf(" -> AVDTP_SIGNALING_CONNECTION_OPENED\n"); + connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; + avdtp_send_seps_response(connection->l2cap_signaling_cid, connection->acceptor_transaction_label, (avdtp_stream_endpoint_t *)&stream_endpoints); + return; + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED\n"); + connection->state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SEPS_DISCOVERED; + avdtp_initiator_send_signaling_cmd(connection->l2cap_signaling_cid, AVDTP_SI_DISCOVER, connection->initiator_transaction_label); + return; + default: + break; + } + } + + // re-register + int more_to_send = connection->wait_to_send_acceptor || connection->wait_to_send_initiator || connection->wait_to_send_self; + if (more_to_send){ + l2cap_request_can_send_now_event(l2cap_cid); + } +} + +static void avdtp_sink_request_can_send_now_acceptor(avdtp_connection_t * connection, uint16_t l2cap_cid){ + connection->wait_to_send_acceptor = 1; + l2cap_request_can_send_now_event(l2cap_cid); +} +static void avdtp_sink_request_can_send_now_initiator(avdtp_connection_t * connection, uint16_t l2cap_cid){ + connection->wait_to_send_initiator = 1; + l2cap_request_can_send_now_event(l2cap_cid); +} +static void avdtp_sink_request_can_send_now_self(avdtp_connection_t * connection, uint16_t l2cap_cid){ + connection->wait_to_send_self = 1; + l2cap_request_can_send_now_event(l2cap_cid); +} + +/* END: tracking can send now requests pro l2cap cid */ +static void handle_l2cap_data_packet_for_stream_endpoint(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size){ + avdtp_signaling_packet_header_t signaling_header; + avdtp_read_signaling_header(&signaling_header, packet, size); + + if (signaling_header.message_type == AVDTP_CMD_MSG){ + if (avdtp_acceptor_stream_config_subsm(stream_endpoint, &signaling_header, packet, size)){ + avdtp_sink_request_can_send_now_acceptor(connection, connection->l2cap_signaling_cid); + } + } else { + if (avdtp_initiator_stream_config_subsm(connection, stream_endpoint, &signaling_header, packet, size)){ + avdtp_sink_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); + } + } +} + +static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ + bd_addr_t event_addr; + hci_con_handle_t con_handle; + uint16_t psm; + uint16_t local_cid; + avdtp_stream_endpoint_t * stream_endpoint = NULL; + avdtp_connection_t * connection = NULL; + + switch (packet_type) { + case L2CAP_DATA_PACKET: + connection = get_avdtp_connection_for_l2cap_signaling_cid(channel); + if (connection){ + avdtp_signaling_packet_header_t signaling_header; + avdtp_read_signaling_header(&signaling_header, packet, size); + connection->acceptor_transaction_label = signaling_header.transaction_label; + + switch (signaling_header.signal_identifier){ + case AVDTP_SI_DISCOVER: + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; + if (signaling_header.message_type == AVDTP_CMD_MSG){ + printf(" -> AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS\n"); + connection->state = AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS; + } else { + if (signaling_header.transaction_label != connection->initiator_transaction_label){ + printf("unexpected transaction label, got %d, expected %d\n", signaling_header.transaction_label, connection->initiator_transaction_label); + return; + } + + if (signaling_header.message_type != AVDTP_RESPONSE_ACCEPT_MSG){ + printf("request rejected...\n"); + return; + } + if (size == 3){ + printf("ERROR code %02x\n", packet[2]); + return; + } + + avdtp_sep_t sep; + int i; + for (i = 2; i> 2; + if (sep.seid < 0x01 || sep.seid > 0x3E){ + printf("invalid sep id\n"); + return; + } + sep.in_use = (packet[i] >> 1) & 0x01; + sep.media_type = (avdtp_media_type_t)(packet[i+1] >> 4); + sep.type = (avdtp_sep_type_t)((packet[i+1] >> 3) & 0x01); + stream_endpoint->remote_seps[stream_endpoint->remote_seps_num++] = sep; + // printf("found sep: seid %u, in_use %d, media type %d, sep type %d (1-SNK)\n", + // sep.seid, sep.in_use, sep.media_type, sep.type); + } + connection->initiator_transaction_label++; + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS\n"); + connection->state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS; + } + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); + return; + + case AVDTP_SI_GET_CAPABILITIES: + case AVDTP_SI_SET_CONFIGURATION: + case AVDTP_SI_OPEN: + case AVDTP_SI_START: + connection->query_seid = packet[2] >> 2; + stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); + handle_l2cap_data_packet_for_stream_endpoint(connection, stream_endpoint, packet, size); + break; + default: + break; + } + printf(" handle_l2cap_data_packet_for_connection 3, signal %d\n", signaling_header.signal_identifier); + } + + stream_endpoint = get_avdtp_stream_endpoint_for_l2cap_cid(channel); + if (!stream_endpoint){ + printf("avdtp L2CAP_DATA_PACKET: no stream enpoint for local cid 0x%02x found", channel); + break; + } + + if (channel == stream_endpoint->connection->l2cap_signaling_cid){ + handle_l2cap_data_packet_for_stream_endpoint(stream_endpoint->connection, stream_endpoint, packet, size); + break; + } + + if (channel == stream_endpoint->l2cap_media_cid){ + (*handle_media_data)(stream_endpoint, packet, size); + break; + } + + if (channel == stream_endpoint->l2cap_reporting_cid){ + // TODO + printf("L2CAP_DATA_PACKET for reporting: NOT IMPLEMENTED\n"); + } else if (channel == stream_endpoint->l2cap_recovery_cid){ + // TODO + printf("L2CAP_DATA_PACKET for recovery: NOT IMPLEMENTED\n"); + } else { + log_error("avdtp packet handler L2CAP_DATA_PACKET: local cid 0x%02x not found", channel); + } + break; + + case HCI_EVENT_PACKET: + switch (hci_event_packet_get_type(packet)) { + case L2CAP_EVENT_INCOMING_CONNECTION: + l2cap_event_incoming_connection_get_address(packet, event_addr); + local_cid = l2cap_event_incoming_connection_get_local_cid(packet); + + connection = get_avdtp_connection_for_bd_addr(event_addr); + if (!connection){ + connection = avdtp_sink_create_connection(event_addr); + connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; + l2cap_accept_connection(local_cid); + break; + } + + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + printf("L2CAP_EVENT_INCOMING_CONNECTION no streamendpoint found for seid %d\n", connection->query_seid); + break; + } + + if (stream_endpoint->l2cap_media_cid == 0){ + if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED) break; + l2cap_accept_connection(local_cid); + break; + } + break; + + case L2CAP_EVENT_CHANNEL_OPENED: + // inform about new l2cap connection + l2cap_event_channel_opened_get_address(packet, event_addr); + + if (l2cap_event_channel_opened_get_status(packet)){ + log_error("L2CAP connection to connection %s failed. status code 0x%02x", + bd_addr_to_str(event_addr), l2cap_event_channel_opened_get_status(packet)); + break; + } + psm = l2cap_event_channel_opened_get_psm(packet); + if (psm != PSM_AVDTP){ + log_error("unexpected PSM - Not implemented yet, avdtp sink: L2CAP_EVENT_CHANNEL_OPENED"); + return; + } + + con_handle = l2cap_event_channel_opened_get_handle(packet); + local_cid = l2cap_event_channel_opened_get_local_cid(packet); + + // printf("L2CAP_EVENT_CHANNEL_OPENED: Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x\n", + // bd_addr_to_str(event_addr), con_handle, psm, local_cid, l2cap_event_channel_opened_get_remote_cid(packet)); + + if (psm != PSM_AVDTP) break; + + connection = get_avdtp_connection_for_bd_addr(event_addr); + if (!connection) break; + + if (connection->l2cap_signaling_cid == 0) { + if (connection->state != AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED) break; + connection->l2cap_signaling_cid = local_cid; + connection->con_handle = con_handle; + connection->query_seid = 0; + connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; + printf(" -> AVDTP_SIGNALING_CONNECTION_OPENED\n"); + break; + } + + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint){ + printf("L2CAP_EVENT_CHANNEL_OPENED: stream_endpoint not found"); + return; + } + + if (stream_endpoint->l2cap_media_cid == 0){ + if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED) return; + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; + stream_endpoint->l2cap_media_cid = local_cid; + printf(" -> AVDTP_STREAM_ENDPOINT_OPENED\n"); + break; + } + break; + + case L2CAP_EVENT_CHANNEL_CLOSED: + // data: event (8), len(8), channel (16) + local_cid = l2cap_event_channel_closed_get_local_cid(packet); + connection = get_avdtp_connection_for_l2cap_signaling_cid(local_cid); + if (connection){ + log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED signaling cid 0x%0x", local_cid); + stream_endpoint = get_avdtp_stream_endpoint_for_connection(connection); + stream_endpoint->connection = NULL; + connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; + btstack_linked_list_remove(&avdtp_connections, (btstack_linked_item_t*) connection); + break; + } + + stream_endpoint = get_avdtp_stream_endpoint_for_l2cap_cid(local_cid); + if (!stream_endpoint) return; + + if (stream_endpoint->l2cap_media_cid == local_cid){ + log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED media cid 0x%0x", local_cid); + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; + stream_endpoint->l2cap_media_cid = 0; + break; + } + + if (stream_endpoint->l2cap_recovery_cid == local_cid){ + log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED recovery cid 0x%0x", local_cid); + stream_endpoint->l2cap_recovery_cid = 0; + break; + } + + if (stream_endpoint->l2cap_reporting_cid == local_cid){ + log_info("L2CAP_EVENT_CHANNEL_CLOSED reporting cid 0x%0x", local_cid); + stream_endpoint->l2cap_reporting_cid = 0; + break; + } + + break; + + case HCI_EVENT_DISCONNECTION_COMPLETE: + break; + + case L2CAP_EVENT_CAN_SEND_NOW: + connection = get_avdtp_connection_for_l2cap_signaling_cid(channel); + if (!connection) { + stream_endpoint = get_avdtp_stream_endpoint_for_l2cap_cid(channel); + if (!stream_endpoint->connection) break; + connection = stream_endpoint->connection; + } + avdtp_sink_handle_can_send_now(connection, channel); + break; + default: + printf("unknown HCI event type %02x\n", hci_event_packet_get_type(packet)); + break; + } + break; + + default: + // other packet type + break; + } +} + +// TODO: find out which security level is needed, and replace LEVEL_0 in avdtp_sink_init +void avdtp_sink_init(void){ + stream_endpoints = NULL; + avdtp_connections = NULL; + stream_endpoints_id_counter = 0; + l2cap_register_service(&packet_handler, PSM_AVDTP, 0xffff, LEVEL_0); +} + +void avdtp_sink_register_media_handler(void (*callback)(avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size)){ + if (callback == NULL){ + log_error("avdtp_sink_register_media_handler called with NULL callback"); + return; + } + handle_media_data = callback; +} + +void avdtp_sink_register_packet_handler(btstack_packet_handler_t callback){ + if (callback == NULL){ + log_error("avdtp_sink_register_packet_handler called with NULL callback"); + return; + } + avdtp_sink_callback = callback; +} + +void avdtp_sink_connect(bd_addr_t bd_addr){ + avdtp_connection_t * connection = get_avdtp_connection_for_bd_addr(bd_addr); + if (!connection){ + connection = avdtp_sink_create_connection(bd_addr); + } + if (connection->state != AVDTP_SIGNALING_CONNECTION_IDLE) return; + connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; + l2cap_create_channel(packet_handler, connection->remote_addr, PSM_AVDTP, 0xffff, NULL); +} + +void avdtp_sink_disconnect(uint16_t con_handle){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (connection->state == AVDTP_SIGNALING_CONNECTION_IDLE) return; + if (connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED) return; + + connection->disconnect = 1; + + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); + while (btstack_linked_list_iterator_has_next(&it)){ + avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); + stream_endpoint->disconnect = stream_endpoint->state != AVDTP_STREAM_ENDPOINT_IDLE; + } + //avdtp_sink_run(connection); +} + +