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 #include <unistd.h> 46 47 #include "btstack.h" 48 #include "avdtp.h" 49 #include "avdtp_util.h" 50 #include "avdtp_acceptor.h" 51 #include "avdtp_initiator.h" 52 53 static void (*handle_media_data)(avdtp_stream_endpoint_t * stream_endpoint, uint8_t *packet, uint16_t size); 54 55 void avdtp_register_media_transport_category(avdtp_stream_endpoint_t * stream_endpoint){ 56 if (!stream_endpoint){ 57 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 58 return; 59 } 60 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_TRANSPORT, 1); 61 stream_endpoint->sep.registered_service_categories = bitmap; 62 printf("registered services AVDTP_MEDIA_TRANSPORT(%d) %02x\n", AVDTP_MEDIA_TRANSPORT, stream_endpoint->sep.registered_service_categories); 63 } 64 65 void avdtp_register_reporting_category(avdtp_stream_endpoint_t * stream_endpoint){ 66 if (!stream_endpoint){ 67 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 68 return; 69 } 70 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_REPORTING, 1); 71 stream_endpoint->sep.registered_service_categories = bitmap; 72 } 73 74 void avdtp_register_delay_reporting_category(avdtp_stream_endpoint_t * stream_endpoint){ 75 if (!stream_endpoint){ 76 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 77 return; 78 } 79 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_DELAY_REPORTING, 1); 80 stream_endpoint->sep.registered_service_categories = bitmap; 81 } 82 83 void avdtp_register_recovery_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t maximum_recovery_window_size, uint8_t maximum_number_media_packets){ 84 if (!stream_endpoint){ 85 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 86 return; 87 } 88 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_RECOVERY, 1); 89 stream_endpoint->sep.registered_service_categories = bitmap; 90 stream_endpoint->sep.capabilities.recovery.recovery_type = 0x01; // 0x01 = RFC2733 91 stream_endpoint->sep.capabilities.recovery.maximum_recovery_window_size = maximum_recovery_window_size; 92 stream_endpoint->sep.capabilities.recovery.maximum_number_media_packets = maximum_number_media_packets; 93 } 94 95 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){ 96 if (!stream_endpoint){ 97 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 98 return; 99 } 100 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_CONTENT_PROTECTION, 1); 101 stream_endpoint->sep.registered_service_categories = bitmap; 102 stream_endpoint->sep.capabilities.content_protection.cp_type = cp_type; 103 stream_endpoint->sep.capabilities.content_protection.cp_type_value = cp_type_value; 104 stream_endpoint->sep.capabilities.content_protection.cp_type_value_len = cp_type_value_len; 105 } 106 107 void avdtp_register_header_compression_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t back_ch, uint8_t media, uint8_t recovery){ 108 if (!stream_endpoint){ 109 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 110 return; 111 } 112 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_HEADER_COMPRESSION, 1); 113 stream_endpoint->sep.registered_service_categories = bitmap; 114 stream_endpoint->sep.capabilities.header_compression.back_ch = back_ch; 115 stream_endpoint->sep.capabilities.header_compression.media = media; 116 stream_endpoint->sep.capabilities.header_compression.recovery = recovery; 117 } 118 119 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){ 120 if (!stream_endpoint){ 121 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 122 return; 123 } 124 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MEDIA_CODEC, 1); 125 stream_endpoint->sep.registered_service_categories = bitmap; 126 printf("registered services AVDTP_MEDIA_CODEC(%d) %02x\n", AVDTP_MEDIA_CODEC, stream_endpoint->sep.registered_service_categories); 127 stream_endpoint->sep.capabilities.media_codec.media_type = media_type; 128 stream_endpoint->sep.capabilities.media_codec.media_codec_type = media_codec_type; 129 stream_endpoint->sep.capabilities.media_codec.media_codec_information = media_codec_info; 130 stream_endpoint->sep.capabilities.media_codec.media_codec_information_len = media_codec_info_len; 131 } 132 133 void avdtp_register_multiplexing_category(avdtp_stream_endpoint_t * stream_endpoint, uint8_t fragmentation){ 134 if (!stream_endpoint){ 135 log_error("avdtp_register_media_transport_category: stream endpoint with given seid is not registered"); 136 return; 137 } 138 uint16_t bitmap = store_bit16(stream_endpoint->sep.registered_service_categories, AVDTP_MULTIPLEXING, 1); 139 stream_endpoint->sep.registered_service_categories = bitmap; 140 stream_endpoint->sep.capabilities.multiplexing_mode.fragmentation = fragmentation; 141 } 142 143 144 /* START: tracking can send now requests pro l2cap cid */ 145 void avdtp_handle_can_send_now(avdtp_connection_t * connection, uint16_t l2cap_cid, avdtp_context_t * context){ 146 if (connection->wait_to_send_acceptor){ 147 connection->wait_to_send_acceptor = 0; 148 avdtp_acceptor_stream_config_subsm_run(connection, context); 149 } else if (connection->wait_to_send_initiator){ 150 connection->wait_to_send_initiator = 0; 151 avdtp_initiator_stream_config_subsm_run(connection, context); 152 } else if (connection->wait_to_send_self){ 153 connection->wait_to_send_self = 0; 154 if (connection->disconnect){ 155 btstack_linked_list_iterator_t it; 156 btstack_linked_list_iterator_init(&it, &context->stream_endpoints); 157 while (btstack_linked_list_iterator_has_next(&it)){ 158 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 159 if (stream_endpoint->connection == connection){ 160 if (stream_endpoint->state >= AVDTP_STREAM_ENDPOINT_OPENED && stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED){ 161 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_DISCONNECTED; 162 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 163 l2cap_disconnect(stream_endpoint->l2cap_media_cid, 0); 164 return; 165 } 166 } 167 } 168 connection->disconnect = 0; 169 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED; 170 l2cap_disconnect(connection->l2cap_signaling_cid, 0); 171 return; 172 } 173 } 174 175 // re-register 176 int more_to_send = connection->wait_to_send_acceptor || connection->wait_to_send_initiator || connection->wait_to_send_self; 177 if (more_to_send){ 178 l2cap_request_can_send_now_event(l2cap_cid); 179 } 180 } 181 /* END: tracking can send now requests pro l2cap cid */ 182 183 avdtp_connection_t * avdtp_create_connection(bd_addr_t remote_addr, avdtp_context_t * context){ 184 avdtp_connection_t * connection = btstack_memory_avdtp_connection_get(); 185 memset(connection, 0, sizeof(avdtp_connection_t)); 186 connection->state = AVDTP_SIGNALING_CONNECTION_IDLE; 187 connection->initiator_transaction_label++; 188 memcpy(connection->remote_addr, remote_addr, 6); 189 btstack_linked_list_add(&context->connections, (btstack_linked_item_t *) connection); 190 return connection; 191 } 192 193 avdtp_stream_endpoint_t * avdtp_create_stream_endpoint(avdtp_sep_type_t sep_type, avdtp_media_type_t media_type, avdtp_context_t * context){ 194 avdtp_stream_endpoint_t * stream_endpoint = btstack_memory_avdtp_stream_endpoint_get(); 195 memset(stream_endpoint, 0, sizeof(avdtp_stream_endpoint_t)); 196 context->stream_endpoints_id_counter++; 197 stream_endpoint->sep.seid = context->stream_endpoints_id_counter; 198 stream_endpoint->sep.media_type = media_type; 199 stream_endpoint->sep.type = sep_type; 200 btstack_linked_list_add(&context->stream_endpoints, (btstack_linked_item_t *) stream_endpoint); 201 return stream_endpoint; 202 } 203 204 205 static void handle_l2cap_data_packet_for_signaling_connection(avdtp_connection_t * connection, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 206 int offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size); 207 switch (connection->signaling_packet.message_type){ 208 case AVDTP_CMD_MSG: 209 avdtp_acceptor_stream_config_subsm(connection, packet, size, offset, context); 210 break; 211 default: 212 avdtp_initiator_stream_config_subsm(connection, packet, size, offset, context); 213 break; 214 } 215 } 216 217 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){ 218 uint16_t local_cid; 219 switch (packet_type){ 220 case L2CAP_DATA_PACKET:{ 221 int offset = avdtp_read_signaling_header(&connection->signaling_packet, packet, size); 222 if (connection->signaling_packet.message_type == AVDTP_CMD_MSG){ 223 avdtp_acceptor_stream_config_subsm(connection, packet, size, offset, context); 224 } else { 225 avdtp_initiator_stream_config_subsm(connection, packet, size, offset, context); 226 } 227 break; 228 } 229 case HCI_EVENT_PACKET: 230 switch (event){ 231 case L2CAP_EVENT_CHANNEL_OPENED: 232 if (stream_endpoint->l2cap_media_cid == 0){ 233 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED) return; 234 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_OPENED; 235 stream_endpoint->connection = connection; 236 stream_endpoint->l2cap_media_cid = l2cap_event_channel_opened_get_local_cid(packet); 237 stream_endpoint->media_con_handle = l2cap_event_channel_opened_get_handle(packet); 238 printf(" -> AVDTP_STREAM_ENDPOINT_OPENED, media con handle 0x%02x, l2cap_media_cid 0x%02x\n", stream_endpoint->media_con_handle, stream_endpoint->l2cap_media_cid); 239 avdtp_streaming_emit_connection_established(context->avdtp_callback, connection->l2cap_signaling_cid, stream_endpoint->sep.seid, connection->remote_seps[stream_endpoint->remote_sep_index].seid, 0); 240 break; 241 } 242 break; 243 case L2CAP_EVENT_CHANNEL_CLOSED: 244 local_cid = l2cap_event_channel_closed_get_local_cid(packet); 245 if (stream_endpoint->l2cap_media_cid == local_cid){ 246 stream_endpoint->l2cap_media_cid = 0; 247 printf(" -> L2CAP_EVENT_CHANNEL_CLOSED: AVDTP_STREAM_ENDPOINT_IDLE\n"); 248 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_IDLE; 249 stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; 250 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; 251 stream_endpoint->remote_sep_index = 0; 252 break; 253 } 254 if (stream_endpoint->l2cap_recovery_cid == local_cid){ 255 log_info(" -> L2CAP_EVENT_CHANNEL_CLOSED recovery cid 0x%0x", local_cid); 256 stream_endpoint->l2cap_recovery_cid = 0; 257 break; 258 } 259 260 if (stream_endpoint->l2cap_reporting_cid == local_cid){ 261 log_info("L2CAP_EVENT_CHANNEL_CLOSED reporting cid 0x%0x", local_cid); 262 stream_endpoint->l2cap_reporting_cid = 0; 263 break; 264 } 265 break; 266 default: 267 break; 268 } 269 break; 270 default: 271 break; 272 } 273 } 274 275 void avdtp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size, avdtp_context_t * context){ 276 bd_addr_t event_addr; 277 hci_con_handle_t con_handle; 278 uint16_t psm; 279 uint16_t local_cid; 280 avdtp_stream_endpoint_t * stream_endpoint = NULL; 281 avdtp_connection_t * connection = NULL; 282 btstack_linked_list_t * avdtp_connections = &context->connections; 283 btstack_linked_list_t * stream_endpoints = &context->stream_endpoints; 284 handle_media_data = context->handle_media_data; 285 // printf("avdtp_packet_handler packet type %02x, event %02x \n", packet_type, hci_event_packet_get_type(packet)); 286 switch (packet_type) { 287 case L2CAP_DATA_PACKET: 288 connection = avdtp_connection_for_l2cap_signaling_cid(channel, context); 289 if (connection){ 290 handle_l2cap_data_packet_for_signaling_connection(connection, packet, size, context); 291 break; 292 } 293 294 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(channel, context); 295 if (!stream_endpoint){ 296 if (!connection) break; 297 handle_l2cap_data_packet_for_signaling_connection(connection, packet, size, context); 298 break; 299 } 300 301 if (channel == stream_endpoint->connection->l2cap_signaling_cid){ 302 stream_endpoint_state_machine(stream_endpoint->connection, stream_endpoint, L2CAP_DATA_PACKET, 0, packet, size, context); 303 break; 304 } 305 306 if (channel == stream_endpoint->l2cap_media_cid){ 307 (*handle_media_data)(stream_endpoint, packet, size); 308 break; 309 } 310 311 if (channel == stream_endpoint->l2cap_reporting_cid){ 312 // TODO 313 printf("L2CAP_DATA_PACKET for reporting: NOT IMPLEMENTED\n"); 314 } else if (channel == stream_endpoint->l2cap_recovery_cid){ 315 // TODO 316 printf("L2CAP_DATA_PACKET for recovery: NOT IMPLEMENTED\n"); 317 } else { 318 log_error("avdtp packet handler L2CAP_DATA_PACKET: local cid 0x%02x not found", channel); 319 } 320 break; 321 322 case HCI_EVENT_PACKET: 323 switch (hci_event_packet_get_type(packet)) { 324 case L2CAP_EVENT_INCOMING_CONNECTION: 325 l2cap_event_incoming_connection_get_address(packet, event_addr); 326 local_cid = l2cap_event_incoming_connection_get_local_cid(packet); 327 328 connection = avdtp_connection_for_bd_addr(event_addr, context); 329 if (!connection){ 330 connection = avdtp_create_connection(event_addr, context); 331 connection->state = AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED; 332 l2cap_accept_connection(local_cid); 333 break; 334 } 335 336 stream_endpoint = avdtp_stream_endpoint_for_seid(connection->query_seid, context); 337 if (!stream_endpoint) { 338 printf("L2CAP_EVENT_INCOMING_CONNECTION no streamendpoint found for seid %d\n", connection->query_seid); 339 break; 340 } 341 342 if (stream_endpoint->l2cap_media_cid == 0){ 343 if (stream_endpoint->state != AVDTP_STREAM_ENDPOINT_W4_L2CAP_FOR_MEDIA_CONNECTED) break; 344 l2cap_accept_connection(local_cid); 345 break; 346 } 347 break; 348 349 case L2CAP_EVENT_CHANNEL_OPENED: 350 // inform about new l2cap connection 351 l2cap_event_channel_opened_get_address(packet, event_addr); 352 if (l2cap_event_channel_opened_get_status(packet)){ 353 log_error("L2CAP connection to connection %s failed. status code 0x%02x", 354 bd_addr_to_str(event_addr), l2cap_event_channel_opened_get_status(packet)); 355 break; 356 } 357 psm = l2cap_event_channel_opened_get_psm(packet); 358 if (psm != BLUETOOTH_PROTOCOL_AVDTP){ 359 log_error("unexpected PSM - Not implemented yet, avdtp sink: L2CAP_EVENT_CHANNEL_OPENED"); 360 return; 361 } 362 363 con_handle = l2cap_event_channel_opened_get_handle(packet); 364 local_cid = l2cap_event_channel_opened_get_local_cid(packet); 365 366 // printf("L2CAP_EVENT_CHANNEL_OPENED: Channel successfully opened: %s, handle 0x%02x, psm 0x%02x, local cid 0x%02x, remote cid 0x%02x\n", 367 // bd_addr_to_str(event_addr), con_handle, psm, local_cid, l2cap_event_channel_opened_get_remote_cid(packet)); 368 369 if (psm != BLUETOOTH_PROTOCOL_AVDTP) break; 370 371 connection = avdtp_connection_for_bd_addr(event_addr, context); 372 if (!connection) break; 373 374 if (connection->l2cap_signaling_cid == 0) { 375 if (connection->state != AVDTP_SIGNALING_CONNECTION_W4_L2CAP_CONNECTED) break; 376 connection->l2cap_signaling_cid = local_cid; 377 connection->con_handle = con_handle; 378 connection->query_seid = 0; 379 connection->state = AVDTP_SIGNALING_CONNECTION_OPENED; 380 printf(" -> AVDTP_SIGNALING_CONNECTION_OPENED, connection %p\n", connection); 381 avdtp_signaling_emit_connection_established(context->avdtp_callback, connection->l2cap_signaling_cid, event_addr, 0); 382 break; 383 } 384 385 stream_endpoint = avdtp_stream_endpoint_for_seid(connection->query_seid, context); 386 if (!stream_endpoint){ 387 printf("L2CAP_EVENT_CHANNEL_OPENED: stream_endpoint not found"); 388 return; 389 } 390 stream_endpoint_state_machine(connection, stream_endpoint, HCI_EVENT_PACKET, L2CAP_EVENT_CHANNEL_OPENED, packet, size, context); 391 break; 392 393 case L2CAP_EVENT_CHANNEL_CLOSED: 394 // data: event (8), len(8), channel (16) 395 local_cid = l2cap_event_channel_closed_get_local_cid(packet); 396 connection = avdtp_connection_for_l2cap_signaling_cid(local_cid, context); 397 printf(" -> L2CAP_EVENT_CHANNEL_CLOSED signaling cid 0x%0x\n", local_cid); 398 399 if (connection){ 400 printf(" -> AVDTP_STREAM_ENDPOINT_IDLE, connection closed\n"); 401 btstack_linked_list_remove(avdtp_connections, (btstack_linked_item_t*) connection); 402 btstack_linked_list_iterator_t it; 403 btstack_linked_list_iterator_init(&it, stream_endpoints); 404 while (btstack_linked_list_iterator_has_next(&it)){ 405 avdtp_stream_endpoint_t * _stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 406 avdtp_initialize_stream_endpoint(_stream_endpoint); 407 } 408 break; 409 } 410 411 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(local_cid, context); 412 if (!stream_endpoint) return; 413 414 stream_endpoint_state_machine(connection, stream_endpoint, HCI_EVENT_PACKET, L2CAP_EVENT_CHANNEL_CLOSED, packet, size, context); 415 break; 416 417 case HCI_EVENT_DISCONNECTION_COMPLETE: 418 break; 419 420 case L2CAP_EVENT_CAN_SEND_NOW: 421 connection = avdtp_connection_for_l2cap_signaling_cid(channel, context); 422 if (!connection) { 423 stream_endpoint = avdtp_stream_endpoint_for_l2cap_cid(channel, context); 424 if (!stream_endpoint->connection) break; 425 connection = stream_endpoint->connection; 426 } 427 avdtp_handle_can_send_now(connection, channel, context); 428 break; 429 default: 430 printf("unknown HCI event type %02x\n", hci_event_packet_get_type(packet)); 431 break; 432 } 433 break; 434 435 default: 436 // other packet type 437 break; 438 } 439 } 440 441 void avdtp_disconnect(uint16_t avdtp_cid, avdtp_context_t * context){ 442 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 443 if (!connection) return; 444 if (connection->state == AVDTP_SIGNALING_CONNECTION_IDLE) return; 445 if (connection->state == AVDTP_SIGNALING_CONNECTION_W4_L2CAP_DISCONNECTED) return; 446 447 connection->disconnect = 1; 448 avdtp_request_can_send_now_self(connection, connection->l2cap_signaling_cid); 449 } 450 451 void avdtp_open_stream(uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, avdtp_context_t * context){ 452 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 453 if (!connection){ 454 printf("avdtp_media_connect: no connection for signaling cid 0x%02x found\n", avdtp_cid); 455 return; 456 } 457 if (avdtp_find_remote_sep(connection, acp_seid) == 0xFF){ 458 printf("avdtp_media_connect: no remote sep for seid %d found\n", acp_seid); 459 return; 460 } 461 462 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) { 463 printf("avdtp_media_connect: wrong connection state %d\n", connection->state); 464 return; 465 } 466 467 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(int_seid, context); 468 if (!stream_endpoint) { 469 printf("avdtp_media_connect: no stream_endpoint with seid %d found\n", int_seid); 470 return; 471 } 472 473 if (stream_endpoint->state < AVDTP_STREAM_ENDPOINT_CONFIGURED) return; 474 if (stream_endpoint->remote_sep_index == 0xFF) return; 475 476 connection->initiator_transaction_label++; 477 connection->acp_seid = acp_seid; 478 connection->int_seid = stream_endpoint->sep.seid; 479 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_OPEN_STREAM; 480 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_W2_REQUEST_OPEN_STREAM; 481 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 482 } 483 484 void avdtp_start_stream(uint8_t int_seid, avdtp_context_t * context){ 485 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(int_seid, context); 486 if (!stream_endpoint) { 487 printf("avdtp_start_stream: no stream_endpoint with seid %d found\n", int_seid); 488 return; 489 } 490 avdtp_connection_t * connection = stream_endpoint->connection; 491 if (!connection){ 492 printf("avdtp_start_stream: no connection for seid %d found\n",stream_endpoint->sep.seid); 493 return; 494 } 495 496 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->start_stream) return; 497 stream_endpoint->start_stream = 1; 498 connection->int_seid = int_seid; 499 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 500 } 501 502 void avdtp_stop_stream(uint8_t int_seid, avdtp_context_t * context){ 503 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(int_seid, context); 504 if (!stream_endpoint) { 505 printf("avdtp_stop_stream: no stream_endpoint with seid %d found\n", int_seid); 506 return; 507 } 508 avdtp_connection_t * connection = stream_endpoint->connection; 509 if (!connection){ 510 printf("avdtp_stop_stream: no connection for seid %d found\n",stream_endpoint->sep.seid); 511 return; 512 } 513 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->stop_stream) return; 514 stream_endpoint->stop_stream = 1; 515 connection->int_seid = int_seid; 516 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 517 } 518 519 void avdtp_abort_stream(uint8_t int_seid, avdtp_context_t * context){ 520 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(int_seid, context); 521 if (!stream_endpoint) { 522 printf("avdtp_abort_stream: no stream_endpoint with seid %d found\n", int_seid); 523 return; 524 } 525 avdtp_connection_t * connection = stream_endpoint->connection; 526 if (!connection){ 527 printf("avdtp_abort_stream: no connection for seid %d found\n",stream_endpoint->sep.seid); 528 return; 529 } 530 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->abort_stream) return; 531 stream_endpoint->abort_stream = 1; 532 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 533 } 534 535 void avdtp_suspend_stream(uint8_t int_seid, avdtp_context_t * context){ 536 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_with_seid(int_seid, context); 537 if (!stream_endpoint) { 538 printf("avdtp_abort_stream: no stream_endpoint with seid %d found\n", int_seid); 539 return; 540 } 541 avdtp_connection_t * connection = stream_endpoint->connection; 542 if (!connection){ 543 printf("avdtp_abort_stream: no connection for seid %d found\n",stream_endpoint->sep.seid); 544 return; 545 } 546 if (stream_endpoint->remote_sep_index == 0xFF || stream_endpoint->suspend_stream) return; 547 stream_endpoint->suspend_stream = 1; 548 connection->int_seid = int_seid; 549 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 550 } 551 552 void avdtp_discover_stream_endpoints(uint16_t avdtp_cid, avdtp_context_t * context){ 553 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 554 if (!connection){ 555 printf("avdtp_discover_stream_endpoints: no connection for signaling cid 0x%02x found\n", avdtp_cid); 556 return; 557 } 558 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 559 560 switch (connection->initiator_connection_state){ 561 case AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE: 562 connection->initiator_transaction_label++; 563 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_DISCOVER_SEPS; 564 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 565 break; 566 default: 567 printf("avdtp_discover_stream_endpoints: wrong state\n"); 568 break; 569 } 570 } 571 572 573 void avdtp_get_capabilities(uint16_t avdtp_cid, uint8_t acp_seid, avdtp_context_t * context){ 574 printf("avdtp_get_capabilities: acp_seid %d\n", acp_seid); 575 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 576 if (!connection){ 577 printf("avdtp_get_capabilities: no connection for signaling cid 0x%02x found\n", avdtp_cid); 578 return; 579 } 580 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 581 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 582 connection->initiator_transaction_label++; 583 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CAPABILITIES; 584 connection->acp_seid = acp_seid; 585 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 586 } 587 588 589 void avdtp_get_all_capabilities(uint16_t avdtp_cid, uint8_t acp_seid, avdtp_context_t * context){ 590 printf("avdtp_get_all_capabilities: acp_seid %d\n", acp_seid); 591 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 592 if (!connection){ 593 printf("avdtp_get_all_capabilities: no connection for signaling cid 0x%02x found\n", avdtp_cid); 594 return; 595 } 596 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 597 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 598 connection->initiator_transaction_label++; 599 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_ALL_CAPABILITIES; 600 connection->acp_seid = acp_seid; 601 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 602 } 603 604 void avdtp_get_configuration(uint16_t avdtp_cid, uint8_t acp_seid, avdtp_context_t * context){ 605 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 606 if (!connection){ 607 printf("avdtp_get_configuration: no connection for signaling cid 0x%02x found\n", avdtp_cid); 608 return; 609 } 610 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 611 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 612 connection->initiator_transaction_label++; 613 connection->initiator_connection_state = AVDTP_SIGNALING_CONNECTION_INITIATOR_W2_GET_CONFIGURATION; 614 connection->acp_seid = acp_seid; 615 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 616 } 617 618 void avdtp_set_configuration(uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, uint16_t configured_services_bitmap, avdtp_capabilities_t configuration, avdtp_context_t * context){ 619 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 620 if (!connection){ 621 log_error("avdtp_set_configuration: no connection for signaling cid 0x%02x found\n", avdtp_cid); 622 return; 623 } 624 if (connection->state != AVDTP_SIGNALING_CONNECTION_OPENED) return; 625 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 626 627 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(int_seid, context); 628 if (!stream_endpoint) { 629 log_error("avdtp_set_configuration: no initiator stream endpoint for seid %d\n", int_seid); 630 return; 631 } 632 // printf("avdtp_set_configuration int seid %d, acp seid %d\n", int_seid, acp_seid); 633 634 connection->initiator_transaction_label++; 635 connection->acp_seid = acp_seid; 636 connection->int_seid = int_seid; 637 stream_endpoint->remote_capabilities_bitmap = configured_services_bitmap; 638 stream_endpoint->remote_capabilities = configuration; 639 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_SET_CONFIGURATION; 640 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 641 } 642 643 void avdtp_reconfigure(uint16_t avdtp_cid, uint8_t int_seid, uint8_t acp_seid, uint16_t configured_services_bitmap, avdtp_capabilities_t configuration, avdtp_context_t * context){ 644 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 645 if (!connection){ 646 printf("avdtp_reconfigure: no connection for signaling cid 0x%02x found\n", avdtp_cid); 647 return; 648 } 649 //TODO: if opened only app capabilities, enable reconfigure for not opened 650 if (connection->state < AVDTP_SIGNALING_CONNECTION_OPENED) return; 651 if (connection->initiator_connection_state != AVDTP_SIGNALING_CONNECTION_INITIATOR_IDLE) return; 652 653 avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(int_seid, context); 654 if (!stream_endpoint) { 655 log_error("avdtp_reconfigure: no initiator stream endpoint for seid %d\n", int_seid); 656 return; 657 } 658 659 if (stream_endpoint->remote_sep_index == 0xFF){ 660 log_error("avdtp_reconfigure: no associated remote sep\n"); 661 return; 662 } 663 664 connection->initiator_transaction_label++; 665 connection->acp_seid = acp_seid; 666 connection->int_seid = stream_endpoint->sep.seid; 667 stream_endpoint->remote_capabilities_bitmap = configured_services_bitmap; 668 stream_endpoint->remote_capabilities = configuration; 669 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_W2_RECONFIGURE_STREAM_WITH_SEID; 670 avdtp_request_can_send_now_initiator(connection, connection->l2cap_signaling_cid); 671 } 672 673 uint8_t avdtp_remote_seps_num(uint16_t avdtp_cid, avdtp_context_t * context){ 674 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 675 if (!connection){ 676 printf("avdtp_suspend: no connection for signaling cid 0x%02x found\n", avdtp_cid); 677 return 0; 678 } 679 return connection->remote_seps_num; 680 } 681 682 avdtp_sep_t * avdtp_remote_sep(uint16_t avdtp_cid, uint8_t index, avdtp_context_t * context){ 683 avdtp_connection_t * connection = avdtp_connection_for_l2cap_signaling_cid(avdtp_cid, context); 684 if (!connection){ 685 printf("avdtp_suspend: no connection for signaling cid 0x%02x found\n", avdtp_cid); 686 return NULL; 687 } 688 return &connection->remote_seps[index]; 689 } 690 691 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){ 692 UNUSED(packet_size); 693 if (storage_size < 4) { 694 printf("storage must have 4 bytes\n"); 695 return; 696 } 697 uint8_t sampling_frequency = avdtp_choose_sbc_sampling_frequency(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_sampling_frequency_bitmap(packet)); 698 uint8_t channel_mode = avdtp_choose_sbc_channel_mode(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_channel_mode_bitmap(packet)); 699 uint8_t block_length = avdtp_choose_sbc_block_length(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_block_length_bitmap(packet)); 700 uint8_t subbands = avdtp_choose_sbc_subbands(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_subbands_bitmap(packet)); 701 702 uint8_t allocation_method = avdtp_choose_sbc_allocation_method(stream_endpoint, avdtp_subevent_signaling_media_codec_sbc_capability_get_allocation_method_bitmap(packet)); 703 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)); 704 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)); 705 706 config_storage[0] = (sampling_frequency << 4) | channel_mode; 707 config_storage[1] = (block_length << 4) | (subbands << 2) | allocation_method; 708 config_storage[2] = min_bitpool_value; 709 config_storage[3] = max_bitpool_value; 710 711 stream_endpoint->remote_configuration_bitmap = store_bit16(stream_endpoint->remote_configuration_bitmap, AVDTP_MEDIA_CODEC, 1); 712 stream_endpoint->remote_configuration.media_codec.media_type = AVDTP_AUDIO; 713 stream_endpoint->remote_configuration.media_codec.media_codec_type = AVDTP_CODEC_SBC; 714 stream_endpoint->remote_configuration.media_codec.media_codec_information_len = storage_size; 715 stream_endpoint->remote_configuration.media_codec.media_codec_information = config_storage; 716 } 717 718 uint8_t avdtp_choose_sbc_channel_mode(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_channel_mode_bitmap){ 719 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 720 uint8_t channel_mode_bitmap = (media_codec[0] & 0x0F) & remote_channel_mode_bitmap; 721 722 uint8_t channel_mode = AVDTP_SBC_STEREO; 723 if (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO){ 724 channel_mode = AVDTP_SBC_JOINT_STEREO; 725 } else if (channel_mode_bitmap & AVDTP_SBC_STEREO){ 726 channel_mode = AVDTP_SBC_STEREO; 727 } else if (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL){ 728 channel_mode = AVDTP_SBC_DUAL_CHANNEL; 729 } else if (channel_mode_bitmap & AVDTP_SBC_MONO){ 730 channel_mode = AVDTP_SBC_MONO; 731 } 732 return channel_mode; 733 } 734 735 uint8_t avdtp_choose_sbc_allocation_method(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_allocation_method_bitmap){ 736 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 737 uint8_t allocation_method_bitmap = (media_codec[1] & 0x03) & remote_allocation_method_bitmap; 738 739 uint8_t allocation_method = AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS; 740 if (allocation_method_bitmap & AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS){ 741 allocation_method = AVDTP_SBC_ALLOCATION_METHOD_LOUDNESS; 742 } else if (allocation_method_bitmap & AVDTP_SBC_ALLOCATION_METHOD_SNR){ 743 allocation_method = AVDTP_SBC_ALLOCATION_METHOD_SNR; 744 } 745 return allocation_method; 746 } 747 748 uint8_t avdtp_stream_endpoint_seid(avdtp_stream_endpoint_t * stream_endpoint){ 749 if (!stream_endpoint) return 0; 750 return stream_endpoint->sep.seid; 751 } 752 uint8_t avdtp_choose_sbc_subbands(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_subbands_bitmap){ 753 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 754 uint8_t subbands_bitmap = ((media_codec[1] >> 2) & 0x03) & remote_subbands_bitmap; 755 756 uint8_t subbands = AVDTP_SBC_SUBBANDS_8; 757 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){ 758 subbands = AVDTP_SBC_SUBBANDS_8; 759 } else if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){ 760 subbands = AVDTP_SBC_SUBBANDS_4; 761 } 762 return subbands; 763 } 764 765 uint8_t avdtp_choose_sbc_block_length(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_block_length_bitmap){ 766 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 767 uint8_t block_length_bitmap = (media_codec[1] >> 4) & remote_block_length_bitmap; 768 769 uint8_t block_length = AVDTP_SBC_BLOCK_LENGTH_16; 770 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){ 771 block_length = AVDTP_SBC_BLOCK_LENGTH_16; 772 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){ 773 block_length = AVDTP_SBC_BLOCK_LENGTH_12; 774 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){ 775 block_length = AVDTP_SBC_BLOCK_LENGTH_8; 776 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){ 777 block_length = AVDTP_SBC_BLOCK_LENGTH_4; 778 } 779 return block_length; 780 } 781 782 uint8_t avdtp_choose_sbc_sampling_frequency(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_sampling_frequency_bitmap){ 783 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 784 uint8_t sampling_frequency_bitmap = (media_codec[0] >> 4) & remote_sampling_frequency_bitmap; 785 786 uint8_t sampling_frequency = AVDTP_SBC_44100; 787 if (sampling_frequency_bitmap & AVDTP_SBC_48000){ 788 sampling_frequency = AVDTP_SBC_48000; 789 } else if (sampling_frequency_bitmap & AVDTP_SBC_44100){ 790 sampling_frequency = AVDTP_SBC_44100; 791 } else if (sampling_frequency_bitmap & AVDTP_SBC_32000){ 792 sampling_frequency = AVDTP_SBC_32000; 793 } else if (sampling_frequency_bitmap & AVDTP_SBC_16000){ 794 sampling_frequency = AVDTP_SBC_16000; 795 } 796 return sampling_frequency; 797 } 798 799 uint8_t avdtp_choose_sbc_max_bitpool_value(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_max_bitpool_value){ 800 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 801 return btstack_min(media_codec[3], remote_max_bitpool_value); 802 } 803 804 uint8_t avdtp_choose_sbc_min_bitpool_value(avdtp_stream_endpoint_t * stream_endpoint, uint8_t remote_min_bitpool_value){ 805 uint8_t * media_codec = stream_endpoint->sep.capabilities.media_codec.media_codec_information; 806 return btstack_max(media_codec[2], remote_min_bitpool_value); 807 }