diff --git a/test/avdtp/avdtp.h b/test/avdtp/avdtp.h index ff3c6e38a..5f0b1ba88 100644 --- a/test/avdtp/avdtp.h +++ b/test/avdtp/avdtp.h @@ -297,7 +297,11 @@ typedef enum { AVDTP_INITIATOR_STREAM_CONFIG_IDLE, AVDTP_INITIATOR_W2_GET_CONFIGURATION, AVDTP_INITIATOR_W4_CONFIGURATION_RECEIVED, - AVDTP_INITIATOR_STREAM_CONFIGURED + AVDTP_INITIATOR_STREAM_CONFIGURED, + AVDTP_INITIATOR_W2_SUSPEND_STREAM, + AVDTP_INITIATOR_W2_REJECT_WITH_ERROR_CODE, + AVDTP_INITIATOR_W2_REJECT_CATEGORY_WITH_ERROR_CODE, + AVDTP_INITIATOR_W2_REJECT_UNKNOWN_CMD } avdtp_initiator_stream_endpoint_state_t; typedef enum { @@ -356,8 +360,25 @@ typedef enum { AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_CAPABILITIES, AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES, AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ALL_CAPABILITIES, + // TODO move to initiator code AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_SET_CAPABILITIES, AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SET_CAPABILITIES, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_GET_CONFIGURATION, + + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_SUSPEND_STREAM_WITH_SEID, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SUSPEND_STREAM_WITH_SEID, + + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_RECONFIGURE_STREAM_WITH_SEID, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_MEDIA_CONNECTED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_MEDIA_CONNECTED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STARTED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_STARTED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STOPED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_STOPED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_ABORTED, + AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_ABORTED, AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_REJECT_WITH_ERROR_CODE, AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_REJECT_CATEGORY_WITH_ERROR_CODE, @@ -435,8 +456,9 @@ typedef struct avdtp_stream_endpoint { // currently active remote seid uint8_t remote_sep_index; - // register request for L2cap connection release - uint8_t disconnect; + // register request for media L2cap connection release + uint8_t media_disconnect; + uint8_t media_connect; } avdtp_stream_endpoint_t; #if defined __cplusplus diff --git a/test/avdtp/avdtp_initiator.c b/test/avdtp/avdtp_initiator.c index 5051b6971..a5989cf66 100644 --- a/test/avdtp/avdtp_initiator.c +++ b/test/avdtp/avdtp_initiator.c @@ -81,6 +81,27 @@ int avdtp_initiator_stream_config_subsm(avdtp_connection_t * connection, avdtp_s int avdtp_initiator_stream_config_subsm_run(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint){ int sent = 1; + + // if (stream_endpoint->media_connect){ + // switch (stream_endpoint->initiator_config_state){ + // case AVDTP_INITIATOR_STREAM_CONFIG_IDLE: + // stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_GET_CAPABILITIES + // // 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; + // } + // if (sent) return 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"); diff --git a/test/avdtp/avdtp_sink.c b/test/avdtp/avdtp_sink.c index bc312326e..e4b7a4ed4 100644 --- a/test/avdtp/avdtp_sink.c +++ b/test/avdtp/avdtp_sink.c @@ -503,18 +503,32 @@ static void avdtp_sink_handle_can_send_now(avdtp_connection_t * connection, uint 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; + if (connection->disconnect){ + 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){ + if (stream_endpoint->state >= AVDTP_STREAM_ENDPOINT_OPENED && stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED){ + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); + l2cap_disconnect(stream_endpoint->l2cap_media_cid, 0); + return; + } + } + } + connection->disconnect = 0; + connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED; + l2cap_disconnect(connection->l2cap_signaling_cid, 0); + return; + } - // if (connection->confirm_suspend){ - // printf(" -> STREAMS SUSPENDED\n"); - // connection->confirm_suspend = 0; - // avdtp_acceptor_send_accept_response(connection->l2cap_signaling_cid, AVDTP_SI_SUSPEND, connection->acceptor_transaction_label); - // return; - // } switch (connection->acceptor_connection_state){ case AVDTP_SIGNALING_CONNECTION_ACCEPTOR_W2_ANSWER_DISCOVER_SEPS: @@ -538,6 +552,26 @@ static void avdtp_sink_handle_can_send_now(avdtp_connection_t * connection, uint break; } switch (connection->initiator_connection_state){ + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STOPED: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_STOPED\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_STOPED; + avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_CLOSE, connection->initiator_transaction_label, connection->query_seid); + return; + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_ABORTED: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_ABORTED\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_ABORTED; + avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_ABORT, connection->initiator_transaction_label, connection->query_seid); + return; + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STARTED: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_STARTED\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_STREAMING_STARTED; + avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_START, connection->initiator_transaction_label, connection->query_seid); + return; + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_MEDIA_CONNECTED: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_MEDIA_CONNECTED\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_L2CAP_FOR_MEDIA_CONNECTED; + avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_OPEN, connection->initiator_transaction_label, connection->query_seid); + return; 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; @@ -549,16 +583,22 @@ static void avdtp_sink_handle_can_send_now(avdtp_connection_t * connection, uint avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_CAPABILITIES, connection->initiator_transaction_label, connection->query_seid); return; case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES: - printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES\n"); + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_GET_ALL_CAPABILITIES\n"); connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_ALL_CAPABILITIES; avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_ALL_CAPABILITIES, connection->initiator_transaction_label, connection->query_seid); return; + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_GET_CONFIGURATION\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_GET_CONFIGURATION; + avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_GET_CONFIGURATION, connection->initiator_transaction_label, connection->query_seid); + return; case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_SET_CAPABILITIES:{ printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_SET_CAPABILITIES bitmap %02x\n", connection->remote_capabilities_bitmap); - printf_hexdump( connection->remote_capabilities.media_codec.media_codec_information, connection->remote_capabilities.media_codec.media_codec_information_len); + // printf_hexdump( connection->remote_capabilities.media_codec.media_codec_information, connection->remote_capabilities.media_codec.media_codec_information_len); connection->signaling_packet.acp_seid = connection->query_seid; connection->signaling_packet.int_seid = connection->int_seid; - + connection->signaling_packet.signal_identifier = AVDTP_SI_SET_CONFIGURATION; + avdtp_prepare_capabilities(&connection->signaling_packet, connection->initiator_transaction_label, connection->remote_capabilities_bitmap, connection->remote_capabilities, AVDTP_SI_SET_CONFIGURATION); l2cap_reserve_packet_buffer(); uint8_t * out_buffer = l2cap_get_outgoing_buffer(); @@ -568,8 +608,32 @@ static void avdtp_sink_handle_can_send_now(avdtp_connection_t * connection, uint printf(" ACP: fragmented\n"); } l2cap_send_prepared(connection->l2cap_signaling_cid, pos); - break; + return; } + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_SUSPEND_STREAM_WITH_SEID: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SUSPEND_STREAM_WITH_SEID\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_SUSPEND_STREAM_WITH_SEID; + avdtp_initiator_send_signaling_cmd_with_seid(connection->l2cap_signaling_cid, AVDTP_SI_SUSPEND, connection->initiator_transaction_label, connection->query_seid); + return; + case AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID: + printf(" -> AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_RECONFIGURE_STREAM_WITH_SEID\n"); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_RECONFIGURE_STREAM_WITH_SEID; + + connection->signaling_packet.acp_seid = connection->query_seid; + connection->signaling_packet.int_seid = 0; + connection->signaling_packet.signal_identifier = AVDTP_SI_RECONFIGURE; + + avdtp_prepare_capabilities(&connection->signaling_packet, connection->initiator_transaction_label, connection->remote_capabilities_bitmap, connection->remote_capabilities, AVDTP_SI_RECONFIGURE); + l2cap_reserve_packet_buffer(); + uint8_t * out_buffer = l2cap_get_outgoing_buffer(); + uint16_t pos = avdtp_signaling_create_fragment(connection->l2cap_signaling_cid, &connection->signaling_packet, out_buffer); + if (connection->signaling_packet.packet_type != AVDTP_SINGLE_PACKET && connection->signaling_packet.packet_type != AVDTP_END_PACKET){ + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W4_RECONFIGURE_STREAM_WITH_SEID; + printf(" ACP: fragmented\n"); + } + l2cap_send_prepared(connection->l2cap_signaling_cid, pos); + + return; default: break; } @@ -605,7 +669,8 @@ static int handle_l2cap_data_packet_for_stream_endpoint(avdtp_connection_t * con static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size){ avdtp_stream_endpoint_t * stream_endpoint = NULL; - avdtp_read_signaling_header(&connection->signaling_packet, packet, size); + int pos = avdtp_read_signaling_header(&connection->signaling_packet, packet, size); + switch (connection->signaling_packet.message_type){ case AVDTP_CMD_MSG: if (size < 2) { @@ -636,7 +701,7 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); return 1; } - connection->query_seid = packet[2] >> 2; + connection->query_seid = packet[pos++] >> 2; stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); if (!stream_endpoint){ printf(" ACP: cmd %d - RESPONSE REJECT BAD_ACP_SEID\n", connection->signaling_packet.signal_identifier); @@ -655,7 +720,7 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); return 1; } - connection->query_seid = packet[2] >> 2; + connection->query_seid = packet[pos++] >> 2; stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); if (!stream_endpoint){ printf(" ACP: AVDTP_SI_RECONFIGURE - CATEGORY RESPONSE REJECT BAD_ACP_SEID\n"); @@ -676,7 +741,7 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); return 1; } - connection->query_seid = packet[2] >> 2; + connection->query_seid = packet[pos++] >> 2; stream_endpoint = get_avdtp_stream_endpoint_for_active_seid(connection->query_seid); if (!stream_endpoint){ printf(" ACP: AVDTP_SI_OPEN - RESPONSE REJECT BAD_STATE\n"); @@ -694,7 +759,7 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t printf(" ACP: AVDTP_SI_SUSPEND seids: "); connection->num_suspended_seids = 0; - for (i=2; isuspended_seids[connection->num_suspended_seids] = packet[i] >> 2; printf("%d, \n", connection->suspended_seids[connection->num_suspended_seids]); connection->num_suspended_seids++; @@ -736,26 +801,26 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t } break; case AVDTP_RESPONSE_ACCEPT_MSG: - printf("handle_l2cap_data_packet_for_signaling_connection AVDTP_RESPONSE_ACCEPT_MSG, identifier %d\n", connection->signaling_packet.signal_identifier); + printf(" AVDTP_RESPONSE_ACCEPT_MSG, identifier %d\n", connection->signaling_packet.signal_identifier); switch (connection->signaling_packet.signal_identifier){ case AVDTP_SI_DISCOVER:{ if (connection->signaling_packet.transaction_label != connection->initiator_transaction_label){ - printf("unexpected transaction label, got %d, expected %d\n", connection->signaling_packet.transaction_label, connection->initiator_transaction_label); + printf(" unexpected transaction label, got %d, expected %d\n", connection->signaling_packet.transaction_label, connection->initiator_transaction_label); return 0; } if (size == 3){ - printf("ERROR code %02x\n", packet[2]); + printf(" ERROR code %02x\n", packet[pos]); 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"); + printf(" invalid sep id\n"); return 0; } sep.in_use = (packet[i] >> 1) & 0x01; @@ -768,10 +833,14 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; return 0; } + case AVDTP_SI_GET_CAPABILITIES: - case AVDTP_SI_GET_ALL_CAPABILITIES:{ + case AVDTP_SI_GET_ALL_CAPABILITIES: + case AVDTP_SI_GET_CONFIGURATION:{ avdtp_sep_t sep; - sep.registered_service_categories = avdtp_unpack_service_capabilities(connection, &sep.capabilities, packet+2, size-2); + sep.registered_service_categories = avdtp_unpack_service_capabilities(connection, &sep.capabilities, packet+pos, size-pos); + printf_hexdump(packet, size); + if (get_bit16(sep.registered_service_categories, AVDTP_MEDIA_CODEC)){ switch (sep.capabilities.media_codec.media_codec_type){ case AVDTP_CODEC_SBC: @@ -787,23 +856,92 @@ static int handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; return 0; } + case AVDTP_SI_SUSPEND: + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + } + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; case AVDTP_SI_SET_CONFIGURATION: connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + } + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CONFIGURED; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + case AVDTP_SI_START: + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + } + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_STREAMING; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + break; + case AVDTP_SI_OPEN: + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + } + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + + case AVDTP_SI_CLOSE: + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + } + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + + case AVDTP_SI_ABORT: + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + stream_endpoint = get_avdtp_stream_endpoint_for_seid(connection->query_seid); + if (!stream_endpoint) { + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + } + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); + return 0; + + case AVDTP_SI_RECONFIGURE: + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 0); return 0; default: - printf("AVDTP_RESPONSE_ACCEPT_MSG signal %d not implemented\n", connection->signaling_packet.signal_identifier); + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 1); + printf(" AVDTP_RESPONSE_ACCEPT_MSG, signal %d not implemented\n", connection->signaling_packet.signal_identifier); return 0; } break; case AVDTP_RESPONSE_REJECT_MSG: - printf("AVDTP_RESPONSE_REJECT_MSG signal %d not implemented\n", connection->signaling_packet.signal_identifier); - break; + printf(" AVDTP_RESPONSE_REJECT_MSG signal %d\n", connection->signaling_packet.signal_identifier); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 1); + return 0; case AVDTP_GENERAL_REJECT_MSG: - printf("AVDTP_GENERAL_REJECT_MSG signal %d not implemented\n", connection->signaling_packet.signal_identifier); - break; + printf(" AVDTP_GENERAL_REJECT_MSG signal %d\n", connection->signaling_packet.signal_identifier); + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE; + avdtp_signaling_emit_done(avdtp_sink_callback, connection->con_handle, 1); + return 0; } - printf(" handle_l2cap_data_packet_for_signaling_connection 3, signal %d\n", connection->signaling_packet.signal_identifier); + printf(" handle_l2cap_data_packet_for_signaling_connection, signal %d not handled\n", connection->signaling_packet.signal_identifier); return 0; } @@ -988,7 +1126,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; stream_endpoint->remote_sep_index = 0; - stream_endpoint->disconnect = 0; + stream_endpoint->media_disconnect = 0; stream_endpoint->remote_seps_num = 0; memset(stream_endpoint->remote_seps, 0, sizeof(stream_endpoint->remote_seps)); break; @@ -1058,20 +1196,137 @@ void avdtp_sink_connect(bd_addr_t bd_addr){ l2cap_create_channel(packet_handler, connection->remote_addr, PSM_AVDTP, 0xffff, NULL); } +void avdtp_sink_open_stream(uint16_t con_handle, uint8_t seid){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_media_connect: no connection for handle 0x%02x found\n", con_handle); + return; + } + + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { + printf("avdtp_sink_media_connect: wrong connection state %d\n", connection->state); + return; + } + + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint) { + printf("avdtp_sink_media_connect: no stream_endpoint for seid %d found\n", seid); + return; + } + printf(" AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_MEDIA_CONNECTED \n"); + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_MEDIA_CONNECTED; + connection->query_seid = seid; + + if (stream_endpoint->state < AVDTP_STREAM_ENDPOINT_CONFIGURED){ + return; + } else { + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED; + } + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); +} + +void avdtp_sink_start_stream(uint16_t con_handle, uint8_t seid){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_media_connect: no connection for handle 0x%02x found\n", con_handle); + return; + } + + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { + printf("avdtp_sink_media_connect: wrong connection state %d\n", connection->state); + return; + } + + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint) { + printf("avdtp_sink_media_connect: no stream_endpoint for seid %d found\n", seid); + return; + } + printf(" AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STARTED \n"); + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STARTED; + connection->query_seid = seid; + + if (stream_endpoint->state < AVDTP_STREAM_ENDPOINT_OPENED) return; + + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); +} + +void avdtp_sink_stop_stream(uint16_t con_handle, uint8_t seid){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_stop_stream: no connection for handle 0x%02x found\n", con_handle); + return; + } + + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { + printf("avdtp_sink_stop_stream: wrong connection state %d\n", connection->state); + return; + } + + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint) { + printf("avdtp_sink_stop_stream: no stream_endpoint for seid %d found\n", seid); + return; + } + + switch (stream_endpoint->state){ + case AVDTP_STREAM_ENDPOINT_OPENED: + case AVDTP_STREAM_ENDPOINT_STREAMING: + printf(" AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STOPED \n"); + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_STOPED; + connection->query_seid = seid; + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_CLOSING; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); + break; + default: + break; + } +} + +void avdtp_sink_abort_stream(uint16_t con_handle, uint8_t seid){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_abort_stream: no connection for handle 0x%02x found\n", con_handle); + return; + } + + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { + printf("avdtp_sink_abort_stream: wrong connection state %d\n", connection->state); + return; + } + + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint) { + printf("avdtp_sink_abort_stream: no stream_endpoint for seid %d found\n", seid); + return; + } + switch (stream_endpoint->state){ + case AVDTP_STREAM_ENDPOINT_CONFIGURED: + case AVDTP_STREAM_ENDPOINT_CLOSING: + case AVDTP_STREAM_ENDPOINT_OPENED: + case AVDTP_STREAM_ENDPOINT_STREAMING: + printf(" AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_ABORTED \n"); + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_L2CAP_FOR_STREAMING_ABORTED; + connection->query_seid = seid; + stream_endpoint->state = AVDTP_STREAM_ENDPOINT_ABORTING; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); + break; + default: + break; + } +} + 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); + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); } void avdtp_sink_discover_stream_endpoints(uint16_t con_handle){ @@ -1098,7 +1353,7 @@ void avdtp_sink_discover_stream_endpoints(uint16_t con_handle){ void avdtp_sink_get_capabilities(uint16_t con_handle, uint8_t seid){ avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); if (!connection){ - printf("avdtp_sink_discover_stream_endpoints: no connection for handle 0x%02x found\n", con_handle); + printf("avdtp_sink_get_capabilities: no connection for handle 0x%02x found\n", con_handle); return; } if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; @@ -1113,7 +1368,7 @@ void avdtp_sink_get_capabilities(uint16_t con_handle, uint8_t seid){ void avdtp_sink_get_all_capabilities(uint16_t con_handle, uint8_t seid){ avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); if (!connection){ - printf("avdtp_sink_discover_stream_endpoints: no connection for handle 0x%02x found\n", con_handle); + printf("avdtp_sink_get_all_capabilities: no connection for handle 0x%02x found\n", con_handle); return; } if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; @@ -1124,11 +1379,24 @@ void avdtp_sink_get_all_capabilities(uint16_t con_handle, uint8_t seid){ avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); } - -void avdtp_sink_set_capabilities(uint16_t con_handle, uint8_t acp_seid, uint8_t int_seid, uint16_t remote_capabilities_bitmap, avdtp_capabilities_t remote_capabilities){ +void avdtp_sink_get_configuration(uint16_t con_handle, uint8_t seid){ avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); if (!connection){ - printf("avdtp_sink_discover_stream_endpoints: no connection for handle 0x%02x found\n", con_handle); + printf("avdtp_sink_get_configuration: no connection for handle 0x%02x found\n", con_handle); + return; + } + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; + if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION; + connection->query_seid = seid; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); +} + +void avdtp_sink_set_configuration(uint16_t con_handle, uint8_t acp_seid, uint8_t int_seid, uint16_t remote_capabilities_bitmap, avdtp_capabilities_t remote_capabilities){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_set_configuration: no connection for handle 0x%02x found\n", con_handle); return; } if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; @@ -1141,3 +1409,38 @@ void avdtp_sink_set_capabilities(uint16_t con_handle, uint8_t acp_seid, uint8_t connection->remote_capabilities_bitmap = remote_capabilities_bitmap; avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); } + +void avdtp_sink_suspend(uint16_t con_handle, uint8_t seid){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_suspend: no connection for handle 0x%02x found\n", con_handle); + return; + } + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; + if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint) return; + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_SUSPEND_STREAM_WITH_SEID; + connection->query_seid = seid; + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); +} + + +void avdtp_sink_reconfigure(uint16_t con_handle, uint8_t seid){ + avdtp_connection_t * connection = get_avdtp_connection_for_con_handle(con_handle); + if (!connection){ + printf("avdtp_sink_reconfigure: no connection for handle 0x%02x found\n", con_handle); + return; + } + if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; + if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; + avdtp_stream_endpoint_t * stream_endpoint = get_avdtp_stream_endpoint_for_seid(seid); + if (!stream_endpoint) return; + connection->initiator_transaction_label++; + connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID; + connection->query_seid = seid; + + avdtp_sink_request_can_send_now_self(connection, connection->l2cap_signaling_cid); +} + diff --git a/test/avdtp/avdtp_sink.h b/test/avdtp/avdtp_sink.h index ba76cc7b8..fae62163d 100644 --- a/test/avdtp/avdtp_sink.h +++ b/test/avdtp/avdtp_sink.h @@ -121,11 +121,60 @@ void avdtp_sink_get_capabilities(uint16_t con_handle, uint8_t seid); void avdtp_sink_get_all_capabilities(uint16_t con_handle, uint8_t seid); /** - * @brief Set capabilities + * @brief Set configuration * @param con_handle */ -void avdtp_sink_set_capabilities(uint16_t con_handle, uint8_t acp_seid, uint8_t int_seid, uint16_t remote_capabilities_bitmap, avdtp_capabilities_t remote_capabilities); +void avdtp_sink_set_configuration(uint16_t con_handle, uint8_t acp_seid, uint8_t int_seid, uint16_t remote_capabilities_bitmap, avdtp_capabilities_t remote_capabilities); +/** + * @brief Get configuration + * @param con_handle + */ +void avdtp_sink_get_configuration(uint16_t con_handle, uint8_t acp_seid); + +/** + * @brief Suspend stream + * @param con_handle + * @param seid + */ +void avdtp_sink_suspend(uint16_t con_handle, uint8_t seid); + + +/** + * @brief Reconfigure stream + * @param con_handle + * @param seid + */ +void avdtp_sink_reconfigure(uint16_t con_handle, uint8_t seid); + + +/** + * @brief Open stream + * @param con_handle + * @param seid + */ +void avdtp_sink_open_stream(uint16_t con_handle, uint8_t seid); + +/** + * @brief Start stream + * @param con_handle + * @param seid + */ +void avdtp_sink_start_stream(uint16_t con_handle, uint8_t seid); + +/** + * @brief Start stream + * @param con_handle + * @param seid + */ +void avdtp_sink_abort_stream(uint16_t con_handle, uint8_t seid); + +/** + * @brief Start stream + * @param con_handle + * @param seid + */ +void avdtp_sink_stop_stream(uint16_t con_handle, uint8_t seid); /* API_END */ diff --git a/test/avdtp/avdtp_test.c b/test/avdtp/avdtp_test.c index abba055e6..37df625c0 100644 --- a/test/avdtp/avdtp_test.c +++ b/test/avdtp/avdtp_test.c @@ -167,7 +167,14 @@ typedef enum { AVDTP_APPLICATION_W2_DISCOVER_SEPS, AVDTP_APPLICATION_W2_GET_CAPABILITIES, AVDTP_APPLICATION_W2_GET_ALL_CAPABILITIES, - AVDTP_APPLICATION_W2_SET_CAPABILITIES + AVDTP_APPLICATION_W2_SET_CAPABILITIES, + AVDTP_APPLICATION_W2_SUSPEND_STREAM_WITH_SEID, + AVDTP_APPLICATION_W2_RECONFIGURE_WITH_SEID, + AVDTP_APPLICATION_W2_OPEN_STREAM_WITH_SEID, + AVDTP_APPLICATION_W2_START_STREAM_WITH_SEID, + AVDTP_APPLICATION_W2_ABORT_STREAM_WITH_SEID, + AVDTP_APPLICATION_W2_STOP_STREAM_WITH_SEID, + AVDTP_APPLICATION_W2_GET_CONFIGURATION } avdtp_application_state_t; avdtp_application_state_t app_state = AVDTP_APPLICATION_IDLE; @@ -406,8 +413,14 @@ static void show_usage(void){ printf("d - discover stream endpoints\n"); printf("g - get capabilities\n"); printf("a - get all capabilities\n"); - printf("s - set capabilities\n"); - + printf("s - set configuration\n"); + printf("f - get configuration\n"); + printf("R - reconfigure stream with seid 1\n"); + printf("o - open stream with seid 1\n"); + printf("m - start stream with seid 1\n"); + printf("A - abort stream with seid 1\n"); + printf("S - stop stream with seid 1\n"); + printf("P - suspend stream with seid 1\n"); printf("Ctrl-c - exit\n"); printf("---\n"); } @@ -433,7 +446,7 @@ static void stdin_process(btstack_data_source_t *ds, btstack_data_source_callbac break; case 'C': printf("Disconnect not implemented\n"); - // avdtp_sink_disconnect(local_cid); + avdtp_sink_disconnect(con_handle); break; case 'd': app_state = AVDTP_APPLICATION_W2_DISCOVER_SEPS; @@ -443,6 +456,10 @@ static void stdin_process(btstack_data_source_t *ds, btstack_data_source_callbac app_state = AVDTP_APPLICATION_W2_GET_CAPABILITIES; avdtp_sink_get_capabilities(con_handle, sep.seid); break; + case 'f': + app_state = AVDTP_APPLICATION_W2_GET_CONFIGURATION; + avdtp_sink_get_configuration(con_handle, sep.seid); + break; case 'a': app_state = AVDTP_APPLICATION_W2_GET_ALL_CAPABILITIES; avdtp_sink_get_all_capabilities(con_handle, sep.seid); @@ -455,8 +472,31 @@ static void stdin_process(btstack_data_source_t *ds, btstack_data_source_callbac remote_capabilities.media_codec.media_codec_information_len = sizeof(media_sbc_codec_info); remote_capabilities.media_codec.media_codec_information = media_sbc_codec_info; - avdtp_sink_set_capabilities(con_handle, sep.seid, 1, remote_capabilities_bitmap, remote_capabilities); + avdtp_sink_set_configuration(con_handle, sep.seid, 1, remote_capabilities_bitmap, remote_capabilities); break; + case 'R': + app_state = AVDTP_APPLICATION_W2_RECONFIGURE_WITH_SEID; + avdtp_sink_reconfigure(con_handle, sep.seid); + break; + case 'o': + app_state = AVDTP_APPLICATION_W2_OPEN_STREAM_WITH_SEID; + avdtp_sink_open_stream(con_handle, sep.seid); + break; + case 'm': + app_state = AVDTP_APPLICATION_W2_START_STREAM_WITH_SEID; + avdtp_sink_start_stream(con_handle, sep.seid); + break; + case 'A': + app_state = AVDTP_APPLICATION_W2_ABORT_STREAM_WITH_SEID; + avdtp_sink_abort_stream(con_handle, sep.seid); + case 'S': + app_state = AVDTP_APPLICATION_W2_STOP_STREAM_WITH_SEID; + avdtp_sink_stop_stream(con_handle, sep.seid); + case 'P': + app_state = AVDTP_APPLICATION_W2_SUSPEND_STREAM_WITH_SEID; + avdtp_sink_suspend(con_handle, sep.seid); + break; + case '\n': case '\r': break; diff --git a/test/avdtp/avdtp_util.c b/test/avdtp/avdtp_util.c index e6c07e1d8..17cb43f2e 100644 --- a/test/avdtp/avdtp_util.c +++ b/test/avdtp/avdtp_util.c @@ -63,9 +63,9 @@ uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value){ return bitmap; } -void avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size){ - if (size < 2) return; +int avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size){ int pos = 0; + if (size < 2) return pos; signaling_header->transaction_label = packet[pos] >> 4; signaling_header->packet_type = (avdtp_packet_type_t)((packet[pos] >> 2) & 0x03); signaling_header->message_type = (avdtp_message_type_t) (packet[pos] & 0x03); @@ -93,7 +93,8 @@ void avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, ui signaling_header->num_packets--; break; } - signaling_header->signal_identifier = packet[pos] & 0x3f; + signaling_header->signal_identifier = packet[pos++] & 0x3f; + return pos; } int avdtp_pack_service_capabilities(uint8_t * buffer, int size, avdtp_capabilities_t caps, avdtp_service_category_t category, uint8_t pack_all_capabilities){ @@ -145,15 +146,16 @@ int avdtp_pack_service_capabilities(uint8_t * buffer, int size, avdtp_capabiliti static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * connection, avdtp_service_category_t category, uint8_t cap_len){ connection->error_code = 0; - if (category < AVDTP_MEDIA_TRANSPORT || category > AVDTP_DELAY_REPORTING){ - printf(" ACP: BAD SERVICE CATEGORY %d\n", category); + if (category < AVDTP_MEDIA_TRANSPORT){ + printf(" ERROR: BAD SERVICE CATEGORY %d\n", category); connection->reject_service_category = category; connection->error_code = BAD_SERV_CATEGORY; return 1; } + if (connection->signaling_packet.signal_identifier == AVDTP_SI_RECONFIGURE){ if (category != AVDTP_CONTENT_PROTECTION && category != AVDTP_MEDIA_CODEC){ - printf(" ACP: REJECT CATEGORY, INVALID_CAPABILITIES\n"); + printf(" ERROR: REJECT CATEGORY, INVALID_CAPABILITIES\n"); connection->reject_service_category = category; connection->error_code = INVALID_CAPABILITIES; return 1; @@ -163,7 +165,7 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con switch(category){ case AVDTP_MEDIA_TRANSPORT: if (cap_len != 0){ - printf(" ACP: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); + printf(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); connection->reject_service_category = category; connection->error_code = BAD_MEDIA_TRANSPORT_FORMAT; return 1; @@ -172,7 +174,7 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con case AVDTP_REPORTING: case AVDTP_DELAY_REPORTING: if (cap_len != 0){ - printf(" ACP: REJECT CATEGORY, BAD_LENGTH\n"); + printf(" ERROR: REJECT CATEGORY, BAD_LENGTH\n"); connection->reject_service_category = category; connection->error_code = BAD_LENGTH; return 1; @@ -180,7 +182,7 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con break; case AVDTP_RECOVERY: if (cap_len < 3){ - printf(" ACP: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); + printf(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); connection->reject_service_category = category; connection->error_code = BAD_RECOVERY_FORMAT; return 1; @@ -188,6 +190,7 @@ static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * con break; case AVDTP_CONTENT_PROTECTION: if (cap_len < 2){ + printf(" ERROR: REJECT CATEGORY, BAD_CP_FORMAT\n"); connection->reject_service_category = category; connection->error_code = BAD_CP_FORMAT; return 1; @@ -211,18 +214,15 @@ uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdt int i; avdtp_service_category_t category = (avdtp_service_category_t)packet[pos++]; uint8_t cap_len = packet[pos++]; + if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; - int processed_cap_len = 0; + int rfa = 0; + while (pos < size){ + rfa = 0; processed_cap_len = pos; switch(category){ - case AVDTP_MEDIA_TRANSPORT: - break; - case AVDTP_REPORTING: - break; - case AVDTP_DELAY_REPORTING: - break; case AVDTP_RECOVERY: caps->recovery.recovery_type = packet[pos++]; caps->recovery.maximum_recovery_window_size = packet[pos++]; @@ -238,7 +238,7 @@ uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdt // connection->reject_service_category = category; // connection->error_code = UNSUPPORTED_CONFIGURATION; // support for content protection goes here - return 0; + break; case AVDTP_HEADER_COMPRESSION: caps->header_compression.back_ch = packet[pos] >> 7; @@ -262,17 +262,30 @@ uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdt caps->media_codec.media_codec_information = &packet[pos]; pos += caps->media_codec.media_codec_information_len; break; + case AVDTP_MEDIA_TRANSPORT: + case AVDTP_REPORTING: + case AVDTP_DELAY_REPORTING: + pos += cap_len; + break; default: + pos += cap_len; + rfa = 1; break; } - - registered_service_categories = store_bit16(registered_service_categories, category, 1); processed_cap_len = pos - processed_cap_len; - if (cap_len <= processed_cap_len && pos < size-2){ - category = (avdtp_service_category_t)packet[pos++]; - cap_len = packet[pos++]; - if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; - } + + if (cap_len == processed_cap_len){ + if (!rfa) { + registered_service_categories = store_bit16(registered_service_categories, category, 1); + } + if (pos < size-2){ + category = (avdtp_service_category_t)packet[pos++]; + cap_len = packet[pos++]; + if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; + // printf("category %d, pos %d + 2 + %d -> %d\n", category, old_pos, cap_len, pos + cap_len); + // printf_hexdump(packet+old_pos, size-old_pos); + } + } } return registered_service_categories; } @@ -280,23 +293,34 @@ uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdt void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uint8_t transaction_label, uint16_t registered_service_categories, avdtp_capabilities_t capabilities, uint8_t identifier){ if (signaling_packet->offset) return; uint8_t pack_all_capabilities = 1; + signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG; signaling_packet->size = 0; int i; + signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2; + switch (identifier) { case AVDTP_SI_GET_CAPABILITIES: pack_all_capabilities = 0; break; + case AVDTP_SI_GET_ALL_CAPABILITIES: + pack_all_capabilities = 1; + break; case AVDTP_SI_SET_CONFIGURATION: - signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2; signaling_packet->command[signaling_packet->size++] = signaling_packet->int_seid << 2; + signaling_packet->message_type = AVDTP_CMD_MSG; + break; + case AVDTP_SI_RECONFIGURE: + signaling_packet->message_type = AVDTP_CMD_MSG; break; default: + printf("avdtp_prepare_capabilities identifier %d\n", identifier); break; } for (i = 1; i < 9; i++){ if (get_bit16(registered_service_categories, i)){ // service category + printf(" pack service category %d\n", i); signaling_packet->command[signaling_packet->size++] = i; signaling_packet->size += avdtp_pack_service_capabilities(signaling_packet->command+signaling_packet->size, sizeof(signaling_packet->command)-signaling_packet->size, capabilities, (avdtp_service_category_t)i, pack_all_capabilities); } @@ -308,11 +332,6 @@ void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uin signaling_packet->signal_identifier = identifier; signaling_packet->transaction_label = transaction_label; - if (identifier == AVDTP_SI_SET_CONFIGURATION){ - signaling_packet->message_type = AVDTP_CMD_MSG; - } else { - signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG; - } } int avdtp_signaling_create_fragment(uint16_t cid, avdtp_signaling_packet_t * signaling_packet, uint8_t * out_buffer) { diff --git a/test/avdtp/avdtp_util.h b/test/avdtp/avdtp_util.h index c0bb96486..c2dcdbb0a 100644 --- a/test/avdtp/avdtp_util.h +++ b/test/avdtp/avdtp_util.h @@ -52,7 +52,7 @@ extern "C" { #endif uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_type, avdtp_message_type_t msg_type); -void avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size); +int avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size); uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value); int get_bit16(uint16_t bitmap, int position);