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 BLUEKITCHEN 24 * GMBH 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_util.c" 39 40 #include <stdint.h> 41 #include <string.h> 42 43 #include "classic/avdtp.h" 44 #include "classic/avdtp_util.h" 45 46 #include "btstack_debug.h" 47 #include "l2cap.h" 48 49 /* 50 51 List of AVDTP_SUBEVENTs sorted by packet handler 52 53 54 Sink + Source: 55 - AVDTP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED 56 - AVDTP_SUBEVENT_SIGNALING_CONNECTION_RELEASED 57 - AVDTP_SUBEVENT_SIGNALING_SEP_FOUND 58 - AVDTP_SUBEVENT_SIGNALING_ACCEPT 59 - AVDTP_SUBEVENT_SIGNALING_REJECT 60 - AVDTP_SUBEVENT_SIGNALING_GENERAL_REJECT 61 - AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CAPABILITY 62 - AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CAPABILITY 63 - AVDTP_SUBEVENT_SIGNALING_MEDIA_TRANSPORT_CAPABILITY 64 - AVDTP_SUBEVENT_SIGNALING_REPORTING_CAPABILITY 65 - AVDTP_SUBEVENT_SIGNALING_RECOVERY_CAPABILITY 66 - AVDTP_SUBEVENT_SIGNALING_CONTENT_PROTECTION_CAPABILITY 67 - AVDTP_SUBEVENT_SIGNALING_MULTIPLEXING_CAPABILITY 68 - AVDTP_SUBEVENT_SIGNALING_DELAY_REPORTING_CAPABILITY 69 - AVDTP_SUBEVENT_SIGNALING_HEADER_COMPRESSION_CAPABILITY 70 - AVDTP_SUBEVENT_SIGNALING_CAPABILITIES_DONE 71 - AVDTP_SUBEVENT_SIGNALING_SEP_DICOVERY_DONE 72 73 Source: 74 - AVDTP_SUBEVENT_SIGNALING_DELAY_REPORT 75 76 Sink or Source based on SEP Type: 77 - AVDTP_SUBEVENT_STREAMING_CONNECTION_ESTABLISHED 78 - AVDTP_SUBEVENT_STREAMING_CONNECTION_RELEASED 79 - AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW 80 - AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION 81 - AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION 82 83 */ 84 85 static const char * avdtp_si_name[] = { 86 "ERROR", 87 "AVDTP_SI_DISCOVER", 88 "AVDTP_SI_GET_CAPABILITIES", 89 "AVDTP_SI_SET_CONFIGURATION", 90 "AVDTP_SI_GET_CONFIGURATION", 91 "AVDTP_SI_RECONFIGURE", 92 "AVDTP_SI_OPEN", 93 "AVDTP_SI_START", 94 "AVDTP_SI_CLOSE", 95 "AVDTP_SI_SUSPEND", 96 "AVDTP_SI_ABORT", 97 "AVDTP_SI_SECURITY_CONTROL", 98 "AVDTP_SI_GET_ALL_CAPABILITIES", 99 "AVDTP_SI_DELAY_REPORT" 100 }; 101 102 const char * avdtp_si2str(uint16_t index){ 103 if ((index <= 0) || (index >= sizeof(avdtp_si_name)/sizeof(avdtp_si_name[0]) )) return avdtp_si_name[0]; 104 return avdtp_si_name[index]; 105 } 106 107 108 static const uint32_t usac_sampling_frequency_table[] = { 109 96000, 88200, 76800, 70560, 110 64000, 58800, 48000, 44100, 38400, 35280, 32000, 29400, 111 24000, 22050, 19200, 17640, 16000, 14700, 12800, 12000, 112 11760, 11025, 9600, 8820, 8000, 7350 113 }; 114 static const uint8_t usac_sampling_frequency_table_size = sizeof(usac_sampling_frequency_table)/sizeof(uint32_t); 115 116 static const uint32_t mpeg_sampling_frequency_table[] = { 117 48000, 44100, 32000, 24000, 22040, 16000 118 }; 119 static const uint8_t mpeg_sampling_frequency_table_size = sizeof(mpeg_sampling_frequency_table)/sizeof(uint32_t); 120 121 static const uint32_t sbc_sampling_frequency_table[] = { 122 48000, 44100, 32000, 16000 123 }; 124 static const uint8_t sbc_sampling_frequency_table_size = sizeof(sbc_sampling_frequency_table)/sizeof(uint32_t); 125 126 static const uint32_t atrac_sampling_frequency_table[] = { 127 48000, 44100 128 }; 129 static const uint8_t atrac_sampling_frequency_table_size = sizeof(atrac_sampling_frequency_table)/sizeof(uint32_t); 130 131 static const uint32_t aac_sampling_frequency_table[] = { 132 96000, 88200, 64000, 48000, 44100, 32000, 24000, 22050, 16000, 12000, 11025, 8000 133 }; 134 static const uint8_t aac_sampling_frequency_table_size = sizeof(aac_sampling_frequency_table)/sizeof(uint32_t); 135 136 static uint32_t avdtp_config_get_sampling_frequency_bitmap_from_table(uint16_t sampling_frequency_hz, const uint32_t * table, uint8_t table_size) { 137 uint8_t i; 138 for (i = 0; i < table_size; i++){ 139 if (sampling_frequency_hz == table[i]){ 140 return 1 << i; 141 } 142 } 143 return 0; 144 } 145 146 static uint32_t avdtp_config_get_sampling_frequency_from_table(uint16_t sampling_frequency_bitmap, const uint32_t * table, uint8_t table_size) { 147 uint8_t i; 148 for (i = 0; i < table_size; i++){ 149 if (sampling_frequency_bitmap & (1U << i)) { 150 return table[i]; 151 } 152 } 153 return 0; 154 } 155 156 157 void avdtp_reset_stream_endpoint(avdtp_stream_endpoint_t * stream_endpoint){ 158 stream_endpoint->media_con_handle = 0; 159 stream_endpoint->l2cap_media_cid = 0; 160 stream_endpoint->l2cap_reporting_cid = 0; 161 stream_endpoint->l2cap_recovery_cid = 0; 162 163 stream_endpoint->state = AVDTP_STREAM_ENDPOINT_IDLE; 164 stream_endpoint->acceptor_config_state = AVDTP_ACCEPTOR_STREAM_CONFIG_IDLE; 165 stream_endpoint->initiator_config_state = AVDTP_INITIATOR_STREAM_CONFIG_IDLE; 166 167 stream_endpoint->connection = NULL; 168 169 stream_endpoint->sep.in_use = 0; 170 memset(&stream_endpoint->remote_sep, 0, sizeof(avdtp_sep_t)); 171 172 stream_endpoint->remote_capabilities_bitmap = 0; 173 memset(&stream_endpoint->remote_capabilities, 0, sizeof(avdtp_capabilities_t)); 174 stream_endpoint->remote_configuration_bitmap = 0; 175 memset(&stream_endpoint->remote_configuration, 0, sizeof(avdtp_capabilities_t)); 176 177 // temporary SBC config used by A2DP Source 178 memset(stream_endpoint->media_codec_info, 0, 8); 179 180 stream_endpoint->media_disconnect = 0; 181 stream_endpoint->media_connect = 0; 182 stream_endpoint->start_stream = 0; 183 stream_endpoint->close_stream = 0; 184 stream_endpoint->request_can_send_now = false; 185 stream_endpoint->abort_stream = 0; 186 stream_endpoint->suspend_stream = 0; 187 stream_endpoint->sequence_number = 0; 188 } 189 190 int get_bit16(uint16_t bitmap, int position){ 191 return (bitmap >> position) & 1; 192 } 193 194 uint16_t store_bit16(uint16_t bitmap, int position, uint8_t value){ 195 if (value){ 196 bitmap |= 1 << position; 197 } else { 198 bitmap &= ~ (1 << position); 199 } 200 return bitmap; 201 } 202 203 avdtp_message_type_t avdtp_get_signaling_message_type(uint8_t * packet){ 204 return (avdtp_message_type_t) (packet[0] & 0x03); 205 } 206 207 // returns 0 if header incomplete 208 int avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size){ 209 int pos = 0; 210 if (size < 2) return 0; 211 signaling_header->transaction_label = packet[pos] >> 4; 212 signaling_header->packet_type = (avdtp_packet_type_t)((packet[pos] >> 2) & 0x03); 213 signaling_header->message_type = (avdtp_message_type_t) (packet[pos] & 0x03); 214 pos++; 215 memset(signaling_header->command, 0, sizeof(signaling_header->command)); 216 switch (signaling_header->packet_type){ 217 case AVDTP_SINGLE_PACKET: 218 signaling_header->num_packets = 0; 219 signaling_header->offset = 0; 220 signaling_header->size = 0; 221 break; 222 case AVDTP_END_PACKET: 223 signaling_header->num_packets = 0; 224 break; 225 case AVDTP_START_PACKET: 226 signaling_header->num_packets = packet[pos++]; 227 if (pos < 3) return 0; 228 signaling_header->size = 0; 229 signaling_header->offset = 0; 230 break; 231 case AVDTP_CONTINUE_PACKET: 232 if (signaling_header->num_packets <= 0) { 233 log_info(" ERROR: wrong num fragmented packets\n"); 234 break; 235 } 236 signaling_header->num_packets--; 237 break; 238 default: 239 btstack_assert(false); 240 break; 241 } 242 signaling_header->signal_identifier = (avdtp_signal_identifier_t)(packet[pos++] & 0x3f); 243 return pos; 244 } 245 246 static bool avdtp_is_basic_capability(int service_category){ 247 return (AVDTP_MEDIA_TRANSPORT <= service_category) && (service_category <= AVDTP_MEDIA_CODEC); 248 } 249 250 int avdtp_pack_service_capabilities(uint8_t *buffer, int size, avdtp_capabilities_t caps, avdtp_service_category_t category) { 251 UNUSED(size); 252 253 int i; 254 // pos = 0 reserved for length 255 int pos = 1; 256 switch(category){ 257 case AVDTP_MEDIA_TRANSPORT: 258 case AVDTP_REPORTING: 259 case AVDTP_DELAY_REPORTING: 260 break; 261 case AVDTP_RECOVERY: 262 buffer[pos++] = caps.recovery.recovery_type; // 0x01=RFC2733 263 buffer[pos++] = caps.recovery.maximum_recovery_window_size; 264 buffer[pos++] = caps.recovery.maximum_number_media_packets; 265 break; 266 case AVDTP_CONTENT_PROTECTION: 267 buffer[pos++] = caps.content_protection.cp_type_value_len + 2; 268 big_endian_store_16(buffer, pos, caps.content_protection.cp_type); 269 pos += 2; 270 (void)memcpy(buffer + pos, caps.content_protection.cp_type_value, 271 caps.content_protection.cp_type_value_len); 272 pos += caps.content_protection.cp_type_value_len; 273 break; 274 case AVDTP_HEADER_COMPRESSION: 275 buffer[pos++] = (caps.header_compression.back_ch << 7) | (caps.header_compression.media << 6) | (caps.header_compression.recovery << 5); 276 break; 277 case AVDTP_MULTIPLEXING: 278 buffer[pos++] = caps.multiplexing_mode.fragmentation << 7; 279 for (i=0; i<caps.multiplexing_mode.transport_identifiers_num; i++){ 280 buffer[pos++] = caps.multiplexing_mode.transport_session_identifiers[i] << 7; 281 buffer[pos++] = caps.multiplexing_mode.tcid[i] << 7; 282 // media, reporting. recovery 283 } 284 break; 285 case AVDTP_MEDIA_CODEC: 286 buffer[pos++] = ((uint8_t)caps.media_codec.media_type) << 4; 287 buffer[pos++] = (uint8_t)caps.media_codec.media_codec_type; 288 for (i = 0; i<caps.media_codec.media_codec_information_len; i++){ 289 buffer[pos++] = caps.media_codec.media_codec_information[i]; 290 } 291 break; 292 default: 293 break; 294 } 295 buffer[0] = pos - 1; // length 296 return pos; 297 } 298 299 static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * connection, avdtp_signal_identifier_t signal_identifier, avdtp_service_category_t category, uint8_t cap_len){ 300 connection->error_code = 0; 301 302 if ((category == AVDTP_SERVICE_CATEGORY_INVALID_0) || (category > AVDTP_DELAY_REPORTING)){ 303 log_info(" ERROR: BAD SERVICE CATEGORY %d\n", category); 304 connection->reject_service_category = category; 305 connection->error_code = AVDTP_ERROR_CODE_BAD_SERV_CATEGORY; 306 return 1; 307 } 308 309 if (signal_identifier == AVDTP_SI_RECONFIGURE){ 310 if ( (category != AVDTP_CONTENT_PROTECTION) && (category != AVDTP_MEDIA_CODEC)){ 311 log_info(" ERROR: REJECT CATEGORY, INVALID_CAPABILITIES\n"); 312 connection->reject_service_category = category; 313 connection->error_code = AVDTP_ERROR_CODE_INVALID_CAPABILITIES; 314 return 1; 315 } 316 } 317 318 switch(category){ 319 case AVDTP_MEDIA_TRANSPORT: 320 if (cap_len != 0){ 321 log_info(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); 322 connection->reject_service_category = category; 323 connection->error_code = AVDTP_ERROR_CODE_BAD_MEDIA_TRANSPORT_FORMAT; 324 return 1; 325 } 326 break; 327 case AVDTP_REPORTING: 328 case AVDTP_DELAY_REPORTING: 329 if (cap_len != 0){ 330 log_info(" ERROR: REJECT CATEGORY, BAD_LENGTH\n"); 331 connection->reject_service_category = category; 332 connection->error_code = AVDTP_ERROR_CODE_BAD_LENGTH; 333 return 1; 334 } 335 break; 336 case AVDTP_RECOVERY: 337 if (cap_len != 3){ 338 log_info(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); 339 connection->reject_service_category = category; 340 connection->error_code = AVDTP_ERROR_CODE_BAD_RECOVERY_FORMAT; 341 return 1; 342 } 343 break; 344 case AVDTP_CONTENT_PROTECTION: 345 if (cap_len < 2){ 346 log_info(" ERROR: REJECT CATEGORY, BAD_CP_FORMAT\n"); 347 connection->reject_service_category = category; 348 connection->error_code = AVDTP_ERROR_CODE_BAD_CP_FORMAT; 349 return 1; 350 } 351 break; 352 case AVDTP_HEADER_COMPRESSION: 353 // TODO: find error code for bad header compression 354 if (cap_len != 1){ 355 log_info(" ERROR: REJECT CATEGORY, BAD_HEADER_COMPRESSION\n"); 356 connection->reject_service_category = category; 357 connection->error_code = AVDTP_ERROR_CODE_BAD_RECOVERY_FORMAT; 358 return 1; 359 } 360 break; 361 case AVDTP_MULTIPLEXING: 362 break; 363 case AVDTP_MEDIA_CODEC: 364 break; 365 default: 366 break; 367 } 368 return 0; 369 } 370 371 uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdtp_signal_identifier_t signal_identifier, avdtp_capabilities_t * caps, uint8_t * packet, uint16_t size){ 372 373 int i; 374 375 uint16_t registered_service_categories = 0; 376 uint16_t to_process = size; 377 378 while (to_process >= 2){ 379 380 avdtp_service_category_t category = (avdtp_service_category_t) packet[0]; 381 uint8_t cap_len = packet[1]; 382 packet += 2; 383 to_process -= 2; 384 385 if (cap_len > to_process){ 386 connection->reject_service_category = category; 387 connection->error_code = AVDTP_ERROR_CODE_BAD_LENGTH; 388 return 0; 389 } 390 391 if (avdtp_unpack_service_capabilities_has_errors(connection, signal_identifier, category, cap_len)) return 0; 392 393 int category_valid = 1; 394 395 uint8_t * data = packet; 396 uint16_t pos = 0; 397 398 switch(category){ 399 case AVDTP_RECOVERY: 400 caps->recovery.recovery_type = data[pos++]; 401 caps->recovery.maximum_recovery_window_size = data[pos++]; 402 caps->recovery.maximum_number_media_packets = data[pos++]; 403 break; 404 case AVDTP_CONTENT_PROTECTION: 405 caps->content_protection.cp_type = big_endian_read_16(data, 0); 406 caps->content_protection.cp_type_value_len = cap_len - 2; 407 // connection->reject_service_category = category; 408 // connection->error_code = UNSUPPORTED_CONFIGURATION; 409 // support for content protection goes here 410 break; 411 case AVDTP_HEADER_COMPRESSION: 412 caps->header_compression.back_ch = (data[0] >> 7) & 1; 413 caps->header_compression.media = (data[0] >> 6) & 1; 414 caps->header_compression.recovery = (data[0] >> 5) & 1; 415 break; 416 case AVDTP_MULTIPLEXING: 417 caps->multiplexing_mode.fragmentation = (data[pos++] >> 7) & 1; 418 // read [tsid, tcid] for media, reporting. recovery respectively 419 caps->multiplexing_mode.transport_identifiers_num = 3; 420 for (i=0; i<caps->multiplexing_mode.transport_identifiers_num; i++){ 421 caps->multiplexing_mode.transport_session_identifiers[i] = (data[pos++] >> 7) & 1; 422 caps->multiplexing_mode.tcid[i] = (data[pos++] >> 7) & 1; 423 } 424 break; 425 case AVDTP_MEDIA_CODEC: 426 caps->media_codec.media_type = (avdtp_media_type_t)(data[pos++] >> 4); 427 caps->media_codec.media_codec_type = (avdtp_media_codec_type_t)(data[pos++]); 428 caps->media_codec.media_codec_information_len = cap_len - 2; 429 caps->media_codec.media_codec_information = &data[pos++]; 430 break; 431 case AVDTP_MEDIA_TRANSPORT: 432 case AVDTP_REPORTING: 433 case AVDTP_DELAY_REPORTING: 434 break; 435 default: 436 category_valid = 0; 437 break; 438 } 439 440 if (category_valid) { 441 registered_service_categories = store_bit16(registered_service_categories, category, 1); 442 } 443 444 packet += cap_len; 445 to_process -= cap_len; 446 } 447 448 return registered_service_categories; 449 } 450 451 void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uint8_t transaction_label, uint16_t service_categories, avdtp_capabilities_t capabilities, uint8_t identifier){ 452 if (signaling_packet->offset) return; 453 bool basic_capabilities_only = false; 454 signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG; 455 int i; 456 457 signaling_packet->size = 0; 458 memset(signaling_packet->command, 0 , sizeof(signaling_packet->command)); 459 460 switch (identifier) { 461 case AVDTP_SI_GET_CAPABILITIES: 462 basic_capabilities_only = true; 463 break; 464 case AVDTP_SI_GET_ALL_CAPABILITIES: 465 break; 466 case AVDTP_SI_SET_CONFIGURATION: 467 signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2; 468 signaling_packet->command[signaling_packet->size++] = signaling_packet->int_seid << 2; 469 signaling_packet->message_type = AVDTP_CMD_MSG; 470 break; 471 case AVDTP_SI_RECONFIGURE: 472 signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2; 473 signaling_packet->message_type = AVDTP_CMD_MSG; 474 break; 475 default: 476 log_error("avdtp_prepare_capabilities wrong identifier %d", identifier); 477 break; 478 } 479 480 for (i = AVDTP_MEDIA_TRANSPORT; i <= AVDTP_DELAY_REPORTING; i++){ 481 int registered_category = get_bit16(service_categories, i); 482 if (!registered_category && (identifier == AVDTP_SI_SET_CONFIGURATION)){ 483 // TODO: introduce bitmap of mandatory categories 484 if (i == AVDTP_MEDIA_TRANSPORT){ 485 registered_category = true; 486 } 487 } 488 // AVDTP_SI_GET_CAPABILITIES reports only basic capabilities (i.e., it skips non-basic categories) 489 if (basic_capabilities_only && !avdtp_is_basic_capability(i)){ 490 registered_category = false; 491 } 492 493 if (registered_category){ 494 // service category 495 signaling_packet->command[signaling_packet->size++] = i; 496 signaling_packet->size += avdtp_pack_service_capabilities(signaling_packet->command + signaling_packet->size, 497 sizeof(signaling_packet->command) - signaling_packet->size, capabilities, (avdtp_service_category_t) i); 498 } 499 } 500 signaling_packet->signal_identifier = (avdtp_signal_identifier_t)identifier; 501 signaling_packet->transaction_label = transaction_label; 502 } 503 504 int avdtp_signaling_create_fragment(uint16_t cid, avdtp_signaling_packet_t * signaling_packet, uint8_t * out_buffer) { 505 int mtu = l2cap_get_remote_mtu_for_local_cid(cid); 506 int data_len = 0; 507 508 uint16_t offset = signaling_packet->offset; 509 uint16_t pos = 1; 510 511 if (offset == 0){ 512 if (signaling_packet->size <= (mtu - 2)){ 513 signaling_packet->packet_type = AVDTP_SINGLE_PACKET; 514 out_buffer[pos++] = signaling_packet->signal_identifier; 515 data_len = signaling_packet->size; 516 } else { 517 signaling_packet->packet_type = AVDTP_START_PACKET; 518 out_buffer[pos++] = (mtu + signaling_packet->size)/ (mtu-1); 519 out_buffer[pos++] = signaling_packet->signal_identifier; 520 data_len = mtu - 3; 521 signaling_packet->offset = data_len; 522 } 523 } else { 524 int remaining_bytes = signaling_packet->size - offset; 525 if (remaining_bytes <= (mtu - 1)){ 526 signaling_packet->packet_type = AVDTP_END_PACKET; 527 data_len = remaining_bytes; 528 signaling_packet->offset = 0; 529 } else{ 530 signaling_packet->packet_type = AVDTP_CONTINUE_PACKET; 531 data_len = mtu - 1; 532 signaling_packet->offset += data_len; 533 } 534 } 535 out_buffer[0] = avdtp_header(signaling_packet->transaction_label, signaling_packet->packet_type, signaling_packet->message_type); 536 (void)memcpy(out_buffer + pos, signaling_packet->command + offset, 537 data_len); 538 pos += data_len; 539 return pos; 540 } 541 542 543 void avdtp_signaling_emit_connection_established(uint16_t avdtp_cid, bd_addr_t addr, hci_con_handle_t con_handle, uint8_t status) { 544 uint8_t event[14]; 545 int pos = 0; 546 event[pos++] = HCI_EVENT_AVDTP_META; 547 event[pos++] = sizeof(event) - 2; 548 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED; 549 little_endian_store_16(event, pos, avdtp_cid); 550 pos += 2; 551 reverse_bd_addr(addr,&event[pos]); 552 pos += 6; 553 little_endian_store_16(event, pos, con_handle); 554 pos += 2; 555 event[pos++] = status; 556 avdtp_emit_sink_and_source(event, pos); 557 } 558 559 void avdtp_signaling_emit_connection_released(uint16_t avdtp_cid) { 560 uint8_t event[5]; 561 int pos = 0; 562 event[pos++] = HCI_EVENT_AVDTP_META; 563 event[pos++] = sizeof(event) - 2; 564 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CONNECTION_RELEASED; 565 little_endian_store_16(event, pos, avdtp_cid); 566 pos += 2; 567 avdtp_emit_sink_and_source(event, pos); 568 } 569 570 void avdtp_signaling_emit_sep(uint16_t avdtp_cid, avdtp_sep_t sep) { 571 uint8_t event[9]; 572 int pos = 0; 573 event[pos++] = HCI_EVENT_AVDTP_META; 574 event[pos++] = sizeof(event) - 2; 575 event[pos++] = AVDTP_SUBEVENT_SIGNALING_SEP_FOUND; 576 little_endian_store_16(event, pos, avdtp_cid); 577 pos += 2; 578 event[pos++] = sep.seid; 579 event[pos++] = sep.in_use; 580 event[pos++] = sep.media_type; 581 event[pos++] = sep.type; 582 avdtp_emit_sink_and_source(event, pos); 583 } 584 585 void avdtp_signaling_emit_sep_done(uint16_t avdtp_cid) { 586 uint8_t event[5]; 587 int pos = 0; 588 event[pos++] = HCI_EVENT_AVDTP_META; 589 event[pos++] = sizeof(event) - 2; 590 event[pos++] = AVDTP_SUBEVENT_SIGNALING_SEP_DICOVERY_DONE; 591 little_endian_store_16(event, pos, avdtp_cid); 592 pos += 2; 593 avdtp_emit_sink_and_source(event, pos); 594 } 595 596 void avdtp_signaling_emit_accept(uint16_t avdtp_cid, uint8_t local_seid, avdtp_signal_identifier_t identifier, bool is_initiator) { 597 uint8_t event[8]; 598 int pos = 0; 599 event[pos++] = HCI_EVENT_AVDTP_META; 600 event[pos++] = sizeof(event) - 2; 601 event[pos++] = AVDTP_SUBEVENT_SIGNALING_ACCEPT; 602 little_endian_store_16(event, pos, avdtp_cid); 603 pos += 2; 604 event[pos++] = local_seid; 605 event[pos++] = is_initiator ? 1 : 0; 606 event[pos++] = identifier; 607 avdtp_emit_sink_and_source(event, pos); 608 } 609 610 void avdtp_signaling_emit_accept_for_stream_endpoint(avdtp_stream_endpoint_t * stream_endpoint, uint8_t local_seid, avdtp_signal_identifier_t identifier, bool is_initiator){ 611 uint8_t event[8]; 612 int pos = 0; 613 event[pos++] = HCI_EVENT_AVDTP_META; 614 event[pos++] = sizeof(event) - 2; 615 event[pos++] = AVDTP_SUBEVENT_SIGNALING_ACCEPT; 616 little_endian_store_16(event, pos, stream_endpoint->connection->avdtp_cid); 617 pos += 2; 618 event[pos++] = local_seid; 619 event[pos++] = is_initiator ? 1 : 0; 620 event[pos++] = identifier; 621 622 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 623 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 624 } 625 626 void avdtp_signaling_emit_reject(uint16_t avdtp_cid, uint8_t local_seid, avdtp_signal_identifier_t identifier, bool is_initiator) { 627 uint8_t event[8]; 628 int pos = 0; 629 event[pos++] = HCI_EVENT_AVDTP_META; 630 event[pos++] = sizeof(event) - 2; 631 event[pos++] = AVDTP_SUBEVENT_SIGNALING_REJECT; 632 little_endian_store_16(event, pos, avdtp_cid); 633 pos += 2; 634 event[pos++] = local_seid; 635 event[pos++] = is_initiator ? 1 : 0; 636 event[pos++] = identifier; 637 avdtp_emit_sink_and_source(event, pos); 638 } 639 640 void avdtp_signaling_emit_general_reject(uint16_t avdtp_cid, uint8_t local_seid, avdtp_signal_identifier_t identifier, bool is_initiator) { 641 uint8_t event[8]; 642 int pos = 0; 643 event[pos++] = HCI_EVENT_AVDTP_META; 644 event[pos++] = sizeof(event) - 2; 645 event[pos++] = AVDTP_SUBEVENT_SIGNALING_GENERAL_REJECT; 646 little_endian_store_16(event, pos, avdtp_cid); 647 pos += 2; 648 event[pos++] = local_seid; 649 event[pos++] = is_initiator ? 1 : 0; 650 event[pos++] = identifier; 651 avdtp_emit_sink_and_source(event, pos); 652 } 653 654 static inline void 655 avdtp_signaling_emit_capability(uint8_t capability_subevent_id, uint16_t avdtp_cid, uint8_t remote_seid) { 656 uint8_t event[6]; 657 int pos = 0; 658 event[pos++] = HCI_EVENT_AVDTP_META; 659 event[pos++] = sizeof(event) - 2; 660 event[pos++] = capability_subevent_id; 661 little_endian_store_16(event, pos, avdtp_cid); 662 pos += 2; 663 event[pos++] = remote_seid; 664 avdtp_emit_sink_and_source(event, pos); 665 } 666 667 static void avdtp_signaling_emit_media_codec_sbc_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 668 const uint8_t * media_codec_information = media_codec.media_codec_information; 669 uint8_t event[14]; 670 int pos = 0; 671 event[pos++] = HCI_EVENT_AVDTP_META; 672 event[pos++] = sizeof(event) - 2; 673 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CAPABILITY; 674 little_endian_store_16(event, pos, avdtp_cid); 675 pos += 2; 676 event[pos++] = remote_seid; 677 event[pos++] = media_codec.media_type; 678 event[pos++] = media_codec_information[0] >> 4; 679 event[pos++] = media_codec_information[0] & 0x0F; 680 event[pos++] = media_codec_information[1] >> 4; 681 event[pos++] = (media_codec_information[1] & 0x0F) >> 2; 682 event[pos++] = media_codec_information[1] & 0x03; 683 event[pos++] = media_codec_information[2]; 684 event[pos++] = media_codec_information[3]; 685 avdtp_emit_sink_and_source(event, pos); 686 } 687 688 static void avdtp_signaling_emit_media_codec_mpeg_audio_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 689 const uint8_t * media_codec_information = media_codec.media_codec_information; 690 uint8_t event[15]; 691 int pos = 0; 692 event[pos++] = HCI_EVENT_AVDTP_META; 693 event[pos++] = sizeof(event) - 2; 694 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AUDIO_CAPABILITY; 695 little_endian_store_16(event, pos, avdtp_cid); 696 pos += 2; 697 event[pos++] = remote_seid; 698 event[pos++] = media_codec.media_type; 699 700 uint8_t layer_bitmap = media_codec_information[0] >> 5; 701 uint8_t crc = (media_codec_information[0] >> 4) & 0x01; 702 uint8_t channel_mode_bitmap = media_codec_information[0] & 0x07; 703 uint8_t mpf = (media_codec_information[1] >> 6) & 0x01; 704 uint8_t sampling_frequency_bitmap = media_codec_information[1] & 0x3F; 705 uint8_t vbr = (media_codec_information[2] >> 7) & 0x01; 706 uint16_t bit_rate_index_bitmap = ((media_codec_information[3] & 0x3f) << 8) | media_codec.media_codec_information[4]; 707 708 event[pos++] = layer_bitmap; 709 event[pos++] = crc; 710 event[pos++] = channel_mode_bitmap; 711 event[pos++] = mpf; 712 event[pos++] = sampling_frequency_bitmap; 713 event[pos++] = vbr; 714 little_endian_store_16(event, pos, bit_rate_index_bitmap); // bit rate index 715 pos += 2; 716 avdtp_emit_sink_and_source(event, pos); 717 } 718 719 static void avdtp_signaling_emit_media_codec_mpeg_aac_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 720 const uint8_t * media_codec_information = media_codec.media_codec_information; 721 uint8_t event[16]; 722 int pos = 0; 723 event[pos++] = HCI_EVENT_AVDTP_META; 724 event[pos++] = sizeof(event) - 2; 725 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AAC_CAPABILITY; 726 little_endian_store_16(event, pos, avdtp_cid); 727 pos += 2; 728 event[pos++] = remote_seid; 729 event[pos++] = media_codec.media_type; 730 731 uint8_t object_type_bitmap = media_codec_information[0] >> 1; 732 uint8_t drc = media_codec_information[0] & 0x01; 733 uint16_t sampling_frequency_bitmap = (media_codec_information[1] << 4) | (media_codec_information[2] >> 4); 734 uint8_t channels_bitmap = media_codec_information[2] & 0x0F; 735 uint32_t bit_rate_bitmap = ((media_codec_information[3] & 0x7f) << 16) | (media_codec_information[4] << 8) | media_codec_information[5]; 736 uint8_t vbr = media_codec_information[3] >> 7; 737 738 event[pos++] = object_type_bitmap; 739 little_endian_store_16(event, pos, sampling_frequency_bitmap); 740 pos += 2; 741 event[pos++] = channels_bitmap; 742 little_endian_store_24(event, pos, bit_rate_bitmap); 743 pos += 3; 744 event[pos++] = vbr; 745 event[pos++] = drc; 746 avdtp_emit_sink_and_source(event, pos); 747 } 748 749 static void avdtp_signaling_emit_media_codec_atrac_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 750 const uint8_t * media_codec_information = media_codec.media_codec_information; 751 uint8_t event[16]; 752 int pos = 0; 753 event[pos++] = HCI_EVENT_AVDTP_META; 754 pos++; // set later 755 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_ATRAC_CAPABILITY; 756 little_endian_store_16(event, pos, avdtp_cid); 757 pos += 2; 758 event[pos++] = remote_seid; 759 event[pos++] = media_codec.media_type; 760 761 uint8_t version = media_codec_information[0] >> 5; 762 uint8_t channel_mode_bitmap = (media_codec_information[0] >> 2) & 0x07; 763 uint8_t sampling_frequency_bitmap = (media_codec_information[1] >> 4) & 0x03; 764 uint8_t vbr = (media_codec_information[1] >> 3) & 0x01; 765 uint16_t bit_rate_index_bitmap = ((media_codec_information[1]) & 0x07) << 16 | (media_codec_information[2] << 8) | media_codec_information[3]; 766 uint16_t maximum_sul = (media_codec_information[4] << 8) | media_codec_information[5]; 767 768 event[pos++] = version; 769 event[pos++] = channel_mode_bitmap; 770 event[pos++] = sampling_frequency_bitmap; 771 event[pos++] = vbr; 772 little_endian_store_24(event, pos, bit_rate_index_bitmap); 773 pos += 3; 774 little_endian_store_16(event, pos, maximum_sul); 775 pos += 2; 776 event[1] = pos - 2; 777 avdtp_emit_sink_and_source(event, pos); 778 } 779 780 static void avdtp_signaling_emit_media_codec_mpeg_d_usac_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 781 const uint8_t * media_codec_information = media_codec.media_codec_information; 782 uint8_t event[18]; 783 int pos = 0; 784 event[pos++] = HCI_EVENT_AVDTP_META; 785 pos++; // set later 786 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_D_USAC_CAPABILITY; 787 little_endian_store_16(event, pos, avdtp_cid); 788 pos += 2; 789 event[pos++] = remote_seid; 790 event[pos++] = media_codec.media_type; 791 792 uint32_t sampling_frequency_bitmap = ((media_codec_information[0] & 0x3F) << 20) | 793 (media_codec_information[1] << 12) | 794 (media_codec_information[2] << 4) | 795 (media_codec_information[3] >> 4); 796 797 uint8_t channels_bitmap = (media_codec_information[3] >> 2) & 0x03; 798 uint8_t vbr = (media_codec_information[4] >> 7) & 0x01; 799 800 uint16_t bit_rate_index_bitmap = ((media_codec_information[4]) & 0xEF) << 16 | (media_codec_information[5] << 8) | media_codec_information[6]; 801 802 event[pos++] = media_codec_information[0] >> 6; 803 little_endian_store_32(event, pos, sampling_frequency_bitmap); 804 pos += 4; 805 event[pos++] = channels_bitmap; 806 event[pos++] = sampling_frequency_bitmap; 807 event[pos++] = vbr; 808 little_endian_store_24(event, pos, bit_rate_index_bitmap); 809 pos += 3; 810 event[1] = pos - 2; 811 avdtp_emit_sink_and_source(event, pos); 812 } 813 814 815 static void avdtp_signaling_emit_media_codec_other_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 816 uint8_t event[AVDTP_MAX_MEDIA_CODEC_INFORMATION_LENGTH + 11]; 817 int pos = 0; 818 event[pos++] = HCI_EVENT_AVDTP_META; 819 pos++; // set later 820 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CAPABILITY; 821 little_endian_store_16(event, pos, avdtp_cid); 822 pos += 2; 823 event[pos++] = remote_seid; 824 event[pos++] = media_codec.media_type; 825 little_endian_store_16(event, pos, media_codec.media_codec_type); 826 pos += 2; 827 little_endian_store_16(event, pos, media_codec.media_codec_information_len); 828 pos += 2; 829 uint32_t media_codec_info_len = btstack_min(media_codec.media_codec_information_len, AVDTP_MAX_MEDIA_CODEC_INFORMATION_LENGTH); 830 (void)memcpy(event + pos, media_codec.media_codec_information, media_codec_info_len); 831 pos += media_codec_info_len; 832 event[1] = pos - 2; 833 avdtp_emit_sink_and_source(event, pos); 834 } 835 836 static void 837 avdtp_signaling_emit_media_transport_capability(uint16_t avdtp_cid, uint8_t remote_seid) { 838 avdtp_signaling_emit_capability(AVDTP_SUBEVENT_SIGNALING_MEDIA_TRANSPORT_CAPABILITY, avdtp_cid, 839 remote_seid); 840 } 841 842 static void avdtp_signaling_emit_reporting_capability(uint16_t avdtp_cid, uint8_t remote_seid) { 843 avdtp_signaling_emit_capability(AVDTP_SUBEVENT_SIGNALING_REPORTING_CAPABILITY, avdtp_cid, remote_seid); 844 } 845 846 static void 847 avdtp_signaling_emit_delay_reporting_capability(uint16_t avdtp_cid, uint8_t remote_seid) { 848 avdtp_signaling_emit_capability(AVDTP_SUBEVENT_SIGNALING_DELAY_REPORTING_CAPABILITY, avdtp_cid, 849 remote_seid); 850 } 851 852 static void avdtp_signaling_emit_recovery_capability(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_recovery_capabilities_t *recovery) { 853 uint8_t event[9]; 854 int pos = 0; 855 event[pos++] = HCI_EVENT_AVDTP_META; 856 event[pos++] = sizeof(event) - 2; 857 event[pos++] = AVDTP_SUBEVENT_SIGNALING_RECOVERY_CAPABILITY; 858 little_endian_store_16(event, pos, avdtp_cid); 859 pos += 2; 860 event[pos++] = remote_seid; 861 event[pos++] = recovery->recovery_type; 862 event[pos++] = recovery->maximum_recovery_window_size; 863 event[pos++] = recovery->maximum_number_media_packets; 864 avdtp_emit_sink_and_source(event, pos); 865 } 866 867 #define MAX_CONTENT_PROTECTION_VALUE_LEN 32 868 static void 869 avdtp_signaling_emit_content_protection_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_content_protection_t *content_protection) { 870 uint8_t event[10 + MAX_CONTENT_PROTECTION_VALUE_LEN]; 871 int pos = 0; 872 event[pos++] = HCI_EVENT_AVDTP_META; 873 pos++; // set later 874 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CONTENT_PROTECTION_CAPABILITY; 875 little_endian_store_16(event, pos, avdtp_cid); 876 pos += 2; 877 event[pos++] = remote_seid; 878 879 little_endian_store_16(event, pos, content_protection->cp_type); 880 pos += 2; 881 882 // drop cp protection value if longer than expected 883 if (content_protection->cp_type_value_len <= MAX_CONTENT_PROTECTION_VALUE_LEN){ 884 little_endian_store_16(event, pos, content_protection->cp_type_value_len); 885 pos += 2; 886 (void)memcpy(event + pos, content_protection->cp_type_value, content_protection->cp_type_value_len); 887 pos += content_protection->cp_type_value_len; 888 } else { 889 little_endian_store_16(event, pos, 0); 890 pos += 2; 891 } 892 event[1] = pos - 2; 893 avdtp_emit_sink_and_source(event, pos); 894 } 895 896 897 static void 898 avdtp_signaling_emit_header_compression_capability(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_header_compression_capabilities_t *header_compression) { 899 uint8_t event[9]; 900 int pos = 0; 901 event[pos++] = HCI_EVENT_AVDTP_META; 902 event[pos++] = sizeof(event) - 2; 903 event[pos++] = AVDTP_SUBEVENT_SIGNALING_HEADER_COMPRESSION_CAPABILITY; 904 little_endian_store_16(event, pos, avdtp_cid); 905 pos += 2; 906 event[pos++] = remote_seid; 907 event[pos++] = header_compression->back_ch; 908 event[pos++] = header_compression->media; 909 event[pos++] = header_compression->recovery; 910 avdtp_emit_sink_and_source(event, pos); 911 } 912 913 static void 914 avdtp_signaling_emit_content_multiplexing_capability(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_multiplexing_mode_capabilities_t *multiplexing_mode) { 915 uint8_t event[14]; 916 int pos = 0; 917 event[pos++] = HCI_EVENT_AVDTP_META; 918 event[pos++] = sizeof(event) - 2; 919 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MULTIPLEXING_CAPABILITY; 920 little_endian_store_16(event, pos, avdtp_cid); 921 pos += 2; 922 event[pos++] = remote_seid; 923 924 event[pos++] = multiplexing_mode->fragmentation; 925 event[pos++] = multiplexing_mode->transport_identifiers_num; 926 927 int i; 928 for (i = 0; i < 3; i++){ 929 event[pos++] = multiplexing_mode->transport_session_identifiers[i]; 930 } 931 for (i = 0; i < 3; i++){ 932 event[pos++] = multiplexing_mode->tcid[i]; 933 } 934 avdtp_emit_sink_and_source(event, pos); 935 } 936 937 static void avdtp_signaling_emit_capability_done(uint16_t avdtp_cid, uint8_t remote_seid) { 938 uint8_t event[6]; 939 int pos = 0; 940 event[pos++] = HCI_EVENT_AVDTP_META; 941 event[pos++] = sizeof(event) - 2; 942 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CAPABILITIES_DONE; 943 little_endian_store_16(event, pos, avdtp_cid); 944 pos += 2; 945 event[pos++] = remote_seid; 946 avdtp_emit_sink_and_source(event, pos); 947 } 948 949 static void avdtp_signaling_emit_media_codec_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec){ 950 switch (media_codec.media_codec_type){ 951 case AVDTP_CODEC_SBC: 952 avdtp_signaling_emit_media_codec_sbc_capability(avdtp_cid, remote_seid, media_codec); 953 break; 954 case AVDTP_CODEC_MPEG_1_2_AUDIO: 955 avdtp_signaling_emit_media_codec_mpeg_audio_capability(avdtp_cid, remote_seid, media_codec); 956 break; 957 case AVDTP_CODEC_MPEG_2_4_AAC: 958 avdtp_signaling_emit_media_codec_mpeg_aac_capability(avdtp_cid, remote_seid, media_codec); 959 break; 960 case AVDTP_CODEC_ATRAC_FAMILY: 961 avdtp_signaling_emit_media_codec_atrac_capability(avdtp_cid, remote_seid, media_codec); 962 break; 963 case AVDTP_CODEC_MPEG_D_USAC: 964 avdtp_signaling_emit_media_codec_mpeg_d_usac_capability(avdtp_cid, remote_seid, media_codec); 965 break; 966 default: 967 avdtp_signaling_emit_media_codec_other_capability(avdtp_cid, remote_seid, media_codec); 968 break; 969 } 970 } 971 972 // emit events for all capabilities incl. final done event 973 void avdtp_signaling_emit_capabilities(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_capabilities_t *capabilities, 974 uint16_t registered_service_categories) { 975 if (get_bit16(registered_service_categories, AVDTP_MEDIA_CODEC)){ 976 avdtp_signaling_emit_media_codec_capability(avdtp_cid, remote_seid, capabilities->media_codec); 977 } 978 979 if (get_bit16(registered_service_categories, AVDTP_MEDIA_TRANSPORT)){ 980 avdtp_signaling_emit_media_transport_capability(avdtp_cid, remote_seid); 981 } 982 if (get_bit16(registered_service_categories, AVDTP_REPORTING)){ 983 avdtp_signaling_emit_reporting_capability(avdtp_cid, remote_seid); 984 } 985 if (get_bit16(registered_service_categories, AVDTP_RECOVERY)){ 986 avdtp_signaling_emit_recovery_capability(avdtp_cid, remote_seid, &capabilities->recovery); 987 } 988 if (get_bit16(registered_service_categories, AVDTP_CONTENT_PROTECTION)){ 989 avdtp_signaling_emit_content_protection_capability(avdtp_cid, remote_seid, 990 &capabilities->content_protection); 991 } 992 if (get_bit16(registered_service_categories, AVDTP_HEADER_COMPRESSION)){ 993 avdtp_signaling_emit_header_compression_capability(avdtp_cid, remote_seid, 994 &capabilities->header_compression); 995 } 996 if (get_bit16(registered_service_categories, AVDTP_MULTIPLEXING)){ 997 avdtp_signaling_emit_content_multiplexing_capability(avdtp_cid, remote_seid, 998 &capabilities->multiplexing_mode); 999 } 1000 if (get_bit16(registered_service_categories, AVDTP_DELAY_REPORTING)){ 1001 avdtp_signaling_emit_delay_reporting_capability(avdtp_cid, remote_seid); 1002 } 1003 avdtp_signaling_emit_capability_done(avdtp_cid, remote_seid); 1004 } 1005 1006 static uint16_t 1007 avdtp_signaling_setup_media_codec_sbc_config_event(uint8_t *event, uint16_t size, 1008 const avdtp_stream_endpoint_t *stream_endpoint, 1009 uint16_t avdtp_cid, uint8_t reconfigure, 1010 const uint8_t *media_codec_information) { 1011 1012 btstack_assert(size >= AVDTP_MEDIA_CONFIG_SBC_EVENT_LEN); 1013 1014 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1015 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1016 1017 int pos = 0; 1018 event[pos++] = HCI_EVENT_AVDTP_META; 1019 event[pos++] = AVDTP_MEDIA_CONFIG_SBC_EVENT_LEN - 2; 1020 1021 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION; 1022 little_endian_store_16(event, pos, avdtp_cid); 1023 pos += 2; 1024 event[pos++] = local_seid; 1025 event[pos++] = remote_seid; 1026 event[pos++] = reconfigure; 1027 event[pos++] = AVDTP_CODEC_SBC; 1028 1029 uint8_t sampling_frequency_bitmap = media_codec_information[0] >> 4; 1030 uint8_t channel_mode_bitmap = media_codec_information[0] & 0x0F; 1031 uint8_t block_length_bitmap = media_codec_information[1] >> 4; 1032 uint8_t subbands_bitmap = (media_codec_information[1] & 0x0F) >> 2; 1033 1034 uint8_t num_channels = 0; 1035 avdtp_channel_mode_t channel_mode; 1036 1037 if (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO){ 1038 channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1039 num_channels = 2; 1040 } else if (channel_mode_bitmap & AVDTP_SBC_STEREO){ 1041 channel_mode = AVDTP_CHANNEL_MODE_STEREO; 1042 num_channels = 2; 1043 } else if (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL){ 1044 channel_mode = AVDTP_CHANNEL_MODE_DUAL_CHANNEL; 1045 num_channels = 2; 1046 } else { 1047 channel_mode = AVDTP_CHANNEL_MODE_MONO; 1048 num_channels = 1; 1049 } 1050 1051 uint16_t sampling_frequency = 0; 1052 if (sampling_frequency_bitmap & AVDTP_SBC_48000) { 1053 sampling_frequency = 48000; 1054 } else if (sampling_frequency_bitmap & AVDTP_SBC_44100) { 1055 sampling_frequency = 44100; 1056 } else if (sampling_frequency_bitmap & AVDTP_SBC_32000) { 1057 sampling_frequency = 32000; 1058 } else if (sampling_frequency_bitmap & AVDTP_SBC_16000) { 1059 sampling_frequency = 16000; 1060 } 1061 1062 uint8_t subbands = 0; 1063 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){ 1064 subbands = 8; 1065 } else if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){ 1066 subbands = 4; 1067 } 1068 1069 uint8_t block_length = 0; 1070 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){ 1071 block_length = 16; 1072 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){ 1073 block_length = 12; 1074 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){ 1075 block_length = 8; 1076 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){ 1077 block_length = 4; 1078 } 1079 1080 little_endian_store_16(event, pos, sampling_frequency); 1081 pos += 2; 1082 1083 event[pos++] = (uint8_t) channel_mode; 1084 event[pos++] = num_channels; 1085 event[pos++] = block_length; 1086 event[pos++] = subbands; 1087 event[pos++] = media_codec_information[1] & 0x03; 1088 event[pos++] = media_codec_information[2]; 1089 event[pos++] = media_codec_information[3]; 1090 1091 btstack_assert(pos == AVDTP_MEDIA_CONFIG_SBC_EVENT_LEN); 1092 1093 return pos; 1094 } 1095 1096 static uint16_t 1097 avdtp_signaling_setup_media_codec_mpeg_audio_config_event(uint8_t *event, uint16_t size, 1098 const avdtp_stream_endpoint_t *stream_endpoint, 1099 uint16_t avdtp_cid, uint8_t reconfigure, 1100 const uint8_t *media_codec_information) { 1101 1102 btstack_assert(size >= AVDTP_MEDIA_CONFIG_MPEG_AUDIO_EVENT_LEN); 1103 1104 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1105 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1106 1107 uint16_t pos = 0; 1108 event[pos++] = HCI_EVENT_AVDTP_META; 1109 event[pos++] = AVDTP_MEDIA_CONFIG_MPEG_AUDIO_EVENT_LEN - 2; 1110 1111 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AUDIO_CONFIGURATION; 1112 little_endian_store_16(event, pos, avdtp_cid); 1113 pos += 2; 1114 event[pos++] = local_seid; 1115 event[pos++] = remote_seid; 1116 event[pos++] = reconfigure; 1117 event[pos++] = AVDTP_CODEC_MPEG_1_2_AUDIO; 1118 1119 uint8_t layer_bitmap = media_codec_information[0] >> 5; 1120 uint8_t crc = (media_codec_information[0] >> 4) & 0x01; 1121 uint8_t channel_mode_bitmap = (media_codec_information[0] & 0x07); 1122 uint8_t mpf = (media_codec_information[1] >> 6) & 0x01; 1123 uint8_t sampling_frequency_bitmap = (media_codec_information[1] & 0x3F); 1124 uint8_t vbr = (media_codec_information[2] >> 7) & 0x01; 1125 uint16_t bit_rate_index_bitmap = ((media_codec_information[2] & 0x3f) << 8) | media_codec_information[3]; 1126 1127 uint8_t layer = 0; 1128 if (layer_bitmap & 0x04){ 1129 layer = AVDTP_MPEG_LAYER_1; 1130 } else if (layer_bitmap & 0x02){ 1131 layer = AVDTP_MPEG_LAYER_2; 1132 } else if (layer_bitmap & 0x01){ 1133 layer = AVDTP_MPEG_LAYER_3; 1134 } 1135 1136 uint8_t num_channels = 0; 1137 avdtp_channel_mode_t channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1138 if (channel_mode_bitmap & 0x08){ 1139 num_channels = 1; 1140 channel_mode = AVDTP_CHANNEL_MODE_MONO; 1141 } else if (channel_mode_bitmap & 0x04){ 1142 num_channels = 2; 1143 channel_mode = AVDTP_CHANNEL_MODE_DUAL_CHANNEL; 1144 } else if (channel_mode_bitmap & 0x02){ 1145 num_channels = 2; 1146 channel_mode = AVDTP_CHANNEL_MODE_STEREO; 1147 } else if (channel_mode_bitmap & 0x02){ 1148 num_channels = 2; 1149 channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1150 } 1151 1152 uint16_t sampling_frequency = 0; 1153 if (sampling_frequency_bitmap & 0x01) { 1154 sampling_frequency = 48000; 1155 } else if (sampling_frequency_bitmap & 0x02) { 1156 sampling_frequency = 44100; 1157 } else if (sampling_frequency_bitmap & 0x04) { 1158 sampling_frequency = 32000; 1159 } else if (sampling_frequency_bitmap & 0x08) { 1160 sampling_frequency = 24000; 1161 } else if (sampling_frequency_bitmap & 0x10) { 1162 sampling_frequency = 22050; 1163 } else if (sampling_frequency_bitmap & 0x20) { 1164 sampling_frequency = 16000; 1165 } 1166 1167 uint8_t bitrate_index = 0; 1168 uint8_t i; 1169 for (i=0;i<14;i++){ 1170 if (bit_rate_index_bitmap & (1U << i)) { 1171 bitrate_index = i; 1172 } 1173 } 1174 1175 event[pos++] = (uint8_t) layer; 1176 event[pos++] = crc; 1177 event[pos++] = (uint8_t) channel_mode; 1178 event[pos++] = num_channels; 1179 event[pos++] = mpf; 1180 little_endian_store_16(event, pos, sampling_frequency); 1181 pos += 2; 1182 event[pos++] = vbr; 1183 event[pos++] = bitrate_index; 1184 1185 btstack_assert(pos == AVDTP_MEDIA_CONFIG_MPEG_AUDIO_EVENT_LEN); 1186 1187 return pos; 1188 } 1189 1190 static uint16_t 1191 avdtp_signaling_setup_media_codec_mpec_aac_config_event(uint8_t *event, uint16_t size, 1192 const avdtp_stream_endpoint_t *stream_endpoint, 1193 uint16_t avdtp_cid, uint8_t reconfigure, 1194 const uint8_t *media_codec_information) { 1195 1196 btstack_assert(size >= AVDTP_MEDIA_CONFIG_MPEG_AAC_EVENT_LEN); 1197 1198 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1199 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1200 1201 uint16_t pos = 0; 1202 event[pos++] = HCI_EVENT_AVDTP_META; 1203 event[pos++] = AVDTP_MEDIA_CONFIG_MPEG_AAC_EVENT_LEN - 2; 1204 1205 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AAC_CONFIGURATION; 1206 little_endian_store_16(event, pos, avdtp_cid); 1207 pos += 2; 1208 event[pos++] = local_seid; 1209 event[pos++] = remote_seid; 1210 event[pos++] = reconfigure; 1211 event[pos++] =AVDTP_CODEC_MPEG_2_4_AAC; 1212 1213 uint8_t object_type_bitmap = media_codec_information[0] >> 1; 1214 uint8_t drc = media_codec_information[0] & 0x01; 1215 uint16_t sampling_frequency_bitmap = (media_codec_information[1] << 4) | (media_codec_information[2] >> 4); 1216 uint8_t channels_bitmap = media_codec_information[2] & 0x0F; 1217 uint8_t vbr = media_codec_information[3] >> 7; 1218 uint32_t bit_rate = ((media_codec_information[3] & 0x7f) << 16) | (media_codec_information[4] << 8) | media_codec_information[5]; 1219 1220 uint8_t object_type = 0; 1221 if (object_type_bitmap & 0x01){ 1222 object_type = AVDTP_AAC_MPEG4_HE_AAC_ELDv2; 1223 } else if (object_type_bitmap & 0x02){ 1224 object_type = AVDTP_AAC_MPEG4_HE_AACv2; 1225 } else if (object_type_bitmap & 0x04){ 1226 object_type = AVDTP_AAC_MPEG4_HE_AAC; 1227 } else if (object_type_bitmap & 0x08){ 1228 object_type = AVDTP_AAC_MPEG4_SCALABLE; 1229 } else if (object_type_bitmap & 0x10){ 1230 object_type = AVDTP_AAC_MPEG4_LTP; 1231 } else if (object_type_bitmap & 0x20){ 1232 object_type = AVDTP_AAC_MPEG4_LC; 1233 } else if (object_type_bitmap & 0x40){ 1234 object_type = AVDTP_AAC_MPEG2_LC; 1235 } 1236 1237 uint32_t sampling_frequency = avdtp_config_get_sampling_frequency_from_table(sampling_frequency_bitmap, aac_sampling_frequency_table, aac_sampling_frequency_table_size); 1238 1239 uint8_t num_channels = 0; 1240 if (channels_bitmap & 0x08){ 1241 num_channels = 1; 1242 } else if (channels_bitmap & 0x04){ 1243 num_channels = 2; 1244 } else if (channels_bitmap & 0x02){ 1245 num_channels = 6; 1246 } else if (channels_bitmap & 0x01){ 1247 num_channels = 8; 1248 } 1249 1250 event[pos++] = object_type; 1251 little_endian_store_24(event, pos, sampling_frequency); 1252 pos += 3; 1253 event[pos++] = num_channels; 1254 little_endian_store_24(event, pos, bit_rate); 1255 pos += 3; 1256 event[pos++] = vbr; 1257 event[pos++] = drc; 1258 1259 btstack_assert(AVDTP_MEDIA_CONFIG_MPEG_AAC_EVENT_LEN == pos); 1260 1261 return pos; 1262 } 1263 1264 static uint16_t 1265 avdtp_signaling_setup_media_codec_mpegd_config_event(uint8_t *event, uint16_t size, 1266 const avdtp_stream_endpoint_t *stream_endpoint, 1267 uint16_t avdtp_cid, uint8_t reconfigure, 1268 const uint8_t *media_codec_information) { 1269 1270 btstack_assert(size >= AVDTP_MEDIA_CONFIG_MPEG_D_USAC_EVENT_LEN); 1271 1272 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1273 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1274 1275 uint16_t pos = 0; 1276 event[pos++] = HCI_EVENT_AVDTP_META; 1277 event[pos++] = AVDTP_MEDIA_CONFIG_MPEG_D_USAC_EVENT_LEN - 2; 1278 1279 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_D_USAC_CONFIGURATION; 1280 1281 little_endian_store_16(event, pos, avdtp_cid); 1282 pos += 2; 1283 event[pos++] = local_seid; 1284 event[pos++] = remote_seid; 1285 event[pos++] = reconfigure; 1286 event[pos++] = AVDTP_CODEC_MPEG_D_USAC; 1287 1288 uint8_t object_type_bitmap = media_codec_information[0] >> 6; 1289 uint32_t sampling_frequency_bitmap = ((media_codec_information[0] & 0x3F) << 20) | 1290 (media_codec_information[1] << 12) | 1291 (media_codec_information[2] << 4) | 1292 (media_codec_information[3] >> 4); 1293 1294 uint8_t channels_bitmap = (media_codec_information[3] >> 2) & 0x03; 1295 uint8_t vbr = (media_codec_information[4] >> 7) & 0x01; 1296 1297 uint32_t bit_rate = ((media_codec_information[3] & 0x7f) << 16) | (media_codec_information[4] << 8) | media_codec_information[5]; 1298 1299 uint8_t object_type = 0; 1300 if (object_type_bitmap & 0x10){ 1301 object_type = AVDTP_USAC_OBJECT_TYPE_MPEG_D_DRC; 1302 } else { 1303 object_type = AVDTP_USAC_OBJECT_TYPE_RFU; 1304 } 1305 1306 uint32_t sampling_frequency = avdtp_config_get_sampling_frequency_from_table(sampling_frequency_bitmap, usac_sampling_frequency_table, usac_sampling_frequency_table_size ); 1307 1308 uint8_t num_channels = 0; 1309 if (channels_bitmap & 0x02){ 1310 num_channels = 1; 1311 } else if (channels_bitmap & 0x01){ 1312 num_channels = 2; 1313 } 1314 1315 event[pos++] = object_type; 1316 little_endian_store_24(event, pos, sampling_frequency); 1317 pos += 3; 1318 event[pos++] = num_channels; 1319 event[pos++] = vbr; 1320 little_endian_store_24(event, pos, bit_rate); 1321 pos += 3; 1322 1323 btstack_assert(AVDTP_MEDIA_CONFIG_MPEG_D_USAC_EVENT_LEN == pos); 1324 1325 return pos; 1326 } 1327 1328 static uint16_t avdtp_signaling_setup_media_codec_atrac_config_event(uint8_t *event, uint16_t size, 1329 const avdtp_stream_endpoint_t *stream_endpoint, 1330 uint16_t avdtp_cid, uint8_t reconfigure, 1331 const uint8_t *media_codec_information) { 1332 btstack_assert(size >= AVDTP_MEDIA_CONFIG_ATRAC_EVENT_LEN); 1333 1334 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1335 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1336 1337 uint16_t pos = 0; 1338 event[pos++] = HCI_EVENT_AVDTP_META; 1339 event[pos++] = AVDTP_MEDIA_CONFIG_ATRAC_EVENT_LEN - 2; 1340 1341 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_ATRAC_CONFIGURATION; 1342 little_endian_store_16(event, pos, avdtp_cid); 1343 pos += 2; 1344 event[pos++] = local_seid; 1345 event[pos++] = remote_seid; 1346 event[pos++] = reconfigure; 1347 event[pos++] = AVDTP_CODEC_ATRAC_FAMILY; 1348 1349 avdtp_atrac_version_t version = (avdtp_atrac_version_t) (media_codec_information[0] >> 5); 1350 uint8_t channel_mode_bitmap = (media_codec_information[0] >> 2) & 0x07; 1351 uint16_t sampling_frequency_bitmap = (media_codec_information[1] >> 4) & 0x03; 1352 uint8_t vbr = (media_codec_information[1] >> 3) & 0x01; 1353 uint16_t bit_rate_index_bitmap = ((media_codec_information[1]) & 0x07) << 16 | (media_codec_information[2] << 8) | media_codec_information[3]; 1354 uint16_t maximum_sul = (media_codec_information[4] << 8) | media_codec_information[5]; 1355 1356 uint8_t num_channels = 0; 1357 avdtp_channel_mode_t channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1358 if (channel_mode_bitmap & 0x04){ 1359 num_channels = 1; 1360 channel_mode = AVDTP_CHANNEL_MODE_MONO; 1361 } else if (channel_mode_bitmap & 0x02){ 1362 num_channels = 2; 1363 channel_mode = AVDTP_CHANNEL_MODE_DUAL_CHANNEL; 1364 } else if (channel_mode_bitmap & 0x01){ 1365 num_channels = 2; 1366 channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1367 } 1368 1369 uint16_t sampling_frequency = 0; 1370 if (sampling_frequency_bitmap & 0x02){ 1371 sampling_frequency = 44100; 1372 } else if (sampling_frequency_bitmap & 0x01){ 1373 sampling_frequency = 48000; 1374 } 1375 1376 // bit 0 = index 0x18, bit 19 = index 0 1377 uint8_t bit_rate_index = 0; 1378 uint8_t i; 1379 for (i=0;i <= 19;i++){ 1380 if (bit_rate_index_bitmap & (1U << i)) { 1381 bit_rate_index = 18 - i; 1382 } 1383 } 1384 1385 event[pos++] = (uint8_t) version; 1386 event[pos++] = (uint8_t) channel_mode; 1387 event[pos++] = num_channels; 1388 little_endian_store_16(event, pos, sampling_frequency); 1389 pos += 2; 1390 event[pos++] = vbr; 1391 event[pos++] = bit_rate_index; 1392 little_endian_store_16(event, pos, maximum_sul); 1393 pos += 2; 1394 1395 btstack_assert(pos == AVDTP_MEDIA_CONFIG_ATRAC_EVENT_LEN); 1396 return pos; 1397 } 1398 1399 static uint16_t avdtp_signaling_setup_media_codec_other_config_event(uint8_t *event, uint16_t size, 1400 const avdtp_stream_endpoint_t *stream_endpoint, 1401 uint16_t avdtp_cid, uint8_t reconfigure, 1402 const adtvp_media_codec_capabilities_t *media_codec) { 1403 btstack_assert(size >= AVDTP_MEDIA_CONFIG_OTHER_EVENT_LEN); 1404 1405 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1406 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1407 1408 uint16_t pos = 0; 1409 event[pos++] = HCI_EVENT_AVDTP_META; 1410 pos++; // set later 1411 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION; 1412 little_endian_store_16(event, pos, avdtp_cid); 1413 pos += 2; 1414 event[pos++] = local_seid; 1415 event[pos++] = remote_seid; 1416 event[pos++] = reconfigure; 1417 event[pos++] = media_codec->media_type; 1418 little_endian_store_16(event, pos, media_codec->media_codec_type); 1419 pos += 2; 1420 little_endian_store_16(event, pos, media_codec->media_codec_information_len); 1421 pos += 2; 1422 1423 btstack_assert(pos == 13); 1424 1425 uint16_t media_codec_len = btstack_min(AVDTP_MAX_MEDIA_CODEC_INFORMATION_LENGTH, media_codec->media_codec_information_len); 1426 (void)memcpy(event + pos, media_codec->media_codec_information, media_codec_len); 1427 pos += media_codec_len; 1428 event[1] = pos - 2; 1429 return pos; 1430 } 1431 1432 void avdtp_signaling_emit_delay(uint16_t avdtp_cid, uint8_t local_seid, uint16_t delay) { 1433 uint8_t event[8]; 1434 int pos = 0; 1435 event[pos++] = HCI_EVENT_AVDTP_META; 1436 event[pos++] = sizeof(event) - 2; 1437 event[pos++] = AVDTP_SUBEVENT_SIGNALING_DELAY_REPORT; 1438 little_endian_store_16(event, pos, avdtp_cid); 1439 pos += 2; 1440 event[pos++] = local_seid; 1441 little_endian_store_16(event, pos, delay); 1442 pos += 2; 1443 avdtp_emit_source(event, pos); 1444 } 1445 1446 uint16_t avdtp_setup_media_codec_config_event(uint8_t *event, uint16_t size, const avdtp_stream_endpoint_t *stream_endpoint, 1447 uint16_t avdtp_cid, uint8_t reconfigure, 1448 const adtvp_media_codec_capabilities_t * media_codec) { 1449 switch (media_codec->media_codec_type){ 1450 case AVDTP_CODEC_SBC: 1451 return avdtp_signaling_setup_media_codec_sbc_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1452 media_codec->media_codec_information); 1453 case AVDTP_CODEC_MPEG_1_2_AUDIO: 1454 return avdtp_signaling_setup_media_codec_mpeg_audio_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1455 media_codec->media_codec_information); 1456 case AVDTP_CODEC_MPEG_2_4_AAC: 1457 return avdtp_signaling_setup_media_codec_mpec_aac_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1458 media_codec->media_codec_information); 1459 case AVDTP_CODEC_ATRAC_FAMILY: 1460 return avdtp_signaling_setup_media_codec_atrac_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1461 media_codec->media_codec_information); 1462 case AVDTP_CODEC_MPEG_D_USAC: 1463 return avdtp_signaling_setup_media_codec_mpegd_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1464 media_codec->media_codec_information); 1465 default: 1466 return avdtp_signaling_setup_media_codec_other_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1467 media_codec); 1468 } 1469 } 1470 1471 void avdtp_signaling_emit_configuration(avdtp_stream_endpoint_t *stream_endpoint, uint16_t avdtp_cid, uint8_t reconfigure, 1472 avdtp_capabilities_t *configuration, uint16_t configured_service_categories) { 1473 1474 if (get_bit16(configured_service_categories, AVDTP_MEDIA_CODEC)){ 1475 uint16_t pos = 0; 1476 // assume MEDIA_CONFIG_OTHER_EVENT_LEN is larger than all other events 1477 uint8_t event[AVDTP_MEDIA_CONFIG_OTHER_EVENT_LEN]; 1478 pos = avdtp_setup_media_codec_config_event(event, sizeof(event), stream_endpoint, avdtp_cid, reconfigure, 1479 &configuration->media_codec); 1480 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1481 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1482 } 1483 } 1484 1485 void avdtp_streaming_emit_connection_established(avdtp_stream_endpoint_t *stream_endpoint, uint8_t status) { 1486 uint8_t event[14]; 1487 int pos = 0; 1488 event[pos++] = HCI_EVENT_AVDTP_META; 1489 event[pos++] = sizeof(event) - 2; 1490 event[pos++] = AVDTP_SUBEVENT_STREAMING_CONNECTION_ESTABLISHED; 1491 little_endian_store_16(event, pos, stream_endpoint->connection->avdtp_cid); 1492 pos += 2; 1493 reverse_bd_addr(stream_endpoint->connection->remote_addr, &event[pos]); 1494 pos += 6; 1495 event[pos++] = avdtp_local_seid(stream_endpoint); 1496 event[pos++] = avdtp_remote_seid(stream_endpoint); 1497 event[pos++] = status; 1498 1499 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1500 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1501 } 1502 1503 void avdtp_streaming_emit_connection_released(avdtp_stream_endpoint_t *stream_endpoint, uint16_t avdtp_cid, uint8_t local_seid) { 1504 uint8_t event[6]; 1505 int pos = 0; 1506 event[pos++] = HCI_EVENT_AVDTP_META; 1507 event[pos++] = sizeof(event) - 2; 1508 event[pos++] = AVDTP_SUBEVENT_STREAMING_CONNECTION_RELEASED; 1509 little_endian_store_16(event, pos, avdtp_cid); 1510 pos += 2; 1511 event[pos++] = local_seid; 1512 1513 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1514 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1515 } 1516 1517 void avdtp_streaming_emit_can_send_media_packet_now(avdtp_stream_endpoint_t *stream_endpoint, uint16_t sequence_number) { 1518 uint8_t event[8]; 1519 int pos = 0; 1520 event[pos++] = HCI_EVENT_AVDTP_META; 1521 event[pos++] = sizeof(event) - 2; 1522 event[pos++] = AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW; 1523 little_endian_store_16(event, pos, stream_endpoint->connection->avdtp_cid); 1524 pos += 2; 1525 event[pos++] = avdtp_local_seid(stream_endpoint); 1526 little_endian_store_16(event, pos, sequence_number); 1527 pos += 2; 1528 event[1] = pos - 2; 1529 1530 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1531 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1532 } 1533 1534 uint8_t avdtp_request_can_send_now_acceptor(avdtp_connection_t *connection) { 1535 if (!connection) return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1536 connection->wait_to_send_acceptor = true; 1537 l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); 1538 return ERROR_CODE_SUCCESS; 1539 } 1540 1541 uint8_t avdtp_request_can_send_now_initiator(avdtp_connection_t *connection) { 1542 if (!connection) return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1543 connection->wait_to_send_initiator = true; 1544 l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); 1545 return ERROR_CODE_SUCCESS; 1546 } 1547 1548 uint8_t avdtp_local_seid(const avdtp_stream_endpoint_t * stream_endpoint){ 1549 if (!stream_endpoint) return 0; 1550 return stream_endpoint->sep.seid; 1551 1552 } 1553 1554 uint8_t avdtp_remote_seid(const avdtp_stream_endpoint_t * stream_endpoint){ 1555 if (!stream_endpoint) return AVDTP_INVALID_SEP_SEID; 1556 return stream_endpoint->remote_sep.seid; 1557 } 1558 1559 // helper to set/get configuration 1560 uint8_t avdtp_config_sbc_set_sampling_frequency(uint8_t * config, uint16_t sampling_frequency_hz){ 1561 uint8_t sampling_frequency_bitmap = (uint8_t)avdtp_config_get_sampling_frequency_bitmap_from_table(sampling_frequency_hz, sbc_sampling_frequency_table, sbc_sampling_frequency_table_size); 1562 if (sampling_frequency_bitmap == 0){ 1563 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1564 } 1565 config[0] = (config[0] & 0x0f) | (uint8_t)(sampling_frequency_bitmap << 4); 1566 return ERROR_CODE_SUCCESS; 1567 } 1568 1569 uint8_t avdtp_config_sbc_store(uint8_t * config, const avdtp_configuration_sbc_t * configuration){ 1570 if (configuration->channel_mode > AVDTP_CHANNEL_MODE_JOINT_STEREO){ 1571 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1572 } 1573 if (configuration->allocation_method > AVDTP_SBC_ALLOCATION_METHOD_SNR){ 1574 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1575 } 1576 1577 switch ((avdtp_sbc_block_length_t)configuration->block_length){ 1578 case AVDTP_SBC_BLOCK_LENGTH_4: 1579 case AVDTP_SBC_BLOCK_LENGTH_8: 1580 case AVDTP_SBC_BLOCK_LENGTH_12: 1581 case AVDTP_SBC_BLOCK_LENGTH_16: 1582 break; 1583 default: 1584 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1585 } 1586 switch ((avdtp_sbc_subbands_t)configuration->subbands){ 1587 case AVDTP_SBC_SUBBANDS_4: 1588 case AVDTP_SBC_SUBBANDS_8: 1589 break; 1590 default: 1591 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1592 } 1593 if ((configuration->min_bitpool_value < 2) || (configuration->min_bitpool_value > 250) || (configuration->min_bitpool_value > configuration->max_bitpool_value)){ 1594 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1595 } 1596 if ((configuration->max_bitpool_value < 2) || (configuration->max_bitpool_value > 250)){ 1597 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1598 } 1599 config[0] = 1 << (3 - (configuration->channel_mode - AVDTP_CHANNEL_MODE_MONO)); 1600 config[1] = (configuration->block_length << 4) | (configuration->subbands << 2) | configuration->allocation_method; 1601 config[2] = configuration->min_bitpool_value; 1602 config[3] = configuration->max_bitpool_value; 1603 return avdtp_config_sbc_set_sampling_frequency(config, configuration->sampling_frequency); 1604 } 1605 1606 uint8_t avdtp_config_mpeg_audio_set_sampling_frequency(uint8_t * config, uint16_t sampling_frequency_hz) { 1607 uint8_t sampling_frequency_bitmap = (uint8_t)avdtp_config_get_sampling_frequency_bitmap_from_table(sampling_frequency_hz, mpeg_sampling_frequency_table, mpeg_sampling_frequency_table_size); 1608 if (sampling_frequency_bitmap == 0){ 1609 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1610 } 1611 config[1] = (config[1] & 0xE0) | sampling_frequency_bitmap; 1612 return ERROR_CODE_SUCCESS; 1613 } 1614 1615 uint8_t avdtp_config_mpeg_audio_store(uint8_t * config, const avdtp_configuration_mpeg_audio_t * configuration){ 1616 if (configuration->layer > AVDTP_MPEG_LAYER_3){ 1617 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1618 } 1619 if (configuration->channel_mode > AVDTP_CHANNEL_MODE_JOINT_STEREO){ 1620 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1621 } 1622 uint16_t bit_rate_mask = 1 << configuration->bit_rate_index; 1623 if (bit_rate_mask == 0){ 1624 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1625 } 1626 1627 config[0] = (1 << (7 - (configuration->layer - AVDTP_MPEG_LAYER_1))) | ((configuration->crc & 0x01) << 4) | (1 << (configuration->channel_mode - AVDTP_CHANNEL_MODE_MONO)); 1628 config[1] = ((configuration->media_payload_format & 0x01) << 6) ; 1629 config[2] = ((configuration->vbr & 0x01) << 7) | ((bit_rate_mask >> 7) & 0x3f); 1630 config[3] = bit_rate_mask & 0xff; 1631 return avdtp_config_mpeg_audio_set_sampling_frequency(config, configuration->sampling_frequency); 1632 } 1633 1634 uint8_t avdtp_config_mpeg_aac_set_sampling_frequency(uint8_t * config, uint16_t sampling_frequency_hz) { 1635 uint16_t sampling_frequency_bitmap = (uint16_t)avdtp_config_get_sampling_frequency_bitmap_from_table(sampling_frequency_hz, aac_sampling_frequency_table, aac_sampling_frequency_table_size); 1636 if (sampling_frequency_bitmap == 0){ 1637 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1638 } 1639 1640 config[1] = sampling_frequency_bitmap >> 4; 1641 config[2] = ((sampling_frequency_bitmap & 0x0f) << 4) | (config[2] & 0x0f); 1642 return ERROR_CODE_SUCCESS; 1643 } 1644 1645 uint8_t avdtp_config_mpeg_aac_store(uint8_t * config, const avdtp_configuration_mpeg_aac_t * configuration) { 1646 config[0] = (1 << (7 -(configuration->object_type - AVDTP_AAC_MPEG2_LC))) | (configuration->drc?1u:0u); 1647 uint8_t channels_bitmap = 0; 1648 switch (configuration->channels){ 1649 case 1: 1650 channels_bitmap = 0x08; 1651 break; 1652 case 2: 1653 channels_bitmap = 0x04; 1654 break; 1655 case 6: 1656 channels_bitmap = 0x02; 1657 break; 1658 case 8: 1659 channels_bitmap = 0x01; 1660 break; 1661 default: 1662 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1663 } 1664 config[2] = channels_bitmap; 1665 config[3] = ((configuration->vbr & 0x01) << 7) | ((configuration->bit_rate >> 16) & 0x7f); 1666 config[4] = (configuration->bit_rate >> 8) & 0xff; 1667 config[5] = configuration->bit_rate & 0xff; 1668 return avdtp_config_mpeg_aac_set_sampling_frequency(config, configuration->sampling_frequency); 1669 } 1670 1671 uint8_t avdtp_config_atrac_set_sampling_frequency(uint8_t * config, uint16_t sampling_frequency_hz) { 1672 uint8_t sampling_frequency_bitmap = (uint8_t)avdtp_config_get_sampling_frequency_bitmap_from_table(sampling_frequency_hz, atrac_sampling_frequency_table, atrac_sampling_frequency_table_size); 1673 if (sampling_frequency_bitmap == 0){ 1674 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1675 } 1676 1677 config[1] = (config[1] & 0xCF) | (uint8_t)(sampling_frequency_bitmap << 4); 1678 return ERROR_CODE_SUCCESS; 1679 } 1680 1681 uint8_t avdtp_config_atrac_store(uint8_t * config, const avdtp_configuration_atrac_t * configuration){ 1682 uint8_t channel_mode_bitmap = 0; 1683 switch (configuration->channel_mode){ 1684 case AVDTP_CHANNEL_MODE_MONO: 1685 channel_mode_bitmap = 4; 1686 break; 1687 case AVDTP_CHANNEL_MODE_DUAL_CHANNEL: 1688 channel_mode_bitmap = 2; 1689 break; 1690 case AVDTP_CHANNEL_MODE_JOINT_STEREO: 1691 channel_mode_bitmap = 1; 1692 break; 1693 default: 1694 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1695 } 1696 config[0] = ((configuration->version - AVDTP_ATRAC_VERSION_1 + 1) << 5) | (channel_mode_bitmap << 2); 1697 uint32_t bit_rate_bitmap = 1 << (0x18 - configuration->bit_rate_index); 1698 config[1] = ((configuration->vbr & 0x01) << 3) | ((bit_rate_bitmap >> 16) & 0x07); 1699 config[2] = (bit_rate_bitmap >> 8) & 0xff; 1700 config[3] = bit_rate_bitmap & 0xff; 1701 config[4] = configuration->maximum_sul >> 8; 1702 config[5] = configuration->maximum_sul & 0xff; 1703 config[6] = 0; 1704 return avdtp_config_atrac_set_sampling_frequency(config, configuration->sampling_frequency); 1705 } 1706 1707 uint8_t avdtp_config_mpegd_usac_set_sampling_frequency(uint8_t * config, uint16_t sampling_frequency_hz) { 1708 uint32_t sampling_frequency_bitmap = avdtp_config_get_sampling_frequency_bitmap_from_table(sampling_frequency_hz, usac_sampling_frequency_table, usac_sampling_frequency_table_size); 1709 if (sampling_frequency_bitmap == 0){ 1710 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1711 } 1712 config[0] = (config[0] & 0xC0) | (uint8_t)(sampling_frequency_bitmap >> 20); 1713 config[1] = (uint8_t) (sampling_frequency_bitmap >> 12); 1714 config[2] = (uint8_t) (sampling_frequency_bitmap >> 4); 1715 config[3] = (sampling_frequency_bitmap & 0x0f) << 4; 1716 return ERROR_CODE_SUCCESS; 1717 } 1718 1719 uint8_t avdtp_config_mpegd_usac_store(uint8_t * config, const avdtp_configuration_mpegd_usac_t * configuration) { 1720 if (configuration->object_type != AVDTP_USAC_OBJECT_TYPE_MPEG_D_DRC){ 1721 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1722 } 1723 config[0] = 0x80; 1724 1725 uint8_t channels_bitmap = 0; 1726 switch (configuration->channels){ 1727 case 1: 1728 channels_bitmap = 0x08; 1729 break; 1730 case 2: 1731 channels_bitmap = 0x04; 1732 break; 1733 default: 1734 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1735 } 1736 config[3] = config[3] | channels_bitmap; 1737 config[4] = ((configuration->vbr & 0x01) << 7) | ((configuration->bit_rate >> 16) & 0x7f); 1738 config[5] = (configuration->bit_rate >> 8) & 0xff; 1739 config[6] = configuration->bit_rate & 0xff; 1740 return avdtp_config_mpegd_usac_set_sampling_frequency(config, configuration->sampling_frequency); 1741 }