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 "classic/avdtp.h" 48 #include "classic/avdtp_util.h" 49 #include "classic/avdtp_acceptor.h" 50 #include "classic/avdtp_initiator.h" 51 52 #define CONFIGURATION_TIMEOUT_MS 300 53 54 static int record_id = -1; 55 static uint8_t attribute_value[1000]; 56 static const unsigned int attribute_value_buffer_size = sizeof(attribute_value); 57 58 // typedef struct { 59 // btstack_linked_list_t * avdtp_connections; 60 // avdtp_connection_t * connection; 61 // btstack_packet_handler_t avdtp_callback; 62 // avdtp_sep_type_t query_role; 63 // btstack_packet_handler_t packet_handler; 64 // uint16_t avdtp_l2cap_psm; 65 // uint16_t avdtp_version; 66 // uint8_t role_supported; 67 // } avdtp_sdp_query_context_t; 68 69 static avdtp_context_t * sdp_query_context; 70 static uint16_t avdtp_cid_counter = 0x55; 71 72 static void (*handle_media_data)(uint8_t local_seid, uint8_t *packet, uint16_t size); 73 static void avdtp_handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 74 75 void avdtp_configuration_timeout_handler(btstack_timer_source_t * timer){ 76 avdtp_connection_t * connection = (avdtp_connection_t *) btstack_run_loop_get_timer_context(timer); 77 if (!connection){ 78 log_error("Context of avdtp_configuration_timeout_handler is NULL"); 79 return; 80 } 81 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t*) connection->active_stream_endpoint; 82 if (!stream_endpoint) { 83 log_error("avdtp_configuration_timeout_handler: no initiator stream endpoint for seid %d", connection->local_seid); 84 return; 85 } 86 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_CONFIGURATION_SUBSTATEMACHINE) return; 87 connection->is_configuration_initiated_locally = 1; 88 connection->is_initiator = 1; 89 connection->initiator_transaction_label++; 90 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_SET_CONFIGURATION; 91 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 92 } 93 94 void avdtp_configuration_timer_start(avdtp_connection_t * connection){ 95 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t*) connection->active_stream_endpoint; 96 if (!stream_endpoint) { 97 log_error("avdtp_configuration_timeout_handler: no initiator stream endpoint for seid %d", connection->local_seid); 98 return; 99 } 100 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_CONFIGURATION_SUBSTATEMACHINE) return; 101 102 btstack_run_loop_remove_timer(&connection->configuration_timer); 103 btstack_run_loop_set_timer_handler(&connection->configuration_timer, avdtp_configuration_timeout_handler); 104 btstack_run_loop_set_timer_context(&connection->configuration_timer, connection); 105 btstack_run_loop_set_timer(&connection->configuration_timer, CONFIGURATION_TIMEOUT_MS); 106 btstack_run_loop_add_timer(&connection->configuration_timer); 107 } 108 109 void avdtp_configuration_timer_stop(avdtp_connection_t * connection){ 110 btstack_run_loop_remove_timer(&connection->configuration_timer); 111 } 112 113 static uint16_t avdtp_get_next_initiator_transaction_label(avdtp_context_t * context){ 114 context->initiator_transaction_id_counter++; 115 if (context->initiator_transaction_id_counter == 0){ 116 context->initiator_transaction_id_counter = 1; 117 } 118 return context->initiator_transaction_id_counter; 119 } 120 121 static uint16_t avdtp_get_next_avdtp_cid(void){ 122 avdtp_cid_counter++; 123 if (avdtp_cid_counter == 0){ 124 avdtp_cid_counter = 1; 125 } 126 return avdtp_cid_counter; 127 } 128 129 static uint16_t avdtp_get_next_local_seid(avdtp_context_t * context){ 130 context->stream_endpoints_id_counter++; 131 if (context->stream_endpoints_id_counter == 0){ 132 context->stream_endpoints_id_counter = 1; 133 } 134 return context->stream_endpoints_id_counter; 135 } 136 137 uint8_t avdtp_connect(bd_addr_t remote, avdtp_sep_type_t query_role, avdtp_context_t * avdtp_context, uint16_t * avdtp_cid){ 138 sdp_query_context = avdtp_context; 139 avdtp_connection_t * connection = avdtp_connection_for_bd_addr(remote, avdtp_context); 140 if (!connection){ 141 connection = avdtp_create_connection(remote, avdtp_context); 142 if (!connection){ 143 log_error("Not enough memory to create connection."); 144 return BTSTACK_MEMORY_ALLOC_FAILED; 145 } 146 } 147 148 *avdtp_cid = connection->avdtp_cid; 149 if (!avdtp_cid) { 150 return L2CAP_LOCAL_CID_DOES_NOT_EXIST; 151 } 152 avdtp_context->avdtp_cid = connection->avdtp_cid; 153 154 uint8_t err; 155 switch (connection->state){ 156 case AVDTP_SIGNALING_CONNECTION_IDLE: 157 connection->state = AVDTP_SIGNALING_W4_SDP_QUERY_COMPLETE; 158 connection->is_initiator = 1; 159 sdp_query_context = avdtp_context; 160 avdtp_context->avdtp_l2cap_psm = 0; 161 avdtp_context->avdtp_version = 0; 162 avdtp_context->query_role = query_role; 163 err = sdp_client_query_uuid16(&avdtp_handle_sdp_client_query_result, remote, BLUETOOTH_PROTOCOL_AVDTP); 164 if (err != ERROR_CODE_SUCCESS){ 165 connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 166 btstack_linked_list_remove(&avdtp_context->connections, (btstack_linked_item_t*) connection); 167 btstack_memory_avdtp_connection_free(connection); 168 } 169 return err; 170 case AVDTP_SIGNALING_CONNECTION_OPENED:{ 171 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_signaling_cid(connection->l2cap_signaling_cid, avdtp_context); 172 if (stream_endpoint){ 173 avdtp_streaming_emit_connection_established(avdtp_context->avdtp_callback, connection->avdtp_cid, remote, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), 0); 174 break; 175 } 176 avdtp_signaling_emit_connection_established(avdtp_context->avdtp_callback, connection->avdtp_cid, connection->remote_addr, ERROR_CODE_SUCCESS); 177 break; 178 } 179 default: 180 log_error("avdtp_connect: sink in wrong state"); 181 return AVDTP_CONNECTION_IN_WRONG_STATE; 182 183 } 184 return ERROR_CODE_SUCCESS; 185 } 186 187 void avdtp_register_media_transport_category(avdtp_stream_endpoint_t * stream_endpoint){ 188 if (!stream_endpoint){ 189 log_error("Stream endpoint with given seid is not registered."); 190 return; 191 } 192 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_TRANSPORT, 1); 193 stream_endpoint->sep.registered_service_categories = bitmap; 194 } 195 196 void avdtp_register_reporting_category(avdtp_stream_endpoint_t * stream_endpoint){ 197 if (!stream_endpoint){ 198 log_error("Stream endpoint with given seid is not registered."); 199 return; 200 } 201 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_REPORTING, 1); 202 stream_endpoint->sep.registered_service_categories = bitmap; 203 } 204 205 void avdtp_register_delay_reporting_category(avdtp_stream_endpoint_t * stream_endpoint){ 206 if (!stream_endpoint){ 207 log_error("Stream endpoint with given seid is not registered."); 208 return; 209 } 210 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_DELAY_REPORTING, 1); 211 stream_endpoint->sep.registered_service_categories = bitmap; 212 } 213 214 void avdtp_register_recovery_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t maximum_recovery_window_size, uint8_t maximum_number_media_packets){ 215 if (!stream_endpoint){ 216 log_error("Stream endpoint with given seid is not registered."); 217 return; 218 } 219 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_RECOVERY, 1); 220 stream_endpoint->sep.registered_service_categories = bitmap; 221 stream_endpoint->sep.capabilities.recovery.recovery_type = 0x01; // 0x01 = RFC2733 222 stream_endpoint->sep.capabilities.recovery.maximum_recovery_window_size = maximum_recovery_window_size; 223 stream_endpoint->sep.capabilities.recovery.maximum_number_media_packets = maximum_number_media_packets; 224 } 225 226 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){ 227 if (!stream_endpoint){ 228 log_error("Stream endpoint with given seid is not registered."); 229 return; 230 } 231 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_CONTENT_PROTECTION, 1); 232 stream_endpoint->sep.registered_service_categories = bitmap; 233 stream_endpoint->sep.capabilities.content_protection.cp_type = cp_type; 234 memcpy(stream_endpoint->sep.capabilities.content_protection.cp_type_value, cp_type_value, btstack_min(cp_type_value_len, AVDTP_MAX_CONTENT_PROTECTION_TYPE_VALUE_LEN)); 235 stream_endpoint->sep.capabilities.content_protection.cp_type_value_len = btstack_min(cp_type_value_len, AVDTP_MAX_CONTENT_PROTECTION_TYPE_VALUE_LEN); 236 } 237 238 void avdtp_register_header_compression_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t back_ch, uint8_t media, uint8_t recovery){ 239 if (!stream_endpoint){ 240 log_error("Stream endpoint with given seid is not registered."); 241 return; 242 } 243 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_HEADER_COMPRESSION, 1); 244 stream_endpoint->sep.registered_service_categories = bitmap; 245 stream_endpoint->sep.capabilities.header_compression.back_ch = back_ch; 246 stream_endpoint->sep.capabilities.header_compression.media = media; 247 stream_endpoint->sep.capabilities.header_compression.recovery = recovery; 248 } 249 250 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){ 251 if (!stream_endpoint){ 252 log_error("Stream endpoint with given seid is not registered."); 253 return; 254 } 255 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_CODEC, 1); 256 stream_endpoint->sep.registered_service_categories = bitmap; 257 stream_endpoint->sep.capabilities.media_codec.media_type = media_type; 258 stream_endpoint->sep.capabilities.media_codec.media_codec_type = media_codec_type; 259 stream_endpoint->sep.capabilities.media_codec.media_codec_information = media_codec_info; 260 stream_endpoint->sep.capabilities.media_codec.media_codec_information_len = media_codec_info_len; 261 } 262 263 void avdtp_register_multiplexing_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t fragmentation){ 264 if (!stream_endpoint){ 265 log_error("Stream endpoint with given seid is not registered."); 266 return; 267 } 268 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MULTIPLEXING, 1); 269 stream_endpoint->sep.registered_service_categories = bitmap; 270 stream_endpoint->sep.capabilities.multiplexing_mode.fragmentation = fragmentation; 271 } 272 273 274 /* START: tracking can send now requests pro l2cap cid */ 275 void avdtp_handle_can_send_now(avdtp_connection_t * connection, uint16_t l2cap_cid, avdtp_context_t * context){ 276 if (connection->wait_to_send_acceptor){ 277 connection->wait_to_send_acceptor = 0; 278 avdtp_acceptor_stream_config_subsm_run(connection, context); 279 } else if (connection->wait_to_send_initiator){ 280 connection->wait_to_send_initiator = 0; 281 avdtp_initiator_stream_config_subsm_run(connection, context); 282 } else if (connection->wait_to_send_self){ 283 connection->wait_to_send_self = 0; 284 if (connection->disconnect){ 285 btstack_linked_list_iterator_t it; 286 btstack_linked_list_iterator_init(&it, &context->stream_endpoints); 287 while (btstack_linked_list_iterator_has_next(&it)){ 288 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 289 if (stream_endpoint->connection == connection){ 290 if (stream_endpoint->state >= AVDTP_STREAM_ENDPOINT_OPENED && stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED){ 291 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED; 292 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 293 l2cap_disconnect(stream_endpoint->l2cap_media_cid, 0); 294 return; 295 } 296 } 297 } 298 connection->disconnect = 0; 299 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED; 300 l2cap_disconnect(connection->l2cap_signaling_cid, 0); 301 return; 302 } 303 } 304 305 // re-register 306 int more_to_send = connection->wait_to_send_acceptor || connection->wait_to_send_initiator || connection->wait_to_send_self; 307 if (more_to_send){ 308 l2cap_request_can_send_now_event(l2cap_cid); 309 } 310 } 311 /* END: tracking can send now requests pro l2cap cid */ 312 313 avdtp_connection_t * avdtp_create_connection(bd_addr_t remote_addr, avdtp_context_t * context){ 314 avdtp_connection_t * connection = btstack_memory_avdtp_connection_get(); 315 if (!connection){ 316 log_error("Not enough memory to create connection"); 317 return NULL; 318 } 319 memset(connection, 0, sizeof(avdtp_connection_t)); 320 connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 321 connection->initiator_transaction_label = avdtp_get_next_initiator_transaction_label(context); 322 connection->avdtp_cid = avdtp_get_next_avdtp_cid(); 323 context->avdtp_cid = connection->avdtp_cid; 324 memcpy(connection->remote_addr, remote_addr, 6); 325 btstack_linked_list_add(&context->connections, (btstack_linked_item_t *) connection); 326 return connection; 327 } 328 329 avdtp_stream_endpoint_t * avdtp_create_stream_endpoint(avdtp_sep_type_t sep_type, avdtp_media_type_t media_type, avdtp_context_t * context){ 330 avdtp_stream_endpoint_t * stream_endpoint = btstack_memory_avdtp_stream_endpoint_get(); 331 if (!stream_endpoint){ 332 log_error("Not enough memory to create stream endpoint"); 333 return NULL; 334 } 335 memset(stream_endpoint, 0, sizeof(avdtp_stream_endpoint_t)); 336 stream_endpoint->sep.seid = avdtp_get_next_local_seid(context); 337 stream_endpoint->sep.media_type = media_type; 338 stream_endpoint->sep.type = sep_type; 339 btstack_linked_list_add(&context->stream_endpoints, (btstack_linked_item_t *) stream_endpoint); 340 return stream_endpoint; 341 } 342 343 344 static void handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 345 int offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size); 346 switch (connection->signaling_packet.message_type){ 347 case AVDTP_CMD_MSG: 348 avdtp_acceptor_stream_config_subsm(connection, packet, size, offset, context); 349 break; 350 default: 351 avdtp_initiator_stream_config_subsm(connection, packet, size, offset, context); 352 break; 353 } 354 } 355 356 static void avdtp_handle_sdp_client_query_result(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 357 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(sdp_query_context->avdtp_cid, sdp_query_context); 358 if (!connection) { 359 log_error("Ndp query, connection with 0x%02x cid not found", sdp_query_context->avdtp_cid); 360 return; 361 } 362 if (connection->state != AVDTP_SIGNALING_W4_SDP_QUERY_COMPLETE) return; 363 364 UNUSED(packet_type); 365 UNUSED(channel); 366 UNUSED(size); 367 368 des_iterator_t des_list_it; 369 des_iterator_t prot_it; 370 uint8_t status; 371 372 switch (hci_event_packet_get_type(packet)){ 373 case SDP_EVENT_QUERY_ATTRIBUTE_VALUE: 374 // Handle new SDP record 375 if (sdp_event_query_attribute_byte_get_record_id(packet) != record_id) { 376 record_id = sdp_event_query_attribute_byte_get_record_id(packet); 377 // log_info("SDP Record: Nr: %d", record_id); 378 } 379 380 if (sdp_event_query_attribute_byte_get_attribute_length(packet) <= attribute_value_buffer_size) { 381 attribute_value[sdp_event_query_attribute_byte_get_data_offset(packet)] = sdp_event_query_attribute_byte_get_data(packet); 382 383 if ((uint16_t)(sdp_event_query_attribute_byte_get_data_offset(packet)+1) == sdp_event_query_attribute_byte_get_attribute_length(packet)) { 384 385 switch(sdp_event_query_attribute_byte_get_attribute_id(packet)) { 386 case BLUETOOTH_ATTRIBUTE_SERVICE_CLASS_ID_LIST: 387 if (de_get_element_type(attribute_value) != DE_DES) break; 388 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) { 389 uint8_t * element = des_iterator_get_element(&des_list_it); 390 if (de_get_element_type(element) != DE_UUID) continue; 391 uint32_t uuid = de_get_uuid32(element); 392 switch (uuid){ 393 case BLUETOOTH_SERVICE_CLASS_AUDIO_SOURCE: 394 if (sdp_query_context->query_role == AVDTP_SOURCE) { 395 sdp_query_context->role_supported = 1; 396 break; 397 } 398 // log_info("SDP Attribute 0x%04x: AVDTP SOURCE protocol UUID: 0x%04x", sdp_event_query_attribute_byte_get_attribute_id(packet), uuid); 399 // avdtp_remote_uuid = uuid; 400 break; 401 case BLUETOOTH_SERVICE_CLASS_AUDIO_SINK: 402 if (sdp_query_context->query_role == AVDTP_SINK) { 403 sdp_query_context->role_supported = 1; 404 break; 405 } 406 // log_info("SDP Attribute 0x%04x: AVDTP SINK protocol UUID: 0x%04x", sdp_event_query_attribute_byte_get_attribute_id(packet), uuid); 407 // avdtp_remote_uuid = uuid; 408 break; 409 default: 410 break; 411 } 412 } 413 break; 414 415 case BLUETOOTH_ATTRIBUTE_PROTOCOL_DESCRIPTOR_LIST: { 416 // log_info("SDP Attribute: 0x%04x", sdp_event_query_attribute_byte_get_attribute_id(packet)); 417 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) { 418 uint8_t *des_element; 419 uint8_t *element; 420 uint32_t uuid; 421 422 if (des_iterator_get_type(&des_list_it) != DE_DES) continue; 423 424 des_element = des_iterator_get_element(&des_list_it); 425 des_iterator_init(&prot_it, des_element); 426 element = des_iterator_get_element(&prot_it); 427 428 if (de_get_element_type(element) != DE_UUID) continue; 429 430 uuid = de_get_uuid32(element); 431 switch (uuid){ 432 case BLUETOOTH_PROTOCOL_L2CAP: 433 if (!des_iterator_has_more(&prot_it)) continue; 434 des_iterator_next(&prot_it); 435 de_element_get_uint16(des_iterator_get_element(&prot_it), &sdp_query_context->avdtp_l2cap_psm); 436 break; 437 case BLUETOOTH_PROTOCOL_AVDTP: 438 if (!des_iterator_has_more(&prot_it)) continue; 439 des_iterator_next(&prot_it); 440 de_element_get_uint16(des_iterator_get_element(&prot_it), &sdp_query_context->avdtp_version); 441 break; 442 default: 443 break; 444 } 445 } 446 } 447 break; 448 default: 449 break; 450 } 451 } 452 } else { 453 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)); 454 } 455 break; 456 457 case SDP_EVENT_QUERY_COMPLETE: 458 status = sdp_event_query_complete_get_status(packet); 459 if (status != ERROR_CODE_SUCCESS){ 460 avdtp_signaling_emit_connection_established(sdp_query_context->avdtp_callback, sdp_query_context->avdtp_cid, connection->remote_addr, status); 461 btstack_linked_list_remove(&sdp_query_context->connections, (btstack_linked_item_t*) connection); 462 btstack_memory_avdtp_connection_free(connection); 463 log_info("NDP query failed with status 0x%02x.", status); 464 break; 465 } 466 if (!sdp_query_context->role_supported){ 467 btstack_linked_list_remove(&sdp_query_context->connections, (btstack_linked_item_t*) connection); 468 btstack_memory_avdtp_connection_free(connection); 469 avdtp_signaling_emit_connection_established(sdp_query_context->avdtp_callback, sdp_query_context->avdtp_cid, connection->remote_addr, SDP_SERVICE_NOT_FOUND); 470 log_info("NDP query, remote device does not support required role."); 471 break; 472 } 473 if (!sdp_query_context->avdtp_l2cap_psm) { 474 btstack_linked_list_remove(&sdp_query_context->connections, (btstack_linked_item_t*)connection); 475 btstack_memory_avdtp_connection_free(connection); 476 avdtp_signaling_emit_connection_established(sdp_query_context->avdtp_callback, sdp_query_context->avdtp_cid, connection->remote_addr, L2CAP_SERVICE_DOES_NOT_EXIST); 477 log_info("NDP query, no l2cap psm found."); 478 break; 479 } 480 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; 481 l2cap_create_channel(sdp_query_context->packet_handler, connection->remote_addr, sdp_query_context->avdtp_l2cap_psm, l2cap_max_mtu(), NULL); 482 break; 483 } 484 } 485 486 487 void avdtp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 488 bd_addr_t event_addr; 489 uint16_t psm; 490 uint16_t local_cid; 491 uint8_t status; 492 avdtp_stream_endpoint_t * stream_endpoint = NULL; 493 avdtp_connection_t * connection = NULL; 494 btstack_linked_list_t * avdtp_connections = &context->connections; 495 btstack_linked_list_t * stream_endpoints = &context->stream_endpoints; 496 handle_media_data = context->handle_media_data; 497 // log_info("avdtp_packet_handler packet type %02x, event %02x ", packet_type, hci_event_packet_get_type(packet)); 498 switch (packet_type) { 499 case L2CAP_DATA_PACKET: 500 connection = avdtp_connection_for_l2cap_signaling_cid(channel, context); 501 if (connection){ 502 handle_l2cap_data_packet_for_signaling_connection(connection, packet, size, context); 503 break; 504 } 505 506 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(channel, context); 507 if (!stream_endpoint){ 508 if (!connection) break; 509 handle_l2cap_data_packet_for_signaling_connection(connection, packet, size, context); 510 break; 511 } 512 513 if (stream_endpoint->connection){ 514 if (channel == stream_endpoint->connection->l2cap_signaling_cid){ 515 int offset = avdtp_read_signaling_header(&stream_endpoint->connection->signaling_packet, packet, size); 516 if (stream_endpoint->connection->signaling_packet.message_type == AVDTP_CMD_MSG){ 517 avdtp_acceptor_stream_config_subsm(stream_endpoint->connection, packet, size, offset, context); 518 } else { 519 avdtp_initiator_stream_config_subsm(stream_endpoint->connection, packet, size, offset, context); 520 } 521 break; 522 } 523 } 524 525 if (channel == stream_endpoint->l2cap_media_cid){ 526 if (handle_media_data){ 527 (*handle_media_data)(avdtp_local_seid(stream_endpoint), packet, size); 528 } 529 break; 530 } 531 532 if (channel == stream_endpoint->l2cap_reporting_cid){ 533 // TODO 534 log_info("L2CAP_DATA_PACKET for reporting: NOT IMPLEMENTED"); 535 } else if (channel == stream_endpoint->l2cap_recovery_cid){ 536 // TODO 537 log_info("L2CAP_DATA_PACKET for recovery: NOT IMPLEMENTED"); 538 } else { 539 log_error("avdtp packet handler L2CAP_DATA_PACKET: local cid 0x%02x not found", channel); 540 } 541 break; 542 543 case HCI_EVENT_PACKET: 544 switch (hci_event_packet_get_type(packet)) { 545 case L2CAP_EVENT_INCOMING_CONNECTION: 546 l2cap_event_incoming_connection_get_address(packet, event_addr); 547 local_cid = l2cap_event_incoming_connection_get_local_cid(packet); 548 connection = avdtp_connection_for_bd_addr(event_addr, context); 549 log_info("N2CAP_EVENT_INCOMING_CONNECTION, local cid 0x%02x ", local_cid); 550 if (connection){ 551 log_info("Nonnection state %d", connection->state); 552 } 553 if (!connection || connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED){ 554 connection = avdtp_create_connection(event_addr, context); 555 connection->is_initiator = 0; 556 log_info("N2CAP_EVENT_INCOMING_CONNECTION: role is_initiator %d", connection->is_initiator); 557 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; 558 log_info("N2CAP_EVENT_INCOMING_CONNECTION, connection %p, state connection %d, avdtp cid 0x%02x", connection, connection->state, connection->avdtp_cid); 559 l2cap_accept_connection(local_cid); 560 break; 561 } 562 563 stream_endpoint = avdtp_stream_endpoint_for_seid(connection->local_seid, context); 564 if (!stream_endpoint) { 565 log_info("N2CAP_EVENT_INCOMING_CONNECTION no streamendpoint found for seid %d", connection->local_seid); 566 break; 567 } 568 569 log_info("Nhecking l2cap_media_cid %d, for local seid %d, state of stream endpoint %d, role is_initiator %d", stream_endpoint->l2cap_media_cid, connection->local_seid, stream_endpoint->state, connection->is_initiator); 570 if (stream_endpoint->l2cap_media_cid == 0){ 571 if (connection->is_initiator){ 572 l2cap_decline_connection(local_cid); 573 break; 574 } 575 l2cap_accept_connection(local_cid); 576 break; 577 } 578 l2cap_decline_connection(local_cid); 579 break; 580 581 case L2CAP_EVENT_CHANNEL_OPENED: 582 psm = l2cap_event_channel_opened_get_psm(packet); 583 if (psm != BLUETOOTH_PROTOCOL_AVDTP){ 584 log_info("Nnexpected PSM - Not implemented yet, avdtp sink: L2CAP_EVENT_CHANNEL_OPENED "); 585 return; 586 } 587 588 status = l2cap_event_channel_opened_get_status(packet); 589 // inform about new l2cap connection 590 l2cap_event_channel_opened_get_address(packet, event_addr); 591 local_cid = l2cap_event_channel_opened_get_local_cid(packet); 592 connection = avdtp_connection_for_bd_addr(event_addr, context); 593 594 log_info("N2CAP_EVENT_CHANNEL_OPENED: status %d, cid 0x%02x , signaling connection %p ", status, local_cid, connection); 595 connection = avdtp_connection_for_bd_addr(event_addr, context); 596 if (!connection){ 597 log_info("N2CAP_EVENT_CHANNEL_OPENED 2: status %d, signaling connection %p ", status, connection); 598 break; 599 } 600 601 if (connection->l2cap_signaling_cid == 0) { 602 if (status){ 603 log_info("N2CAP connection to %s failed. status code 0x%02x", bd_addr_to_str(event_addr), status); 604 connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 605 connection->l2cap_signaling_cid = 0; 606 avdtp_signaling_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, l2cap_event_channel_opened_get_status(packet)); 607 break; 608 } 609 if (connection->state != AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED) { 610 log_info("N2CAP connection to %s failed. status code 0x%02x", bd_addr_to_str(event_addr), status); 611 avdtp_signaling_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, AVDTP_CONNECTION_IN_WRONG_STATE); 612 break; 613 } 614 connection->l2cap_signaling_cid = local_cid; 615 connection->local_seid = 0; 616 connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; 617 log_info("NVDTP_SIGNALING_CONNECTION_OPENED, connection %p, l2cap_signaling_cid 0x%02x, avdtp_cid 0x%02x", connection, local_cid, connection->avdtp_cid); 618 avdtp_signaling_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, 0); 619 break; 620 } 621 622 stream_endpoint = avdtp_stream_endpoint_for_seid(connection->local_seid, context); 623 if (!stream_endpoint){ 624 log_info("N2CAP_EVENT_CHANNEL_OPENED: stream_endpoint not found"); 625 return; 626 } 627 628 if (stream_endpoint->l2cap_media_cid == 0){ 629 status = l2cap_event_channel_opened_get_status(packet); 630 if (status){ 631 log_info("NVDTP_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)); 632 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_IDLE; 633 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), status); 634 break; 635 } 636 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED){ 637 log_info("NVDTP_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)); 638 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE); 639 break; 640 } 641 642 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; 643 stream_endpoint->connection = connection; 644 stream_endpoint->l2cap_media_cid = l2cap_event_channel_opened_get_local_cid(packet); 645 stream_endpoint->media_con_handle = l2cap_event_channel_opened_get_handle(packet); 646 647 log_info("NVDTP_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)); 648 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->avdtp_cid, event_addr, avdtp_local_seid(stream_endpoint), avdtp_remote_seid(stream_endpoint), 0); 649 return; 650 } 651 break; 652 653 case L2CAP_EVENT_CHANNEL_CLOSED: 654 local_cid = l2cap_event_channel_closed_get_local_cid(packet); 655 connection = avdtp_connection_for_l2cap_signaling_cid(local_cid, context); 656 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(local_cid, context); 657 log_info("Neceived L2CAP_EVENT_CHANNEL_CLOSED, cid 0x%2x, connection %p, stream_endpoint %p", local_cid, connection, stream_endpoint); 658 659 if (stream_endpoint){ 660 if (stream_endpoint->l2cap_media_cid == local_cid){ 661 connection = stream_endpoint->connection; 662 if (connection) { 663 avdtp_streaming_emit_connection_released(context->avdtp_callback, connection->avdtp_cid, avdtp_local_seid(stream_endpoint)); 664 } 665 stream_endpoint->l2cap_media_cid = 0; 666 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_IDLE; 667 stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; 668 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; 669 stream_endpoint->remote_sep_index = 0; 670 if (connection && connection->disconnect){ 671 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 672 } 673 break; 674 } 675 if (stream_endpoint->l2cap_recovery_cid == local_cid){ 676 log_info("N2CAP_EVENT_CHANNEL_CLOSED recovery cid 0x%0x", local_cid); 677 stream_endpoint->l2cap_recovery_cid = 0; 678 break; 679 } 680 681 if (stream_endpoint->l2cap_reporting_cid == local_cid){ 682 log_info("N2CAP_EVENT_CHANNEL_CLOSED reporting cid 0x%0x", local_cid); 683 stream_endpoint->l2cap_reporting_cid = 0; 684 break; 685 } 686 } 687 688 if (connection){ 689 btstack_run_loop_remove_timer(&connection->configuration_timer); 690 avdtp_signaling_emit_connection_released(context->avdtp_callback, connection->avdtp_cid); 691 692 btstack_linked_list_remove(avdtp_connections, (btstack_linked_item_t*) connection); 693 btstack_linked_list_iterator_t it; 694 btstack_linked_list_iterator_init(&it, stream_endpoints); 695 while (btstack_linked_list_iterator_has_next(&it)){ 696 avdtp_stream_endpoint_t * _stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 697 698 if (_stream_endpoint->connection == connection){ 699 avdtp_initialize_stream_endpoint(_stream_endpoint); 700 } 701 } 702 btstack_memory_avdtp_connection_free(connection); 703 break; 704 } 705 706 break; 707 708 case HCI_EVENT_DISCONNECTION_COMPLETE: 709 break; 710 711 case L2CAP_EVENT_CAN_SEND_NOW: 712 connection = avdtp_connection_for_l2cap_signaling_cid(channel, context); 713 if (!connection) { 714 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(channel, context); 715 if (!stream_endpoint->connection) break; 716 connection = stream_endpoint->connection; 717 } 718 avdtp_handle_can_send_now(connection, channel, context); 719 break; 720 default: 721 log_info("Nnknown HCI event type %02x", hci_event_packet_get_type(packet)); 722 break; 723 } 724 break; 725 726 default: 727 // other packet type 728 break; 729 } 730 } 731 732 uint8_t avdtp_disconnect(uint16_t avdtp_cid, avdtp_context_t * context){ 733 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 734 if (!connection) return BTSTACK_MEMORY_ALLOC_FAILED; 735 if (connection->state == AVDTP_SIGNALING_CONNECTION_IDLE){ 736 avdtp_signaling_emit_connection_released(context->avdtp_callback, connection->avdtp_cid); 737 return ERROR_CODE_SUCCESS; 738 } 739 if (connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED) return ERROR_CODE_SUCCESS; 740 741 connection->disconnect = 1; 742 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 743 return ERROR_CODE_SUCCESS; 744 } 745 746 uint8_t avdtp_open_stream(uint16_t avdtp_cid, uint8_t local_seid, uint8_t remote_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_media_connect: no connection for signaling cid 0x%02x found", avdtp_cid); 750 return AVDTP_CONNECTION_DOES_NOT_EXIST; 751 } 752 if (avdtp_find_remote_sep(connection, remote_seid) == 0xFF){ 753 log_error("avdtp_media_connect: no remote sep for seid %d found", remote_seid); 754 return AVDTP_SEID_DOES_NOT_EXIST; 755 } 756 757 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { 758 log_error("avdtp_media_connect: wrong connection state %d", connection->state); 759 return AVDTP_CONNECTION_IN_WRONG_STATE; 760 } 761 762 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 763 if (!stream_endpoint) { 764 log_error("avdtp_media_connect: no stream_endpoint with seid %d found", local_seid); 765 return AVDTP_SEID_DOES_NOT_EXIST; 766 } 767 768 if (stream_endpoint->state < AVDTP_STREAM_ENDPOINT_CONFIGURED) return AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE; 769 if (stream_endpoint->remote_sep_index == AVDTP_INVALID_SEP_INDEX) return AVDTP_SEID_DOES_NOT_EXIST; 770 771 connection->initiator_transaction_label++; 772 connection->remote_seid = remote_seid; 773 connection->local_seid = local_seid; 774 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_OPEN_STREAM; 775 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM; 776 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 777 return ERROR_CODE_SUCCESS; 778 } 779 780 uint8_t avdtp_start_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 781 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 782 if (!connection){ 783 log_error("avdtp_start_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 784 return AVDTP_CONNECTION_DOES_NOT_EXIST; 785 } 786 787 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 788 if (!stream_endpoint) { 789 log_error("avdtp_start_stream: no stream_endpoint with seid %d found", local_seid); 790 return AVDTP_SEID_DOES_NOT_EXIST; 791 } 792 793 if (stream_endpoint->l2cap_media_cid == 0){ 794 log_error("avdtp_start_stream: no media connection for stream_endpoint with seid %d found", local_seid); 795 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 796 } 797 798 if (stream_endpoint->remote_sep_index == AVDTP_INVALID_SEP_INDEX || stream_endpoint->start_stream){ 799 return AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE; 800 } 801 802 stream_endpoint->start_stream = 1; 803 connection->local_seid = local_seid; 804 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 805 return ERROR_CODE_SUCCESS; 806 } 807 808 uint8_t avdtp_stop_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 809 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 810 if (!connection){ 811 log_error("avdtp_stop_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 812 return AVDTP_CONNECTION_DOES_NOT_EXIST; 813 } 814 815 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 816 if (!stream_endpoint) { 817 log_error("avdtp_stop_stream: no stream_endpoint with seid %d found", local_seid); 818 return AVDTP_SEID_DOES_NOT_EXIST; 819 } 820 821 if (stream_endpoint->l2cap_media_cid == 0){ 822 log_error("avdtp_stop_stream: no media connection for stream_endpoint with seid %d found", local_seid); 823 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 824 } 825 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->stop_stream) return ERROR_CODE_SUCCESS; 826 827 stream_endpoint->stop_stream = 1; 828 connection->local_seid = local_seid; 829 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 830 return ERROR_CODE_SUCCESS; 831 } 832 833 uint8_t avdtp_abort_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 834 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 835 if (!connection){ 836 log_error("avdtp_abort_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 837 return AVDTP_CONNECTION_DOES_NOT_EXIST; 838 } 839 840 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 841 if (!stream_endpoint) { 842 log_error("avdtp_abort_stream: no stream_endpoint with seid %d found", local_seid); 843 return AVDTP_SEID_DOES_NOT_EXIST; 844 } 845 846 if (stream_endpoint->l2cap_media_cid == 0){ 847 log_error("avdtp_abort_stream: no media connection for stream_endpoint with seid %d found", local_seid); 848 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 849 } 850 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->abort_stream) return ERROR_CODE_SUCCESS; 851 852 stream_endpoint->abort_stream = 1; 853 connection->local_seid = local_seid; 854 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 855 return ERROR_CODE_SUCCESS; 856 } 857 858 uint8_t avdtp_suspend_stream(uint16_t avdtp_cid, uint8_t local_seid, avdtp_context_t * context){ 859 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 860 if (!connection){ 861 log_error("avdtp_suspend_stream: no connection for signaling cid 0x%02x found", avdtp_cid); 862 return AVDTP_CONNECTION_DOES_NOT_EXIST; 863 } 864 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(local_seid, context); 865 if (!stream_endpoint) { 866 log_error("avdtp_suspend_stream: no stream_endpoint with seid %d found", local_seid); 867 return AVDTP_SEID_DOES_NOT_EXIST; 868 } 869 870 if (stream_endpoint->l2cap_media_cid == 0){ 871 log_error("avdtp_suspend_stream: no media connection for stream_endpoint with seid %d found", local_seid); 872 return AVDTP_MEDIA_CONNECTION_DOES_NOT_EXIST; 873 } 874 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->suspend_stream) return ERROR_CODE_SUCCESS; 875 876 stream_endpoint->suspend_stream = 1; 877 connection->local_seid = local_seid; 878 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 879 return ERROR_CODE_SUCCESS; 880 } 881 882 uint8_t avdtp_discover_stream_endpoints(uint16_t avdtp_cid, avdtp_context_t * context){ 883 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 884 if (!connection){ 885 log_error("avdtp_discover_stream_endpoints: no connection for signaling cid 0x%02x found", avdtp_cid); 886 return AVDTP_CONNECTION_DOES_NOT_EXIST; 887 } 888 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED || 889 connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) { 890 return AVDTP_CONNECTION_IN_WRONG_STATE; 891 } 892 893 connection->initiator_transaction_label++; 894 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS; 895 return avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 896 } 897 898 899 uint8_t avdtp_get_capabilities(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_context_t * context){ 900 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 901 if (!connection){ 902 log_error("No connection for AVDTP cid 0x%02x found", avdtp_cid); 903 return AVDTP_CONNECTION_DOES_NOT_EXIST; 904 } 905 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED || 906 connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) { 907 return AVDTP_CONNECTION_IN_WRONG_STATE; 908 } 909 910 connection->initiator_transaction_label++; 911 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES; 912 connection->remote_seid = remote_seid; 913 return avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 914 } 915 916 917 uint8_t avdtp_get_all_capabilities(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_context_t * context){ 918 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 919 if (!connection){ 920 log_error("No connection for AVDTP cid 0x%02x found", avdtp_cid); 921 return AVDTP_CONNECTION_DOES_NOT_EXIST; 922 } 923 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED || 924 connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) { 925 return AVDTP_CONNECTION_IN_WRONG_STATE; 926 } 927 928 connection->initiator_transaction_label++; 929 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES; 930 connection->remote_seid = remote_seid; 931 return avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 932 } 933 934 uint8_t avdtp_get_configuration(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_context_t * context){ 935 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 936 if (!connection){ 937 log_error("No connection for AVDTP cid 0x%02x found", avdtp_cid); 938 return AVDTP_CONNECTION_DOES_NOT_EXIST; 939 } 940 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED || 941 connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) { 942 return AVDTP_CONNECTION_IN_WRONG_STATE; 943 } 944 945 connection->initiator_transaction_label++; 946 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION; 947 connection->remote_seid = remote_seid; 948 return avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 949 } 950 951 uint8_t 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){ 952 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 953 if (!connection){ 954 log_error("No connection for AVDTP cid 0x%02x found", avdtp_cid); 955 return AVDTP_CONNECTION_DOES_NOT_EXIST; 956 } 957 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED || 958 connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) { 959 return AVDTP_CONNECTION_IN_WRONG_STATE; 960 } 961 962 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(local_seid, context); 963 if (!stream_endpoint) { 964 log_error("No initiator stream endpoint for seid %d", local_seid); 965 return AVDTP_STREAM_ENDPOINT_DOES_NOT_EXIST; 966 } 967 if (stream_endpoint->state >= AVDTP_STREAM_ENDPOINT_CONFIGURED){ 968 log_error("Ntream endpoint seid %d in wrong state", local_seid); 969 return AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE; 970 } 971 connection->active_stream_endpoint = (void*) stream_endpoint; 972 connection->is_configuration_initiated_locally = 1; 973 connection->is_initiator = 1; 974 975 connection->initiator_transaction_label++; 976 connection->remote_seid = remote_seid; 977 connection->local_seid = local_seid; 978 stream_endpoint->remote_configuration_bitmap = configured_services_bitmap; 979 stream_endpoint->remote_configuration = configuration; 980 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_SET_CONFIGURATION; 981 982 // cache media codec information for SBC 983 stream_endpoint->media_codec_type = configuration.media_codec.media_codec_type; 984 if (configuration.media_codec.media_codec_type == AVDTP_CODEC_SBC){ 985 stream_endpoint->media_type = configuration.media_codec.media_type; 986 memcpy(stream_endpoint->media_codec_sbc_info, configuration.media_codec.media_codec_information, 4); 987 } 988 return avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 989 } 990 991 uint8_t 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){ 992 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 993 if (!connection){ 994 log_error("No connection for AVDTP cid 0x%02x found", avdtp_cid); 995 return AVDTP_CONNECTION_DOES_NOT_EXIST; 996 } 997 //TODO: if opened only app capabilities, enable reconfigure for not opened 998 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED || 999 connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) { 1000 return AVDTP_CONNECTION_IN_WRONG_STATE; 1001 } 1002 1003 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(local_seid, context); 1004 if (!stream_endpoint) { 1005 log_error("avdtp_reconfigure: no initiator stream endpoint for seid %d", local_seid); 1006 return AVDTP_STREAM_ENDPOINT_DOES_NOT_EXIST; 1007 } 1008 1009 if (stream_endpoint->remote_sep_index == 0xFF){ 1010 log_error("avdtp_reconfigure: no associated remote sep"); 1011 return AVDTP_STREAM_ENDPOINT_IN_WRONG_STATE; 1012 } 1013 connection->initiator_transaction_label++; 1014 connection->remote_seid = remote_seid; 1015 connection->local_seid = local_seid; 1016 stream_endpoint->remote_configuration_bitmap = configured_services_bitmap; 1017 stream_endpoint->remote_configuration = configuration; 1018 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID; 1019 return avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 1020 } 1021 1022 uint8_t avdtp_remote_seps_num(uint16_t avdtp_cid, avdtp_context_t * context){ 1023 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 1024 if (!connection){ 1025 log_error("avdtp_suspend: no connection for AVDTP cid 0x%02x found", avdtp_cid); 1026 return 0; 1027 } 1028 return connection->remote_seps_num; 1029 } 1030 1031 avdtp_sep_t * avdtp_remote_sep(uint16_t avdtp_cid, uint8_t index, avdtp_context_t * context){ 1032 avdtp_connection_t * connection = avdtp_connection_for_avdtp_cid(avdtp_cid, context); 1033 if (!connection){ 1034 log_error("avdtp_suspend: no connection for AVDTP cid 0x%02x found", avdtp_cid); 1035 return NULL; 1036 } 1037 if (index == AVDTP_INVALID_SEP_INDEX) return NULL; 1038 if (index >= connection->remote_seps_num || index >= AVDTP_MAX_NUM_SEPS) return NULL; 1039 return &connection->remote_seps[index]; 1040 } 1041 1042 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){ 1043 UNUSED(packet_size); 1044 if (storage_size < 4) { 1045 log_error("storage must have 4 bytes"); 1046 return; 1047 } 1048 uint8_t sampling_frequency = avdtp_choose_sbc_sampling_frequency(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_sampling_frequency_bitmap(packet)); 1049 uint8_t channel_mode = avdtp_choose_sbc_channel_mode(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_channel_mode_bitmap(packet)); 1050 uint8_t block_length = avdtp_choose_sbc_block_length(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_block_length_bitmap(packet)); 1051 uint8_t subbands = avdtp_choose_sbc_subbands(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_subbands_bitmap(packet)); 1052 1053 uint8_t allocation_method = avdtp_choose_sbc_allocation_method(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_allocation_method_bitmap(packet)); 1054 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)); 1055 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)); 1056 1057 config_storage[0] = (sampling_frequency << 4) | channel_mode; 1058 config_storage[1] = (block_length << 4) | (subbands << 2) | allocation_method; 1059 config_storage[2] = min_bitpool_value; 1060 config_storage[3] = max_bitpool_value; 1061 1062 stream_endpoint->remote_configuration_bitmap = store_bit16(stream_endpoint->remote_configuration_bitmap, AVDTP_MEDIA_CODEC, 1); 1063 stream_endpoint->remote_configuration.media_codec.media_type = AVDTP_AUDIO; 1064 stream_endpoint->remote_configuration.media_codec.media_codec_type = AVDTP_CODEC_SBC; 1065 stream_endpoint->remote_configuration.media_codec.media_codec_information_len = storage_size; 1066 stream_endpoint->remote_configuration.media_codec.media_codec_information = config_storage; 1067 } 1068 1069 uint8_t avdtp_choose_sbc_channel_mode(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_channel_mode_bitmap){ 1070 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1071 uint8_t channel_mode_bitmap = (media_codec[0] & 0x0F) & remote_channel_mode_bitmap; 1072 1073 uint8_t channel_mode = AVDTP_SBC_STEREO; 1074 if (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO){ 1075 channel_mode = AVDTP_SBC_JOINT_STEREO; 1076 } else if (channel_mode_bitmap & AVDTP_SBC_STEREO){ 1077 channel_mode = AVDTP_SBC_STEREO; 1078 } else if (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL){ 1079 channel_mode = AVDTP_SBC_DUAL_CHANNEL; 1080 } else if (channel_mode_bitmap & AVDTP_SBC_MONO){ 1081 channel_mode = AVDTP_SBC_MONO; 1082 } 1083 return channel_mode; 1084 } 1085 1086 uint8_t avdtp_choose_sbc_allocation_method(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_allocation_method_bitmap){ 1087 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1088 uint8_t allocation_method_bitmap = (media_codec[1] & 0x03) & remote_allocation_method_bitmap; 1089 1090 uint8_t allocation_method = AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS; 1091 if (allocation_method_bitmap & AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS){ 1092 allocation_method = AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS; 1093 } else if (allocation_method_bitmap & AVDTP_SBC_ALLOCATION_METHOD_SNR){ 1094 allocation_method = AVDTP_SBC_ALLOCATION_METHOD_SNR; 1095 } 1096 return allocation_method; 1097 } 1098 1099 uint8_t avdtp_stream_endpoint_seid(avdtp_stream_endpoint_t * stream_endpoint){ 1100 if (!stream_endpoint) return 0; 1101 return stream_endpoint->sep.seid; 1102 } 1103 uint8_t avdtp_choose_sbc_subbands(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_subbands_bitmap){ 1104 if (!stream_endpoint) return 0; 1105 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1106 uint8_t subbands_bitmap = ((media_codec[1] >> 2) & 0x03) & remote_subbands_bitmap; 1107 1108 uint8_t subbands = AVDTP_SBC_SUBBANDS_8; 1109 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){ 1110 subbands = AVDTP_SBC_SUBBANDS_8; 1111 } else if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){ 1112 subbands = AVDTP_SBC_SUBBANDS_4; 1113 } 1114 return subbands; 1115 } 1116 1117 uint8_t avdtp_choose_sbc_block_length(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_block_length_bitmap){ 1118 if (!stream_endpoint) return 0; 1119 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1120 uint8_t block_length_bitmap = (media_codec[1] >> 4) & remote_block_length_bitmap; 1121 1122 uint8_t block_length = AVDTP_SBC_BLOCK_LENGTH_16; 1123 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){ 1124 block_length = AVDTP_SBC_BLOCK_LENGTH_16; 1125 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){ 1126 block_length = AVDTP_SBC_BLOCK_LENGTH_12; 1127 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){ 1128 block_length = AVDTP_SBC_BLOCK_LENGTH_8; 1129 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){ 1130 block_length = AVDTP_SBC_BLOCK_LENGTH_4; 1131 } 1132 return block_length; 1133 } 1134 1135 uint8_t avdtp_choose_sbc_sampling_frequency(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_sampling_frequency_bitmap){ 1136 if (!stream_endpoint) return 0; 1137 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1138 uint8_t sampling_frequency_bitmap = (media_codec[0] >> 4) & remote_sampling_frequency_bitmap; 1139 1140 uint8_t sampling_frequency = AVDTP_SBC_44100; 1141 if (sampling_frequency_bitmap & AVDTP_SBC_48000){ 1142 sampling_frequency = AVDTP_SBC_48000; 1143 } else if (sampling_frequency_bitmap & AVDTP_SBC_44100){ 1144 sampling_frequency = AVDTP_SBC_44100; 1145 } else if (sampling_frequency_bitmap & AVDTP_SBC_32000){ 1146 sampling_frequency = AVDTP_SBC_32000; 1147 } else if (sampling_frequency_bitmap & AVDTP_SBC_16000){ 1148 sampling_frequency = AVDTP_SBC_16000; 1149 } 1150 return sampling_frequency; 1151 } 1152 1153 uint8_t avdtp_choose_sbc_max_bitpool_value(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_max_bitpool_value){ 1154 if (!stream_endpoint) return 0; 1155 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1156 return btstack_min(media_codec[3], remote_max_bitpool_value); 1157 } 1158 1159 uint8_t avdtp_choose_sbc_min_bitpool_value(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_min_bitpool_value){ 1160 if (!stream_endpoint) return 0; 1161 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 1162 return btstack_max(media_codec[2], remote_min_bitpool_value); 1163 } 1164