1 /* 2 * Copyright (C) 2016 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define __BTSTACK_FILE__ "avdtp.c" 39 40 41 #include <stdint.h> 42 #include <stdio.h> 43 #include <stdlib.h> 44 #include <string.h> 45 46 #include "btstack.h" 47 #include "avdtp.h" 48 #include "avdtp_util.h" 49 #include "avdtp_acceptor.h" 50 #include "avdtp_initiator.h" 51 52 static int record_id = -1; 53 static uint8_t attribute_value[1000]; 54 static const unsigned int attribute_value_buffer_size = sizeof(attribute_value); 55 56 typedef struct { 57 avdtp_connection_t * connection; 58 btstack_packet_handler_t avdtp_callback; 59 avdtp_sep_type_t query_role; 60 btstack_packet_handler_t packet_handler; 61 } avdtp_sdp_query_context_t; 62 63 static avdtp_sdp_query_context_t sdp_query_context; 64 static uint16_t avdtp_cid_counter = 0; 65 66 static void (*handle_media_data)(avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size); 67 static void avdtp_handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 68 69 static uint16_t avdtp_get_next_initiator_transaction_label(avdtp_context_t * context){ 70 context->initiator_transaction_id_counter++; 71 if (context->initiator_transaction_id_counter == 0){ 72 context->initiator_transaction_id_counter = 1; 73 } 74 return context->initiator_transaction_id_counter; 75 } 76 77 static uint16_t avdtp_get_next_avdtp_cid(void){ 78 avdtp_cid_counter++; 79 if (avdtp_cid_counter == 0){ 80 avdtp_cid_counter = 1; 81 } 82 return avdtp_cid_counter; 83 } 84 85 static uint16_t avdtp_get_next_local_seid(avdtp_context_t * context){ 86 context->stream_endpoints_id_counter++; 87 if (context->stream_endpoints_id_counter == 0){ 88 context->stream_endpoints_id_counter = 1; 89 } 90 return context->stream_endpoints_id_counter; 91 } 92 93 uint8_t avdtp_connect(bd_addr_t remote, avdtp_sep_type_t query_role, avdtp_context_t * avdtp_context, uint16_t * avdtp_cid){ 94 sdp_query_context.connection = NULL; 95 avdtp_connection_t * connection = avdtp_connection_for_bd_addr(remote, avdtp_context); 96 if (!connection){ 97 connection = avdtp_create_connection(remote, avdtp_context); 98 if (!connection){ 99 log_error("avdtp: not enough memory to create connection"); 100 return BTSTACK_MEMORY_ALLOC_FAILED; 101 } 102 } 103 if (connection->state != AVDTP_SIGNALING_CONNECTION_IDLE){ 104 log_error("avdtp_connect: sink in wrong state,"); 105 return AVDTP_CONNECTION_IN_WRONG_STATE; 106 } 107 108 if (!avdtp_cid) return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 109 110 *avdtp_cid = connection->avdtp_cid; 111 connection->state = AVDTP_SIGNALING_W4_SDP_QUERY_COMPLETE; 112 sdp_query_context.connection = connection; 113 sdp_query_context.query_role = query_role; 114 sdp_query_context.avdtp_callback = avdtp_context->avdtp_callback; 115 sdp_query_context.packet_handler = avdtp_context->packet_handler; 116 117 sdp_client_query_uuid16(&avdtp_handle_sdp_client_query_result, remote, BLUETOOTH_PROTOCOL_AVDTP); 118 return ERROR_CODE_SUCCESS; 119 } 120 121 void avdtp_register_media_transport_category(avdtp_stream_endpoint_t * stream_endpoint){ 122 if (!stream_endpoint){ 123 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 124 return; 125 } 126 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_TRANSPORT, 1); 127 stream_endpoint->sep.registered_service_categories = bitmap; 128 } 129 130 void avdtp_register_reporting_category(avdtp_stream_endpoint_t * stream_endpoint){ 131 if (!stream_endpoint){ 132 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 133 return; 134 } 135 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_REPORTING, 1); 136 stream_endpoint->sep.registered_service_categories = bitmap; 137 } 138 139 void avdtp_register_delay_reporting_category(avdtp_stream_endpoint_t * stream_endpoint){ 140 if (!stream_endpoint){ 141 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 142 return; 143 } 144 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_DELAY_REPORTING, 1); 145 stream_endpoint->sep.registered_service_categories = bitmap; 146 } 147 148 void avdtp_register_recovery_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t maximum_recovery_window_size, uint8_t maximum_number_media_packets){ 149 if (!stream_endpoint){ 150 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 151 return; 152 } 153 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_RECOVERY, 1); 154 stream_endpoint->sep.registered_service_categories = bitmap; 155 stream_endpoint->sep.capabilities.recovery.recovery_type = 0x01; // 0x01 = RFC2733 156 stream_endpoint->sep.capabilities.recovery.maximum_recovery_window_size = maximum_recovery_window_size; 157 stream_endpoint->sep.capabilities.recovery.maximum_number_media_packets = maximum_number_media_packets; 158 } 159 160 void avdtp_register_content_protection_category(avdtp_stream_endpoint_t * stream_endpoint, uint16_t cp_type, const uint8_t * cp_type_value, uint8_t cp_type_value_len){ 161 if (!stream_endpoint){ 162 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 163 return; 164 } 165 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_CONTENT_PROTECTION, 1); 166 stream_endpoint->sep.registered_service_categories = bitmap; 167 stream_endpoint->sep.capabilities.content_protection.cp_type = cp_type; 168 stream_endpoint->sep.capabilities.content_protection.cp_type_value = cp_type_value; 169 stream_endpoint->sep.capabilities.content_protection.cp_type_value_len = cp_type_value_len; 170 } 171 172 void avdtp_register_header_compression_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t back_ch, uint8_t media, uint8_t recovery){ 173 if (!stream_endpoint){ 174 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 175 return; 176 } 177 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_HEADER_COMPRESSION, 1); 178 stream_endpoint->sep.registered_service_categories = bitmap; 179 stream_endpoint->sep.capabilities.header_compression.back_ch = back_ch; 180 stream_endpoint->sep.capabilities.header_compression.media = media; 181 stream_endpoint->sep.capabilities.header_compression.recovery = recovery; 182 } 183 184 void avdtp_register_media_codec_category(avdtp_stream_endpoint_t * stream_endpoint, avdtp_media_type_t media_type, avdtp_media_codec_type_t media_codec_type, uint8_t * media_codec_info, uint16_t media_codec_info_len){ 185 if (!stream_endpoint){ 186 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 187 return; 188 } 189 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_CODEC, 1); 190 stream_endpoint->sep.registered_service_categories = bitmap; 191 stream_endpoint->sep.capabilities.media_codec.media_type = media_type; 192 stream_endpoint->sep.capabilities.media_codec.media_codec_type = media_codec_type; 193 stream_endpoint->sep.capabilities.media_codec.media_codec_information = media_codec_info; 194 stream_endpoint->sep.capabilities.media_codec.media_codec_information_len = media_codec_info_len; 195 } 196 197 void avdtp_register_multiplexing_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t fragmentation){ 198 if (!stream_endpoint){ 199 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 200 return; 201 } 202 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MULTIPLEXING, 1); 203 stream_endpoint->sep.registered_service_categories = bitmap; 204 stream_endpoint->sep.capabilities.multiplexing_mode.fragmentation = fragmentation; 205 } 206 207 208 /* START: tracking can send now requests pro l2cap cid */ 209 void avdtp_handle_can_send_now(avdtp_connection_t * connection, uint16_t l2cap_cid, avdtp_context_t * context){ 210 if (connection->wait_to_send_acceptor){ 211 connection->wait_to_send_acceptor = 0; 212 avdtp_acceptor_stream_config_subsm_run(connection, context); 213 } else if (connection->wait_to_send_initiator){ 214 connection->wait_to_send_initiator = 0; 215 avdtp_initiator_stream_config_subsm_run(connection, context); 216 } else if (connection->wait_to_send_self){ 217 connection->wait_to_send_self = 0; 218 if (connection->disconnect){ 219 btstack_linked_list_iterator_t it; 220 btstack_linked_list_iterator_init(&it, &context->stream_endpoints); 221 while (btstack_linked_list_iterator_has_next(&it)){ 222 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 223 if (stream_endpoint->connection == connection){ 224 if (stream_endpoint->state >= AVDTP_STREAM_ENDPOINT_OPENED && stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED){ 225 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED; 226 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 227 l2cap_disconnect(stream_endpoint->l2cap_media_cid, 0); 228 return; 229 } 230 } 231 } 232 connection->disconnect = 0; 233 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED; 234 l2cap_disconnect(connection->l2cap_signaling_cid, 0); 235 return; 236 } 237 } 238 239 // re-register 240 int more_to_send = connection->wait_to_send_acceptor || connection->wait_to_send_initiator || connection->wait_to_send_self; 241 if (more_to_send){ 242 l2cap_request_can_send_now_event(l2cap_cid); 243 } 244 } 245 /* END: tracking can send now requests pro l2cap cid */ 246 247 avdtp_connection_t * avdtp_create_connection(bd_addr_t remote_addr, avdtp_context_t * context){ 248 avdtp_connection_t * connection = btstack_memory_avdtp_connection_get(); 249 if (!connection){ 250 log_error("avdtp: not enough memory to create connection"); 251 return NULL; 252 } 253 memset(connection, 0, sizeof(avdtp_connection_t)); 254 connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 255 connection->initiator_transaction_label = avdtp_get_next_initiator_transaction_label(context); 256 connection->avdtp_cid = avdtp_get_next_avdtp_cid(); 257 memcpy(connection->remote_addr, remote_addr, 6); 258 btstack_linked_list_add(&context->connections, (btstack_linked_item_t *) connection); 259 return connection; 260 } 261 262 avdtp_stream_endpoint_t * avdtp_create_stream_endpoint(avdtp_sep_type_t sep_type, avdtp_media_type_t media_type, avdtp_context_t * context){ 263 avdtp_stream_endpoint_t * stream_endpoint = btstack_memory_avdtp_stream_endpoint_get(); 264 if (!stream_endpoint){ 265 log_error("avdtp: not enough memory to create stream endpoint"); 266 return NULL; 267 } 268 memset(stream_endpoint, 0, sizeof(avdtp_stream_endpoint_t)); 269 stream_endpoint->sep.seid = avdtp_get_next_local_seid(context); 270 stream_endpoint->sep.media_type = media_type; 271 stream_endpoint->sep.type = sep_type; 272 btstack_linked_list_add(&context->stream_endpoints, (btstack_linked_item_t *) stream_endpoint); 273 return stream_endpoint; 274 } 275 276 277 static void handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 278 int offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size); 279 switch (connection->signaling_packet.message_type){ 280 case AVDTP_CMD_MSG: 281 avdtp_acceptor_stream_config_subsm(connection, packet, size, offset, context); 282 break; 283 default: 284 avdtp_initiator_stream_config_subsm(connection, packet, size, offset, context); 285 break; 286 } 287 } 288 289 static void stream_endpoint_state_machine(avdtp_connection_t * connection, avdtp_stream_endpoint_t * stream_endpoint, uint8_t packet_type, uint8_t event, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 290 uint16_t local_cid; 291 uint8_t status; 292 bd_addr_t address; 293 294 switch (packet_type){ 295 case L2CAP_DATA_PACKET:{ 296 int offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size); 297 if (connection->signaling_packet.message_type == AVDTP_CMD_MSG){ 298 avdtp_acceptor_stream_config_subsm(connection, packet, size, offset, context); 299 } else { 300 avdtp_initiator_stream_config_subsm(connection, packet, size, offset, context); 301 } 302 break; 303 } 304 case HCI_EVENT_PACKET: 305 switch (event){ 306 case L2CAP_EVENT_CHANNEL_OPENED: 307 l2cap_event_channel_opened_get_address(packet, address); 308 if (stream_endpoint->l2cap_media_cid != 0){ 309 log_error("avdtp: channel already opened"); 310 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, address, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), L2CAP_SERVICE_ALREADY_REGISTERED); 311 return; 312 } 313 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED){ 314 log_info(" -> AVDTP_STREAM_ENDPOINT_OPENED failed - stream endpoint in wrong state %d, avdtp cid 0x%02x, l2cap_media_cid 0x%02x, local seid %d, remote seid %d", stream_endpoint->state, connection->avdtp_cid, stream_endpoint->l2cap_media_cid, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint)); 315 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, address, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE); 316 break; 317 } 318 status = l2cap_event_channel_opened_get_status(packet); 319 if (status){ 320 log_info(" -> AVDTP_STREAM_ENDPOINT_OPENED failed with status %d, avdtp cid 0x%02x, l2cap_media_cid 0x%02x, local seid %d, remote seid %d", status, connection->avdtp_cid, stream_endpoint->l2cap_media_cid, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint)); 321 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, address, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), status); 322 break; 323 } 324 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; 325 stream_endpoint->connection = connection; 326 stream_endpoint->l2cap_media_cid = l2cap_event_channel_opened_get_local_cid(packet); 327 stream_endpoint->media_con_handle = l2cap_event_channel_opened_get_handle(packet); 328 329 // log_info(" -> AVDTP_STREAM_ENDPOINT_OPENED, avdtp cid 0x%02x, l2cap_media_cid 0x%02x, local seid %d, remote seid %d", connection->avdtp_cid, stream_endpoint->l2cap_media_cid, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint)); 330 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, address, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), 0); 331 break; 332 333 case L2CAP_EVENT_CHANNEL_CLOSED: 334 local_cid = l2cap_event_channel_closed_get_local_cid(packet); 335 if (stream_endpoint->l2cap_media_cid == local_cid){ 336 avdtp_streaming_emit_connection_released(context->avdtp_callback, stream_endpoint->connection->avdtp_cid, avdtp_local_seid(stream_endpoint)); 337 stream_endpoint->l2cap_media_cid = 0; 338 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_IDLE; 339 stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; 340 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; 341 stream_endpoint->remote_sep_index = 0; 342 break; 343 } 344 if (stream_endpoint->l2cap_recovery_cid == local_cid){ 345 log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED recovery cid 0x%0x", local_cid); 346 stream_endpoint->l2cap_recovery_cid = 0; 347 break; 348 } 349 350 if (stream_endpoint->l2cap_reporting_cid == local_cid){ 351 log_info("L2CAP_EVENT_CHANNEL_CLOSED reporting cid 0x%0x", local_cid); 352 stream_endpoint->l2cap_reporting_cid = 0; 353 break; 354 } 355 break; 356 default: 357 break; 358 } 359 break; 360 default: 361 break; 362 } 363 } 364 365 static void avdtp_handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 366 UNUSED(packet_type); 367 UNUSED(channel); 368 UNUSED(size); 369 370 des_iterator_t des_list_it; 371 des_iterator_t prot_it; 372 uint16_t avdtp_l2cap_psm = 0; 373 uint16_t avdtp_version = 0; 374 // uint32_t avdtp_remote_uuid = 0; 375 376 if (!sdp_query_context.connection) return; 377 378 switch (hci_event_packet_get_type(packet)){ 379 case SDP_EVENT_QUERY_ATTRIBUTE_VALUE: 380 // Handle new SDP record 381 if (sdp_event_query_attribute_byte_get_record_id(packet) != record_id) { 382 record_id = sdp_event_query_attribute_byte_get_record_id(packet); 383 // log_info("SDP Record: Nr: %d", record_id); 384 } 385 386 if (sdp_event_query_attribute_byte_get_attribute_length(packet) <= attribute_value_buffer_size) { 387 attribute_value[sdp_event_query_attribute_byte_get_data_offset(packet)] = sdp_event_query_attribute_byte_get_data(packet); 388 389 if ((uint16_t)(sdp_event_query_attribute_byte_get_data_offset(packet)+1) == sdp_event_query_attribute_byte_get_attribute_length(packet)) { 390 391 switch(sdp_event_query_attribute_byte_get_attribute_id(packet)) { 392 case BLUETOOTH_ATTRIBUTE_SERVICE_CLASS_ID_LIST: 393 if (de_get_element_type(attribute_value) != DE_DES) break; 394 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) { 395 uint8_t * element = des_iterator_get_element(&des_list_it); 396 if (de_get_element_type(element) != DE_UUID) continue; 397 uint32_t uuid = de_get_uuid32(element); 398 switch (uuid){ 399 case BLUETOOTH_SERVICE_CLASS_AUDIO_SOURCE: 400 if (sdp_query_context.query_role != AVDTP_SOURCE) { 401 sdp_query_context.connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 402 avdtp_signaling_emit_connection_established(sdp_query_context.avdtp_callback, sdp_query_context.connection->avdtp_cid, sdp_query_context.connection->remote_addr, SDP_SERVICE_NOT_FOUND); 403 break; 404 } 405 // log_info("SDP Attribute 0x%04x: AVDTP SOURCE protocol UUID: 0x%04x", sdp_event_query_attribute_byte_get_attribute_id(packet), uuid); 406 // avdtp_remote_uuid = uuid; 407 break; 408 case BLUETOOTH_SERVICE_CLASS_AUDIO_SINK: 409 if (sdp_query_context.query_role != AVDTP_SINK) { 410 sdp_query_context.connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 411 avdtp_signaling_emit_connection_established(sdp_query_context.avdtp_callback, sdp_query_context.connection->avdtp_cid, sdp_query_context.connection->remote_addr, SDP_SERVICE_NOT_FOUND); 412 break; 413 } 414 // log_info("SDP Attribute 0x%04x: AVDTP SINK protocol UUID: 0x%04x", sdp_event_query_attribute_byte_get_attribute_id(packet), uuid); 415 // avdtp_remote_uuid = uuid; 416 break; 417 default: 418 break; 419 } 420 } 421 break; 422 423 case BLUETOOTH_ATTRIBUTE_PROTOCOL_DESCRIPTOR_LIST: { 424 // log_info("SDP Attribute: 0x%04x", sdp_event_query_attribute_byte_get_attribute_id(packet)); 425 426 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) { 427 uint8_t *des_element; 428 uint8_t *element; 429 uint32_t uuid; 430 431 if (des_iterator_get_type(&des_list_it) != DE_DES) continue; 432 433 des_element = des_iterator_get_element(&des_list_it); 434 des_iterator_init(&prot_it, des_element); 435 element = des_iterator_get_element(&prot_it); 436 437 if (de_get_element_type(element) != DE_UUID) continue; 438 439 uuid = de_get_uuid32(element); 440 switch (uuid){ 441 case BLUETOOTH_PROTOCOL_L2CAP: 442 if (!des_iterator_has_more(&prot_it)) continue; 443 des_iterator_next(&prot_it); 444 de_element_get_uint16(des_iterator_get_element(&prot_it), &avdtp_l2cap_psm); 445 break; 446 case BLUETOOTH_PROTOCOL_AVDTP: 447 if (!des_iterator_has_more(&prot_it)) continue; 448 des_iterator_next(&prot_it); 449 de_element_get_uint16(des_iterator_get_element(&prot_it), &avdtp_version); 450 break; 451 default: 452 break; 453 } 454 } 455 if (!avdtp_l2cap_psm) { 456 sdp_query_context.connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 457 avdtp_signaling_emit_connection_established(sdp_query_context.avdtp_callback, sdp_query_context.connection->avdtp_cid, sdp_query_context.connection->remote_addr, L2CAP_SERVICE_DOES_NOT_EXIST); 458 break; 459 } 460 sdp_query_context.connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; 461 l2cap_create_channel(sdp_query_context.packet_handler, sdp_query_context.connection->remote_addr, avdtp_l2cap_psm, l2cap_max_mtu(), NULL); 462 } 463 break; 464 default: 465 break; 466 } 467 } 468 } else { 469 log_error("SDP attribute value buffer size exceeded: available %d, required %d", attribute_value_buffer_size, sdp_event_query_attribute_byte_get_attribute_length(packet)); 470 } 471 break; 472 473 case SDP_EVENT_QUERY_COMPLETE: 474 log_info("General query done with status %d.", sdp_event_query_complete_get_status(packet)); 475 break; 476 } 477 } 478 479 480 void avdtp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 481 bd_addr_t event_addr; 482 uint16_t psm; 483 uint16_t local_cid; 484 avdtp_stream_endpoint_t * stream_endpoint = NULL; 485 avdtp_connection_t * connection = NULL; 486 btstack_linked_list_t * avdtp_connections = &context->connections; 487 btstack_linked_list_t * stream_endpoints = &context->stream_endpoints; 488 handle_media_data = context->handle_media_data; 489 // log_info("avdtp_packet_handler packet type %02x, event %02x ", packet_type, hci_event_packet_get_type(packet)); 490 switch (packet_type) { 491 case L2CAP_DATA_PACKET: 492 connection = avdtp_connection_for_l2cap_signaling_cid(channel, context); 493 if (connection){ 494 handle_l2cap_data_packet_for_signaling_connection(connection, packet, size, context); 495 break; 496 } 497 498 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(channel, context); 499 if (!stream_endpoint){ 500 if (!connection) break; 501 handle_l2cap_data_packet_for_signaling_connection(connection, packet, size, context); 502 break; 503 } 504 505 if (channel == stream_endpoint->connection->l2cap_signaling_cid){ 506 stream_endpoint_state_machine(stream_endpoint->connection, stream_endpoint, L2CAP_DATA_PACKET, 0, packet, size, context); 507 break; 508 } 509 510 if (channel == stream_endpoint->l2cap_media_cid){ 511 if (handle_media_data){ 512 (*handle_media_data)(stream_endpoint, packet, size); 513 } 514 break; 515 } 516 517 if (channel == stream_endpoint->l2cap_reporting_cid){ 518 // TODO 519 log_info("L2CAP_DATA_PACKET for reporting: NOT IMPLEMENTED"); 520 } else if (channel == stream_endpoint->l2cap_recovery_cid){ 521 // TODO 522 log_info("L2CAP_DATA_PACKET for recovery: NOT IMPLEMENTED"); 523 } else { 524 log_error("avdtp packet handler L2CAP_DATA_PACKET: local cid 0x%02x not found", channel); 525 } 526 break; 527 528 case HCI_EVENT_PACKET: 529 switch (hci_event_packet_get_type(packet)) { 530 case L2CAP_EVENT_INCOMING_CONNECTION: 531 l2cap_event_incoming_connection_get_address(packet, event_addr); 532 local_cid = l2cap_event_incoming_connection_get_local_cid(packet); 533 connection = avdtp_connection_for_bd_addr(event_addr, context); 534 535 if (!connection || connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED){ 536 connection = avdtp_create_connection(event_addr, context); 537 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; 538 log_info("L2CAP_EVENT_INCOMING_CONNECTION, connection %p, state connection %d", connection, connection->state); 539 l2cap_accept_connection(local_cid); 540 break; 541 } 542 543 stream_endpoint = avdtp_stream_endpoint_for_seid(connection->local_seid, context); 544 if (!stream_endpoint) { 545 log_info("L2CAP_EVENT_INCOMING_CONNECTION no streamendpoint found for seid %d", connection->local_seid); 546 break; 547 } 548 549 if (stream_endpoint->l2cap_media_cid == 0){ 550 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED) break; 551 l2cap_accept_connection(local_cid); 552 break; 553 } 554 break; 555 556 case L2CAP_EVENT_CHANNEL_OPENED: 557 // inform about new l2cap connection 558 l2cap_event_channel_opened_get_address(packet, event_addr); 559 local_cid = l2cap_event_channel_opened_get_local_cid(packet); 560 if (l2cap_event_channel_opened_get_status(packet)){ 561 avdtp_signaling_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, l2cap_event_channel_opened_get_status(packet)); 562 log_error("L2CAP connection to connection %s failed. status code 0x%02x", 563 bd_addr_to_str(event_addr), l2cap_event_channel_opened_get_status(packet)); 564 break; 565 } 566 psm = l2cap_event_channel_opened_get_psm(packet); 567 if (psm != BLUETOOTH_PROTOCOL_AVDTP){ 568 log_error("unexpected PSM - Not implemented yet, avdtp sink: L2CAP_EVENT_CHANNEL_OPENED"); 569 return; 570 } 571 // log_info("L2CAP_EVENT_CHANNEL_OPENED: Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x", 572 // bd_addr_to_str(event_addr), l2cap_event_channel_opened_get_handle(packet), psm, local_cid, l2cap_event_channel_opened_get_remote_cid(packet)); 573 574 if (psm != BLUETOOTH_PROTOCOL_AVDTP) break; 575 576 connection = avdtp_connection_for_bd_addr(event_addr, context); 577 if (!connection) break; 578 579 if (connection->l2cap_signaling_cid == 0) { 580 if (connection->state != AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED) break; 581 connection->l2cap_signaling_cid = local_cid; 582 connection->local_seid = 0; 583 connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; 584 log_info(" -> AVDTP_SIGNALING_CONNECTION_OPENED, connection %p, avdtp_cid 0x%02x", connection, connection->avdtp_cid); 585 avdtp_signaling_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, 0); 586 break; 587 } 588 589 stream_endpoint = avdtp_stream_endpoint_for_seid(connection->local_seid, context); 590 if (!stream_endpoint){ 591 log_info("L2CAP_EVENT_CHANNEL_OPENED: stream_endpoint not found"); 592 return; 593 } 594 stream_endpoint_state_machine(connection, stream_endpoint, HCI_EVENT_PACKET, L2CAP_EVENT_CHANNEL_OPENED, packet, size, context); 595 break; 596 597 case L2CAP_EVENT_CHANNEL_CLOSED: 598 local_cid = l2cap_event_channel_closed_get_local_cid(packet); 599 connection = avdtp_connection_for_l2cap_signaling_cid(local_cid, context); 600 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(local_cid, context); 601 602 if (stream_endpoint){ 603 stream_endpoint_state_machine(connection, stream_endpoint, HCI_EVENT_PACKET, L2CAP_EVENT_CHANNEL_CLOSED, packet, size, context); 604 break; 605 } 606 607 if (connection){ 608 avdtp_signaling_emit_connection_released(context->avdtp_callback, connection->avdtp_cid); 609 btstack_linked_list_remove(avdtp_connections, (btstack_linked_item_t*) connection); 610 btstack_linked_list_iterator_t it; 611 btstack_linked_list_iterator_init(&it, stream_endpoints); 612 while (btstack_linked_list_iterator_has_next(&it)){ 613 avdtp_stream_endpoint_t * _stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 614 615 if (_stream_endpoint->connection == connection){ 616 avdtp_initialize_stream_endpoint(_stream_endpoint); 617 } 618 } 619 btstack_memory_avdtp_connection_free(connection); 620 break; 621 } 622 break; 623 624 case HCI_EVENT_DISCONNECTION_COMPLETE: 625 break; 626 627 case L2CAP_EVENT_CAN_SEND_NOW: 628 connection = avdtp_connection_for_l2cap_signaling_cid(channel, context); 629 if (!connection) { 630 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(channel, context); 631 if (!stream_endpoint->connection) break; 632 connection = stream_endpoint->connection; 633 } 634 avdtp_handle_can_send_now(connection, channel, context); 635 break; 636 default: 637 log_info("unknown HCI event type %02x", hci_event_packet_get_type(packet)); 638 break; 639 } 640 break; 641 642 default: 643 // other packet type 644 break; 645 } 646 } 647 648 uint8_t avdtp_disconnect(uint16_t avdtp_cid, avdtp_context_t * context){ 649 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 650 if (!connection) return BTSTACK_MEMORY_ALLOC_FAILED; 651 if (connection->state == AVDTP_SIGNALING_CONNECTION_IDLE) return AVDTP_CONNECTION_IN_WRONG_STATE; 652 if (connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED) return AVDTP_CONNECTION_IN_WRONG_STATE; 653 654 connection->disconnect = 1; 655 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 656 return ERROR_CODE_SUCCESS; 657 } 658 659 uint8_t avdtp_open_stream(uint16_t avdtp_cid, uint8_t local_seid, uint8_t remote_seid, avdtp_context_t * context){ 660 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 661 if (!connection){ 662 log_error("avdtp_media_connect: no connection for signaling cid 0x%02x found", avdtp_cid); 663 return AVDTP_CONNECTION_DOES_NOT_EXIST; 664 } 665 if (avdtp_find_remote_sep(connection, remote_seid) == 0xFF){ 666 log_error("avdtp_media_connect: no remote sep for seid %d found", remote_seid); 667 return AVDTP_SEID_DOES_NOT_EXIST; 668 } 669 670 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { 671 log_error("avdtp_media_connect: wrong connection state %d", connection->state); 672 return AVDTP_CONNECTION_IN_WRONG_STATE; 673 } 674 675 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 676 if (!stream_endpoint) { 677 log_error("avdtp_media_connect: no stream_endpoint with seid %d found", local_seid); 678 return AVDTP_SEID_DOES_NOT_EXIST; 679 } 680 681 if (stream_endpoint->state < AVDTP_STREAM_ENDPOINT_CONFIGURED) return AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE; 682 if (stream_endpoint->remote_sep_index == AVDTP_INVALID_SEP_INDEX) return AVDTP_SEID_DOES_NOT_EXIST; 683 684 connection->initiator_transaction_label++; 685 connection->remote_seid = remote_seid; 686 connection->local_seid = local_seid; 687 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_OPEN_STREAM; 688 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM; 689 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 690 return ERROR_CODE_SUCCESS; 691 } 692 693 uint8_t avdtp_start_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 694 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 695 if (!connection){ 696 log_error("avdtp_start_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 697 return AVDTP_CONNECTION_DOES_NOT_EXIST; 698 } 699 700 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 701 if (!stream_endpoint) { 702 log_error("avdtp_start_stream: no stream_endpoint with seid %d found", local_seid); 703 return AVDTP_SEID_DOES_NOT_EXIST; 704 } 705 706 if (stream_endpoint->l2cap_media_cid == 0){ 707 log_error("avdtp_start_stream: no media connection for stream_endpoint with seid %d found", local_seid); 708 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 709 } 710 711 if (stream_endpoint->remote_sep_index == AVDTP_INVALID_SEP_INDEX || stream_endpoint->start_stream){ 712 return AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE; 713 } 714 715 stream_endpoint->start_stream = 1; 716 connection->local_seid = local_seid; 717 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 718 return ERROR_CODE_SUCCESS; 719 } 720 721 uint8_t avdtp_stop_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 722 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 723 if (!connection){ 724 log_error("avdtp_stop_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 725 return AVDTP_CONNECTION_DOES_NOT_EXIST; 726 } 727 728 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 729 if (!stream_endpoint) { 730 log_error("avdtp_stop_stream: no stream_endpoint with seid %d found", local_seid); 731 return AVDTP_SEID_DOES_NOT_EXIST; 732 } 733 734 if (stream_endpoint->l2cap_media_cid == 0){ 735 log_error("avdtp_stop_stream: no media connection for stream_endpoint with seid %d found", local_seid); 736 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 737 } 738 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->stop_stream) return ERROR_CODE_SUCCESS; 739 740 stream_endpoint->stop_stream = 1; 741 connection->local_seid = local_seid; 742 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 743 return ERROR_CODE_SUCCESS; 744 } 745 746 uint8_t avdtp_abort_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 747 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 748 if (!connection){ 749 log_error("avdtp_abort_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 750 return AVDTP_CONNECTION_DOES_NOT_EXIST; 751 } 752 753 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 754 if (!stream_endpoint) { 755 log_error("avdtp_abort_stream: no stream_endpoint with seid %d found", local_seid); 756 return AVDTP_SEID_DOES_NOT_EXIST; 757 } 758 759 if (stream_endpoint->l2cap_media_cid == 0){ 760 log_error("avdtp_abort_stream: no media connection for stream_endpoint with seid %d found", local_seid); 761 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 762 } 763 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->abort_stream) return ERROR_CODE_SUCCESS; 764 765 stream_endpoint->abort_stream = 1; 766 connection->local_seid = local_seid; 767 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 768 return ERROR_CODE_SUCCESS; 769 } 770 771 uint8_t avdtp_suspend_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 772 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 773 if (!connection){ 774 log_error("avdtp_suspend_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 775 return AVDTP_CONNECTION_DOES_NOT_EXIST; 776 } 777 778 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 779 if (!stream_endpoint) { 780 log_error("avdtp_suspend_stream: no stream_endpoint with seid %d found", local_seid); 781 return AVDTP_SEID_DOES_NOT_EXIST; 782 } 783 784 if (stream_endpoint->l2cap_media_cid == 0){ 785 log_error("avdtp_suspend_stream: no media connection for stream_endpoint with seid %d found", local_seid); 786 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 787 } 788 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->suspend_stream) return ERROR_CODE_SUCCESS; 789 790 stream_endpoint->suspend_stream = 1; 791 connection->local_seid = local_seid; 792 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 793 return ERROR_CODE_SUCCESS; 794 } 795 796 void avdtp_discover_stream_endpoints(uint16_t avdtp_cid, avdtp_context_t * context){ 797 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 798 if (!connection){ 799 log_error("avdtp_discover_stream_endpoints: no connection for signaling cid 0x%02x found", avdtp_cid); 800 return; 801 } 802 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 803 804 switch (connection->initiator_connection_state){ 805 case AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE: 806 connection->initiator_transaction_label++; 807 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS; 808 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 809 break; 810 default: 811 log_error("avdtp_discover_stream_endpoints: wrong state"); 812 break; 813 } 814 } 815 816 817 void avdtp_get_capabilities(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_context_t * context){ 818 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 819 if (!connection){ 820 log_error("avdtp_get_capabilities: no connection for AVDTP cid 0x%02x found", avdtp_cid); 821 return; 822 } 823 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 824 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 825 connection->initiator_transaction_label++; 826 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES; 827 connection->remote_seid = remote_seid; 828 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 829 } 830 831 832 void avdtp_get_all_capabilities(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_context_t * context){ 833 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 834 if (!connection){ 835 log_error("avdtp_get_all_capabilities: no connection for AVDTP cid 0x%02x found", avdtp_cid); 836 return; 837 } 838 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 839 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 840 connection->initiator_transaction_label++; 841 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES; 842 connection->remote_seid = remote_seid; 843 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 844 } 845 846 void avdtp_get_configuration(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_context_t * context){ 847 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 848 if (!connection){ 849 log_error("avdtp_get_configuration: no connection for AVDTP cid 0x%02x found", avdtp_cid); 850 return; 851 } 852 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 853 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 854 connection->initiator_transaction_label++; 855 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION; 856 connection->remote_seid = remote_seid; 857 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 858 } 859 860 void avdtp_set_configuration(uint16_t avdtp_cid, uint8_t local_seid, uint8_t remote_seid, uint16_t configured_services_bitmap, avdtp_capabilities_t configuration, avdtp_context_t * context){ 861 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 862 if (!connection){ 863 log_error("avdtp_set_configuration: no connection for AVDTP cid 0x%02x found", avdtp_cid); 864 return; 865 } 866 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 867 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 868 869 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(local_seid, context); 870 if (!stream_endpoint) { 871 log_error("avdtp_set_configuration: no initiator stream endpoint for seid %d", local_seid); 872 return; 873 } 874 875 connection->initiator_transaction_label++; 876 connection->remote_seid = remote_seid; 877 connection->local_seid = local_seid; 878 stream_endpoint->remote_configuration_bitmap = configured_services_bitmap; 879 stream_endpoint->remote_configuration = configuration; 880 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_SET_CONFIGURATION; 881 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 882 } 883 884 void avdtp_reconfigure(uint16_t avdtp_cid, uint8_t local_seid, uint8_t remote_seid, uint16_t configured_services_bitmap, avdtp_capabilities_t configuration, avdtp_context_t * context){ 885 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 886 if (!connection){ 887 log_error("avdtp_reconfigure: no connection for AVDTP cid 0x%02x found", avdtp_cid); 888 return; 889 } 890 //TODO: if opened only app capabilities, enable reconfigure for not opened 891 if (connection->state < AVDTP_SIGNALING_CONNECTION_OPENED) return; 892 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 893 894 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(local_seid, context); 895 if (!stream_endpoint) { 896 log_error("avdtp_reconfigure: no initiator stream endpoint for seid %d", local_seid); 897 return; 898 } 899 900 if (stream_endpoint->remote_sep_index == 0xFF){ 901 log_error("avdtp_reconfigure: no associated remote sep"); 902 return; 903 } 904 905 connection->initiator_transaction_label++; 906 connection->remote_seid = remote_seid; 907 connection->local_seid = local_seid; 908 stream_endpoint->remote_configuration_bitmap = configured_services_bitmap; 909 stream_endpoint->remote_configuration = configuration; 910 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID; 911 printf("AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID \n"); 912 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 913 } 914 915 uint8_t avdtp_remote_seps_num(uint16_t avdtp_cid, avdtp_context_t * context){ 916 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 917 if (!connection){ 918 log_error("avdtp_suspend: no connection for AVDTP cid 0x%02x found", avdtp_cid); 919 return 0; 920 } 921 return connection->remote_seps_num; 922 } 923 924 avdtp_sep_t * avdtp_remote_sep(uint16_t avdtp_cid, uint8_t index, avdtp_context_t * context){ 925 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 926 if (!connection){ 927 log_error("avdtp_suspend: no connection for AVDTP cid 0x%02x found", avdtp_cid); 928 return NULL; 929 } 930 return &connection->remote_seps[index]; 931 } 932 933 void avdtp_initialize_sbc_configuration_storage(avdtp_stream_endpoint_t * stream_endpoint, uint8_t * config_storage, uint16_t storage_size, uint8_t * packet, uint16_t packet_size){ 934 UNUSED(packet_size); 935 if (storage_size < 4) { 936 log_error("storage must have 4 bytes"); 937 return; 938 } 939 uint8_t sampling_frequency = avdtp_choose_sbc_sampling_frequency(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_sampling_frequency_bitmap(packet)); 940 uint8_t channel_mode = avdtp_choose_sbc_channel_mode(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_channel_mode_bitmap(packet)); 941 uint8_t block_length = avdtp_choose_sbc_block_length(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_block_length_bitmap(packet)); 942 uint8_t subbands = avdtp_choose_sbc_subbands(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_subbands_bitmap(packet)); 943 944 uint8_t allocation_method = avdtp_choose_sbc_allocation_method(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_allocation_method_bitmap(packet)); 945 uint8_t max_bitpool_value = avdtp_choose_sbc_max_bitpool_value(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_max_bitpool_value(packet)); 946 uint8_t min_bitpool_value = avdtp_choose_sbc_min_bitpool_value(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_min_bitpool_value(packet)); 947 948 config_storage[0] = (sampling_frequency << 4) | channel_mode; 949 config_storage[1] = (block_length << 4) | (subbands << 2) | allocation_method; 950 config_storage[2] = min_bitpool_value; 951 config_storage[3] = max_bitpool_value; 952 953 stream_endpoint->remote_configuration_bitmap = store_bit16(stream_endpoint->remote_configuration_bitmap, AVDTP_MEDIA_CODEC, 1); 954 stream_endpoint->remote_configuration.media_codec.media_type = AVDTP_AUDIO; 955 stream_endpoint->remote_configuration.media_codec.media_codec_type = AVDTP_CODEC_SBC; 956 stream_endpoint->remote_configuration.media_codec.media_codec_information_len = storage_size; 957 stream_endpoint->remote_configuration.media_codec.media_codec_information = config_storage; 958 } 959 960 uint8_t avdtp_choose_sbc_channel_mode(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_channel_mode_bitmap){ 961 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 962 uint8_t channel_mode_bitmap = (media_codec[0] & 0x0F) & remote_channel_mode_bitmap; 963 964 uint8_t channel_mode = AVDTP_SBC_STEREO; 965 if (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO){ 966 channel_mode = AVDTP_SBC_JOINT_STEREO; 967 } else if (channel_mode_bitmap & AVDTP_SBC_STEREO){ 968 channel_mode = AVDTP_SBC_STEREO; 969 } else if (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL){ 970 channel_mode = AVDTP_SBC_DUAL_CHANNEL; 971 } else if (channel_mode_bitmap & AVDTP_SBC_MONO){ 972 channel_mode = AVDTP_SBC_MONO; 973 } 974 return channel_mode; 975 } 976 977 uint8_t avdtp_choose_sbc_allocation_method(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_allocation_method_bitmap){ 978 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 979 uint8_t allocation_method_bitmap = (media_codec[1] & 0x03) & remote_allocation_method_bitmap; 980 981 uint8_t allocation_method = AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS; 982 if (allocation_method_bitmap & AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS){ 983 allocation_method = AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS; 984 } else if (allocation_method_bitmap & AVDTP_SBC_ALLOCATION_METHOD_SNR){ 985 allocation_method = AVDTP_SBC_ALLOCATION_METHOD_SNR; 986 } 987 return allocation_method; 988 } 989 990 uint8_t avdtp_stream_endpoint_seid(avdtp_stream_endpoint_t * stream_endpoint){ 991 if (!stream_endpoint) return 0; 992 return stream_endpoint->sep.seid; 993 } 994 uint8_t avdtp_choose_sbc_subbands(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_subbands_bitmap){ 995 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 996 uint8_t subbands_bitmap = ((media_codec[1] >> 2) & 0x03) & remote_subbands_bitmap; 997 998 uint8_t subbands = AVDTP_SBC_SUBBANDS_8; 999 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){ 1000 subbands = AVDTP_SBC_SUBBANDS_8; 1001 } else if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){ 1002 subbands = AVDTP_SBC_SUBBANDS_4; 1003 } 1004 return subbands; 1005 } 1006 1007 uint8_t avdtp_choose_sbc_block_length(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_block_length_bitmap){ 1008 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1009 uint8_t block_length_bitmap = (media_codec[1] >> 4) & remote_block_length_bitmap; 1010 1011 uint8_t block_length = AVDTP_SBC_BLOCK_LENGTH_16; 1012 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){ 1013 block_length = AVDTP_SBC_BLOCK_LENGTH_16; 1014 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){ 1015 block_length = AVDTP_SBC_BLOCK_LENGTH_12; 1016 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){ 1017 block_length = AVDTP_SBC_BLOCK_LENGTH_8; 1018 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){ 1019 block_length = AVDTP_SBC_BLOCK_LENGTH_4; 1020 } 1021 return block_length; 1022 } 1023 1024 uint8_t avdtp_choose_sbc_sampling_frequency(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_sampling_frequency_bitmap){ 1025 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1026 uint8_t sampling_frequency_bitmap = (media_codec[0] >> 4) & remote_sampling_frequency_bitmap; 1027 1028 uint8_t sampling_frequency = AVDTP_SBC_44100; 1029 if (sampling_frequency_bitmap & AVDTP_SBC_48000){ 1030 sampling_frequency = AVDTP_SBC_48000; 1031 } else if (sampling_frequency_bitmap & AVDTP_SBC_44100){ 1032 sampling_frequency = AVDTP_SBC_44100; 1033 } else if (sampling_frequency_bitmap & AVDTP_SBC_32000){ 1034 sampling_frequency = AVDTP_SBC_32000; 1035 } else if (sampling_frequency_bitmap & AVDTP_SBC_16000){ 1036 sampling_frequency = AVDTP_SBC_16000; 1037 } 1038 return sampling_frequency; 1039 } 1040 1041 uint8_t avdtp_choose_sbc_max_bitpool_value(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_max_bitpool_value){ 1042 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1043 return btstack_min(media_codec[3], remote_max_bitpool_value); 1044 } 1045 1046 uint8_t avdtp_choose_sbc_min_bitpool_value(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_min_bitpool_value){ 1047 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1048 return btstack_max(media_codec[2], remote_min_bitpool_value); 1049 } 1050