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