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[15]; 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 event[pos++] = drc; 740 741 little_endian_store_16(event, pos, sampling_frequency_bitmap); 742 pos += 2; 743 event[pos++] = channels_bitmap; 744 little_endian_store_24(event, pos, bit_rate_bitmap); 745 pos += 3; 746 event[pos++] = vbr; 747 avdtp_emit_sink_and_source(event, pos); 748 } 749 750 static void avdtp_signaling_emit_media_codec_atrac_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 751 const uint8_t * media_codec_information = media_codec.media_codec_information; 752 uint8_t event[16]; 753 int pos = 0; 754 event[pos++] = HCI_EVENT_AVDTP_META; 755 pos++; // set later 756 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_ATRAC_CAPABILITY; 757 little_endian_store_16(event, pos, avdtp_cid); 758 pos += 2; 759 event[pos++] = remote_seid; 760 event[pos++] = media_codec.media_type; 761 762 uint8_t version = media_codec_information[0] >> 5; 763 uint8_t channel_mode_bitmap = (media_codec_information[0] >> 2) & 0x07; 764 uint8_t sampling_frequency_bitmap = (media_codec_information[1] >> 4) & 0x03; 765 uint8_t vbr = (media_codec_information[1] >> 3) & 0x01; 766 uint16_t bit_rate_index_bitmap = ((media_codec_information[1]) & 0x07) << 16 | (media_codec_information[2] << 8) | media_codec_information[3]; 767 uint16_t maximum_sul = (media_codec_information[4] << 8) | media_codec_information[5]; 768 769 event[pos++] = version; 770 event[pos++] = channel_mode_bitmap; 771 event[pos++] = sampling_frequency_bitmap; 772 event[pos++] = vbr; 773 little_endian_store_24(event, pos, bit_rate_index_bitmap); 774 pos += 3; 775 little_endian_store_16(event, pos, maximum_sul); 776 pos += 2; 777 event[1] = pos - 2; 778 avdtp_emit_sink_and_source(event, pos); 779 } 780 781 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) { 782 const uint8_t * media_codec_information = media_codec.media_codec_information; 783 uint8_t event[18]; 784 int pos = 0; 785 event[pos++] = HCI_EVENT_AVDTP_META; 786 pos++; // set later 787 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_D_USAC_CAPABILITY; 788 little_endian_store_16(event, pos, avdtp_cid); 789 pos += 2; 790 event[pos++] = remote_seid; 791 event[pos++] = media_codec.media_type; 792 793 uint32_t sampling_frequency_bitmap = ((media_codec_information[0] & 0x3F) << 20) | 794 (media_codec_information[1] << 12) | 795 (media_codec_information[2] << 4) | 796 (media_codec_information[3] >> 4); 797 798 uint8_t channels_bitmap = (media_codec_information[3] >> 2) & 0x03; 799 uint8_t vbr = (media_codec_information[4] >> 7) & 0x01; 800 801 uint16_t bit_rate_index_bitmap = ((media_codec_information[4]) & 0xEF) << 16 | (media_codec_information[5] << 8) | media_codec_information[6]; 802 803 event[pos++] = media_codec_information[0] >> 6; 804 little_endian_store_32(event, pos, sampling_frequency_bitmap); 805 pos += 4; 806 event[pos++] = channels_bitmap; 807 event[pos++] = sampling_frequency_bitmap; 808 event[pos++] = vbr; 809 little_endian_store_24(event, pos, bit_rate_index_bitmap); 810 pos += 3; 811 event[1] = pos - 2; 812 avdtp_emit_sink_and_source(event, pos); 813 } 814 815 816 static void avdtp_signaling_emit_media_codec_other_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec) { 817 uint8_t event[AVDTP_MAX_MEDIA_CODEC_INFORMATION_LENGTH + 11]; 818 int pos = 0; 819 event[pos++] = HCI_EVENT_AVDTP_META; 820 pos++; // set later 821 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CAPABILITY; 822 little_endian_store_16(event, pos, avdtp_cid); 823 pos += 2; 824 event[pos++] = remote_seid; 825 event[pos++] = media_codec.media_type; 826 little_endian_store_16(event, pos, media_codec.media_codec_type); 827 pos += 2; 828 little_endian_store_16(event, pos, media_codec.media_codec_information_len); 829 pos += 2; 830 uint32_t media_codec_info_len = btstack_min(media_codec.media_codec_information_len, AVDTP_MAX_MEDIA_CODEC_INFORMATION_LENGTH); 831 (void)memcpy(event + pos, media_codec.media_codec_information, media_codec_info_len); 832 pos += media_codec_info_len; 833 event[1] = pos - 2; 834 avdtp_emit_sink_and_source(event, pos); 835 } 836 837 static void 838 avdtp_signaling_emit_media_transport_capability(uint16_t avdtp_cid, uint8_t remote_seid) { 839 avdtp_signaling_emit_capability(AVDTP_SUBEVENT_SIGNALING_MEDIA_TRANSPORT_CAPABILITY, avdtp_cid, 840 remote_seid); 841 } 842 843 static void avdtp_signaling_emit_reporting_capability(uint16_t avdtp_cid, uint8_t remote_seid) { 844 avdtp_signaling_emit_capability(AVDTP_SUBEVENT_SIGNALING_REPORTING_CAPABILITY, avdtp_cid, remote_seid); 845 } 846 847 static void 848 avdtp_signaling_emit_delay_reporting_capability(uint16_t avdtp_cid, uint8_t remote_seid) { 849 avdtp_signaling_emit_capability(AVDTP_SUBEVENT_SIGNALING_DELAY_REPORTING_CAPABILITY, avdtp_cid, 850 remote_seid); 851 } 852 853 static void avdtp_signaling_emit_recovery_capability(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_recovery_capabilities_t *recovery) { 854 uint8_t event[9]; 855 int pos = 0; 856 event[pos++] = HCI_EVENT_AVDTP_META; 857 event[pos++] = sizeof(event) - 2; 858 event[pos++] = AVDTP_SUBEVENT_SIGNALING_RECOVERY_CAPABILITY; 859 little_endian_store_16(event, pos, avdtp_cid); 860 pos += 2; 861 event[pos++] = remote_seid; 862 event[pos++] = recovery->recovery_type; 863 event[pos++] = recovery->maximum_recovery_window_size; 864 event[pos++] = recovery->maximum_number_media_packets; 865 avdtp_emit_sink_and_source(event, pos); 866 } 867 868 #define MAX_CONTENT_PROTECTION_VALUE_LEN 32 869 static void 870 avdtp_signaling_emit_content_protection_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_content_protection_t *content_protection) { 871 uint8_t event[10 + MAX_CONTENT_PROTECTION_VALUE_LEN]; 872 int pos = 0; 873 event[pos++] = HCI_EVENT_AVDTP_META; 874 pos++; // set later 875 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CONTENT_PROTECTION_CAPABILITY; 876 little_endian_store_16(event, pos, avdtp_cid); 877 pos += 2; 878 event[pos++] = remote_seid; 879 880 little_endian_store_16(event, pos, content_protection->cp_type); 881 pos += 2; 882 883 // drop cp protection value if longer than expected 884 if (content_protection->cp_type_value_len <= MAX_CONTENT_PROTECTION_VALUE_LEN){ 885 little_endian_store_16(event, pos, content_protection->cp_type_value_len); 886 pos += 2; 887 (void)memcpy(event + pos, content_protection->cp_type_value, content_protection->cp_type_value_len); 888 pos += content_protection->cp_type_value_len; 889 } else { 890 little_endian_store_16(event, pos, 0); 891 pos += 2; 892 } 893 event[1] = pos - 2; 894 avdtp_emit_sink_and_source(event, pos); 895 } 896 897 898 static void 899 avdtp_signaling_emit_header_compression_capability(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_header_compression_capabilities_t *header_compression) { 900 uint8_t event[9]; 901 int pos = 0; 902 event[pos++] = HCI_EVENT_AVDTP_META; 903 event[pos++] = sizeof(event) - 2; 904 event[pos++] = AVDTP_SUBEVENT_SIGNALING_HEADER_COMPRESSION_CAPABILITY; 905 little_endian_store_16(event, pos, avdtp_cid); 906 pos += 2; 907 event[pos++] = remote_seid; 908 event[pos++] = header_compression->back_ch; 909 event[pos++] = header_compression->media; 910 event[pos++] = header_compression->recovery; 911 avdtp_emit_sink_and_source(event, pos); 912 } 913 914 static void 915 avdtp_signaling_emit_content_multiplexing_capability(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_multiplexing_mode_capabilities_t *multiplexing_mode) { 916 uint8_t event[14]; 917 int pos = 0; 918 event[pos++] = HCI_EVENT_AVDTP_META; 919 event[pos++] = sizeof(event) - 2; 920 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MULTIPLEXING_CAPABILITY; 921 little_endian_store_16(event, pos, avdtp_cid); 922 pos += 2; 923 event[pos++] = remote_seid; 924 925 event[pos++] = multiplexing_mode->fragmentation; 926 event[pos++] = multiplexing_mode->transport_identifiers_num; 927 928 int i; 929 for (i = 0; i < 3; i++){ 930 event[pos++] = multiplexing_mode->transport_session_identifiers[i]; 931 } 932 for (i = 0; i < 3; i++){ 933 event[pos++] = multiplexing_mode->tcid[i]; 934 } 935 avdtp_emit_sink_and_source(event, pos); 936 } 937 938 static void avdtp_signaling_emit_capability_done(uint16_t avdtp_cid, uint8_t remote_seid) { 939 uint8_t event[6]; 940 int pos = 0; 941 event[pos++] = HCI_EVENT_AVDTP_META; 942 event[pos++] = sizeof(event) - 2; 943 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CAPABILITIES_DONE; 944 little_endian_store_16(event, pos, avdtp_cid); 945 pos += 2; 946 event[pos++] = remote_seid; 947 avdtp_emit_sink_and_source(event, pos); 948 } 949 950 static void avdtp_signaling_emit_media_codec_capability(uint16_t avdtp_cid, uint8_t remote_seid, adtvp_media_codec_capabilities_t media_codec){ 951 switch (media_codec.media_codec_type){ 952 case AVDTP_CODEC_SBC: 953 avdtp_signaling_emit_media_codec_sbc_capability(avdtp_cid, remote_seid, media_codec); 954 break; 955 case AVDTP_CODEC_MPEG_1_2_AUDIO: 956 avdtp_signaling_emit_media_codec_mpeg_audio_capability(avdtp_cid, remote_seid, media_codec); 957 break; 958 case AVDTP_CODEC_MPEG_2_4_AAC: 959 avdtp_signaling_emit_media_codec_mpeg_aac_capability(avdtp_cid, remote_seid, media_codec); 960 break; 961 case AVDTP_CODEC_ATRAC_FAMILY: 962 avdtp_signaling_emit_media_codec_atrac_capability(avdtp_cid, remote_seid, media_codec); 963 break; 964 case AVDTP_CODEC_MPEG_D_USAC: 965 avdtp_signaling_emit_media_codec_mpeg_d_usac_capability(avdtp_cid, remote_seid, media_codec); 966 break; 967 default: 968 avdtp_signaling_emit_media_codec_other_capability(avdtp_cid, remote_seid, media_codec); 969 break; 970 } 971 } 972 973 // emit events for all capabilities incl. final done event 974 void avdtp_signaling_emit_capabilities(uint16_t avdtp_cid, uint8_t remote_seid, avdtp_capabilities_t *capabilities, 975 uint16_t registered_service_categories) { 976 if (get_bit16(registered_service_categories, AVDTP_MEDIA_CODEC)){ 977 avdtp_signaling_emit_media_codec_capability(avdtp_cid, remote_seid, capabilities->media_codec); 978 } 979 980 if (get_bit16(registered_service_categories, AVDTP_MEDIA_TRANSPORT)){ 981 avdtp_signaling_emit_media_transport_capability(avdtp_cid, remote_seid); 982 } 983 if (get_bit16(registered_service_categories, AVDTP_REPORTING)){ 984 avdtp_signaling_emit_reporting_capability(avdtp_cid, remote_seid); 985 } 986 if (get_bit16(registered_service_categories, AVDTP_RECOVERY)){ 987 avdtp_signaling_emit_recovery_capability(avdtp_cid, remote_seid, &capabilities->recovery); 988 } 989 if (get_bit16(registered_service_categories, AVDTP_CONTENT_PROTECTION)){ 990 avdtp_signaling_emit_content_protection_capability(avdtp_cid, remote_seid, 991 &capabilities->content_protection); 992 } 993 if (get_bit16(registered_service_categories, AVDTP_HEADER_COMPRESSION)){ 994 avdtp_signaling_emit_header_compression_capability(avdtp_cid, remote_seid, 995 &capabilities->header_compression); 996 } 997 if (get_bit16(registered_service_categories, AVDTP_MULTIPLEXING)){ 998 avdtp_signaling_emit_content_multiplexing_capability(avdtp_cid, remote_seid, 999 &capabilities->multiplexing_mode); 1000 } 1001 if (get_bit16(registered_service_categories, AVDTP_DELAY_REPORTING)){ 1002 avdtp_signaling_emit_delay_reporting_capability(avdtp_cid, remote_seid); 1003 } 1004 avdtp_signaling_emit_capability_done(avdtp_cid, remote_seid); 1005 } 1006 1007 static uint16_t 1008 avdtp_signaling_setup_media_codec_sbc_config_event(uint8_t *event, uint16_t size, 1009 const avdtp_stream_endpoint_t *stream_endpoint, 1010 uint16_t avdtp_cid, uint8_t reconfigure, 1011 const uint8_t *media_codec_information) { 1012 1013 btstack_assert(size >= AVDTP_MEDIA_CONFIG_SBC_EVENT_LEN); 1014 1015 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1016 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1017 1018 int pos = 0; 1019 event[pos++] = HCI_EVENT_AVDTP_META; 1020 event[pos++] = AVDTP_MEDIA_CONFIG_SBC_EVENT_LEN - 2; 1021 1022 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION; 1023 little_endian_store_16(event, pos, avdtp_cid); 1024 pos += 2; 1025 event[pos++] = local_seid; 1026 event[pos++] = remote_seid; 1027 event[pos++] = reconfigure; 1028 event[pos++] = AVDTP_CODEC_SBC; 1029 1030 uint8_t sampling_frequency_bitmap = media_codec_information[0] >> 4; 1031 uint8_t channel_mode_bitmap = media_codec_information[0] & 0x0F; 1032 uint8_t block_length_bitmap = media_codec_information[1] >> 4; 1033 uint8_t subbands_bitmap = (media_codec_information[1] & 0x0F) >> 2; 1034 1035 uint8_t num_channels = 0; 1036 avdtp_channel_mode_t channel_mode; 1037 1038 if (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO){ 1039 channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1040 num_channels = 2; 1041 } else if (channel_mode_bitmap & AVDTP_SBC_STEREO){ 1042 channel_mode = AVDTP_CHANNEL_MODE_STEREO; 1043 num_channels = 2; 1044 } else if (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL){ 1045 channel_mode = AVDTP_CHANNEL_MODE_DUAL_CHANNEL; 1046 num_channels = 2; 1047 } else { 1048 channel_mode = AVDTP_CHANNEL_MODE_MONO; 1049 num_channels = 1; 1050 } 1051 1052 uint16_t sampling_frequency = 0; 1053 if (sampling_frequency_bitmap & AVDTP_SBC_48000) { 1054 sampling_frequency = 48000; 1055 } else if (sampling_frequency_bitmap & AVDTP_SBC_44100) { 1056 sampling_frequency = 44100; 1057 } else if (sampling_frequency_bitmap & AVDTP_SBC_32000) { 1058 sampling_frequency = 32000; 1059 } else if (sampling_frequency_bitmap & AVDTP_SBC_16000) { 1060 sampling_frequency = 16000; 1061 } 1062 1063 uint8_t subbands = 0; 1064 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){ 1065 subbands = 8; 1066 } else if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){ 1067 subbands = 4; 1068 } 1069 1070 uint8_t block_length = 0; 1071 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){ 1072 block_length = 16; 1073 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){ 1074 block_length = 12; 1075 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){ 1076 block_length = 8; 1077 } else if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){ 1078 block_length = 4; 1079 } 1080 1081 little_endian_store_16(event, pos, sampling_frequency); 1082 pos += 2; 1083 1084 event[pos++] = (uint8_t) channel_mode; 1085 event[pos++] = num_channels; 1086 event[pos++] = block_length; 1087 event[pos++] = subbands; 1088 event[pos++] = media_codec_information[1] & 0x03; 1089 event[pos++] = media_codec_information[2]; 1090 event[pos++] = media_codec_information[3]; 1091 1092 btstack_assert(pos == AVDTP_MEDIA_CONFIG_SBC_EVENT_LEN); 1093 1094 return pos; 1095 } 1096 1097 static uint16_t 1098 avdtp_signaling_setup_media_codec_mpeg_audio_config_event(uint8_t *event, uint16_t size, 1099 const avdtp_stream_endpoint_t *stream_endpoint, 1100 uint16_t avdtp_cid, uint8_t reconfigure, 1101 const uint8_t *media_codec_information) { 1102 1103 btstack_assert(size >= AVDTP_MEDIA_CONFIG_MPEG_AUDIO_EVENT_LEN); 1104 1105 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1106 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1107 1108 uint16_t pos = 0; 1109 event[pos++] = HCI_EVENT_AVDTP_META; 1110 event[pos++] = AVDTP_MEDIA_CONFIG_MPEG_AUDIO_EVENT_LEN - 2; 1111 1112 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AUDIO_CONFIGURATION; 1113 little_endian_store_16(event, pos, avdtp_cid); 1114 pos += 2; 1115 event[pos++] = local_seid; 1116 event[pos++] = remote_seid; 1117 event[pos++] = reconfigure; 1118 event[pos++] = AVDTP_CODEC_MPEG_1_2_AUDIO; 1119 1120 uint8_t layer_bitmap = media_codec_information[0] >> 5; 1121 uint8_t crc = (media_codec_information[0] >> 4) & 0x01; 1122 uint8_t channel_mode_bitmap = (media_codec_information[0] & 0x07); 1123 uint8_t mpf = (media_codec_information[1] >> 6) & 0x01; 1124 uint8_t sampling_frequency_bitmap = (media_codec_information[1] & 0x3F); 1125 uint8_t vbr = (media_codec_information[2] >> 7) & 0x01; 1126 uint16_t bit_rate_index_bitmap = ((media_codec_information[2] & 0x3f) << 8) | media_codec_information[3]; 1127 1128 uint8_t layer = 0; 1129 if (layer_bitmap & 0x04){ 1130 layer = AVDTP_MPEG_LAYER_1; 1131 } else if (layer_bitmap & 0x02){ 1132 layer = AVDTP_MPEG_LAYER_2; 1133 } else if (layer_bitmap & 0x01){ 1134 layer = AVDTP_MPEG_LAYER_3; 1135 } 1136 1137 uint8_t num_channels = 0; 1138 avdtp_channel_mode_t channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1139 if (channel_mode_bitmap & 0x08){ 1140 num_channels = 1; 1141 channel_mode = AVDTP_CHANNEL_MODE_MONO; 1142 } else if (channel_mode_bitmap & 0x04){ 1143 num_channels = 2; 1144 channel_mode = AVDTP_CHANNEL_MODE_DUAL_CHANNEL; 1145 } else if (channel_mode_bitmap & 0x02){ 1146 num_channels = 2; 1147 channel_mode = AVDTP_CHANNEL_MODE_STEREO; 1148 } else if (channel_mode_bitmap & 0x02){ 1149 num_channels = 2; 1150 channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1151 } 1152 1153 uint16_t sampling_frequency = 0; 1154 if (sampling_frequency_bitmap & 0x01) { 1155 sampling_frequency = 48000; 1156 } else if (sampling_frequency_bitmap & 0x02) { 1157 sampling_frequency = 44100; 1158 } else if (sampling_frequency_bitmap & 0x04) { 1159 sampling_frequency = 32000; 1160 } else if (sampling_frequency_bitmap & 0x08) { 1161 sampling_frequency = 24000; 1162 } else if (sampling_frequency_bitmap & 0x10) { 1163 sampling_frequency = 22050; 1164 } else if (sampling_frequency_bitmap & 0x20) { 1165 sampling_frequency = 16000; 1166 } 1167 1168 uint8_t bitrate_index = 0; 1169 uint8_t i; 1170 for (i=0;i<14;i++){ 1171 if (bit_rate_index_bitmap & (1U << i)) { 1172 bitrate_index = i; 1173 } 1174 } 1175 1176 event[pos++] = (uint8_t) layer; 1177 event[pos++] = crc; 1178 event[pos++] = (uint8_t) channel_mode; 1179 event[pos++] = num_channels; 1180 event[pos++] = mpf; 1181 little_endian_store_16(event, pos, sampling_frequency); 1182 pos += 2; 1183 event[pos++] = vbr; 1184 event[pos++] = bitrate_index; 1185 1186 btstack_assert(pos == AVDTP_MEDIA_CONFIG_MPEG_AUDIO_EVENT_LEN); 1187 1188 return pos; 1189 } 1190 1191 static uint16_t 1192 avdtp_signaling_setup_media_codec_mpec_aac_config_event(uint8_t *event, uint16_t size, 1193 const avdtp_stream_endpoint_t *stream_endpoint, 1194 uint16_t avdtp_cid, uint8_t reconfigure, 1195 const uint8_t *media_codec_information) { 1196 1197 btstack_assert(size >= AVDTP_MEDIA_CONFIG_MPEG_AAC_EVENT_LEN); 1198 1199 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1200 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1201 1202 uint16_t pos = 0; 1203 event[pos++] = HCI_EVENT_AVDTP_META; 1204 event[pos++] = AVDTP_MEDIA_CONFIG_MPEG_AAC_EVENT_LEN - 2; 1205 1206 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_AAC_CONFIGURATION; 1207 little_endian_store_16(event, pos, avdtp_cid); 1208 pos += 2; 1209 event[pos++] = local_seid; 1210 event[pos++] = remote_seid; 1211 event[pos++] = reconfigure; 1212 event[pos++] =AVDTP_CODEC_MPEG_2_4_AAC; 1213 1214 uint8_t object_type_bitmap = media_codec_information[0] >> 1; 1215 uint8_t drc = media_codec_information[0] & 0x01; 1216 uint16_t sampling_frequency_bitmap = (media_codec_information[1] << 4) | (media_codec_information[2] >> 4); 1217 uint8_t channels_bitmap = media_codec_information[2] & 0x0F; 1218 uint8_t vbr = media_codec_information[3] >> 7; 1219 uint32_t bit_rate = ((media_codec_information[3] & 0x7f) << 16) | (media_codec_information[4] << 8) | media_codec_information[5]; 1220 1221 uint8_t object_type = 0; 1222 if (object_type_bitmap & 0x01){ 1223 object_type = AVDTP_AAC_MPEG4_HE_AAC_ELDv2; 1224 } else if (object_type_bitmap & 0x02){ 1225 object_type = AVDTP_AAC_MPEG4_HE_AACv2; 1226 } else if (object_type_bitmap & 0x04){ 1227 object_type = AVDTP_AAC_MPEG4_HE_AAC; 1228 } else if (object_type_bitmap & 0x08){ 1229 object_type = AVDTP_AAC_MPEG4_SCALABLE; 1230 } else if (object_type_bitmap & 0x10){ 1231 object_type = AVDTP_AAC_MPEG4_LTP; 1232 } else if (object_type_bitmap & 0x20){ 1233 object_type = AVDTP_AAC_MPEG4_LC; 1234 } else if (object_type_bitmap & 0x40){ 1235 object_type = AVDTP_AAC_MPEG2_LC; 1236 } 1237 1238 uint32_t sampling_frequency = avdtp_config_get_sampling_frequency_from_table(sampling_frequency_bitmap, aac_sampling_frequency_table, aac_sampling_frequency_table_size); 1239 1240 uint8_t num_channels = 0; 1241 if (channels_bitmap & 0x08){ 1242 num_channels = 1; 1243 } else if (channels_bitmap & 0x04){ 1244 num_channels = 2; 1245 } else if (channels_bitmap & 0x02){ 1246 num_channels = 6; 1247 } else if (channels_bitmap & 0x01){ 1248 num_channels = 8; 1249 } 1250 1251 event[pos++] = object_type; 1252 event[pos++] = drc; 1253 little_endian_store_24(event, pos, sampling_frequency); 1254 pos += 3; 1255 event[pos++] = num_channels; 1256 little_endian_store_24(event, pos, bit_rate); 1257 pos += 3; 1258 event[pos++] = vbr; 1259 1260 btstack_assert(AVDTP_MEDIA_CONFIG_MPEG_AAC_EVENT_LEN == pos); 1261 1262 return pos; 1263 } 1264 1265 static uint16_t 1266 avdtp_signaling_setup_media_codec_mpegd_config_event(uint8_t *event, uint16_t size, 1267 const avdtp_stream_endpoint_t *stream_endpoint, 1268 uint16_t avdtp_cid, uint8_t reconfigure, 1269 const uint8_t *media_codec_information) { 1270 1271 btstack_assert(size >= AVDTP_MEDIA_CONFIG_MPEG_D_USAC_EVENT_LEN); 1272 1273 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1274 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1275 1276 uint16_t pos = 0; 1277 event[pos++] = HCI_EVENT_AVDTP_META; 1278 event[pos++] = AVDTP_MEDIA_CONFIG_MPEG_D_USAC_EVENT_LEN - 2; 1279 1280 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_MPEG_D_USAC_CONFIGURATION; 1281 1282 little_endian_store_16(event, pos, avdtp_cid); 1283 pos += 2; 1284 event[pos++] = local_seid; 1285 event[pos++] = remote_seid; 1286 event[pos++] = reconfigure; 1287 event[pos++] = AVDTP_CODEC_MPEG_D_USAC; 1288 1289 uint8_t object_type_bitmap = media_codec_information[0] >> 6; 1290 uint32_t sampling_frequency_bitmap = ((media_codec_information[0] & 0x3F) << 20) | 1291 (media_codec_information[1] << 12) | 1292 (media_codec_information[2] << 4) | 1293 (media_codec_information[3] >> 4); 1294 1295 uint8_t channels_bitmap = (media_codec_information[3] >> 2) & 0x03; 1296 uint8_t vbr = (media_codec_information[4] >> 7) & 0x01; 1297 1298 uint32_t bit_rate = ((media_codec_information[3] & 0x7f) << 16) | (media_codec_information[4] << 8) | media_codec_information[5]; 1299 1300 uint8_t object_type = 0; 1301 if (object_type_bitmap & 0x10){ 1302 object_type = AVDTP_USAC_OBJECT_TYPE_MPEG_D_DRC; 1303 } else { 1304 object_type = AVDTP_USAC_OBJECT_TYPE_RFU; 1305 } 1306 1307 uint32_t sampling_frequency = avdtp_config_get_sampling_frequency_from_table(sampling_frequency_bitmap, usac_sampling_frequency_table, usac_sampling_frequency_table_size ); 1308 1309 uint8_t num_channels = 0; 1310 if (channels_bitmap & 0x02){ 1311 num_channels = 1; 1312 } else if (channels_bitmap & 0x01){ 1313 num_channels = 2; 1314 } 1315 1316 event[pos++] = object_type; 1317 little_endian_store_24(event, pos, sampling_frequency); 1318 pos += 3; 1319 event[pos++] = num_channels; 1320 event[pos++] = vbr; 1321 little_endian_store_24(event, pos, bit_rate); 1322 pos += 3; 1323 1324 btstack_assert(AVDTP_MEDIA_CONFIG_MPEG_D_USAC_EVENT_LEN == pos); 1325 1326 return pos; 1327 } 1328 1329 static uint16_t avdtp_signaling_setup_media_codec_atrac_config_event(uint8_t *event, uint16_t size, 1330 const avdtp_stream_endpoint_t *stream_endpoint, 1331 uint16_t avdtp_cid, uint8_t reconfigure, 1332 const uint8_t *media_codec_information) { 1333 btstack_assert(size >= AVDTP_MEDIA_CONFIG_ATRAC_EVENT_LEN); 1334 1335 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1336 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1337 1338 uint16_t pos = 0; 1339 event[pos++] = HCI_EVENT_AVDTP_META; 1340 event[pos++] = AVDTP_MEDIA_CONFIG_ATRAC_EVENT_LEN - 2; 1341 1342 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_ATRAC_CONFIGURATION; 1343 little_endian_store_16(event, pos, avdtp_cid); 1344 pos += 2; 1345 event[pos++] = local_seid; 1346 event[pos++] = remote_seid; 1347 event[pos++] = reconfigure; 1348 event[pos++] = AVDTP_CODEC_ATRAC_FAMILY; 1349 1350 avdtp_atrac_version_t version = (avdtp_atrac_version_t) (media_codec_information[0] >> 5); 1351 uint8_t channel_mode_bitmap = (media_codec_information[0] >> 2) & 0x07; 1352 uint16_t sampling_frequency_bitmap = (media_codec_information[1] >> 4) & 0x03; 1353 uint8_t vbr = (media_codec_information[1] >> 3) & 0x01; 1354 uint16_t bit_rate_index_bitmap = ((media_codec_information[1]) & 0x07) << 16 | (media_codec_information[2] << 8) | media_codec_information[3]; 1355 uint16_t maximum_sul = (media_codec_information[4] << 8) | media_codec_information[5]; 1356 1357 uint8_t num_channels = 0; 1358 avdtp_channel_mode_t channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1359 if (channel_mode_bitmap & 0x04){ 1360 num_channels = 1; 1361 channel_mode = AVDTP_CHANNEL_MODE_MONO; 1362 } else if (channel_mode_bitmap & 0x02){ 1363 num_channels = 2; 1364 channel_mode = AVDTP_CHANNEL_MODE_DUAL_CHANNEL; 1365 } else if (channel_mode_bitmap & 0x01){ 1366 num_channels = 2; 1367 channel_mode = AVDTP_CHANNEL_MODE_JOINT_STEREO; 1368 } 1369 1370 uint16_t sampling_frequency = 0; 1371 if (sampling_frequency_bitmap & 0x02){ 1372 sampling_frequency = 44100; 1373 } else if (sampling_frequency_bitmap & 0x01){ 1374 sampling_frequency = 48000; 1375 } 1376 1377 // bit 0 = index 0x18, bit 19 = index 0 1378 uint8_t bit_rate_index = 0; 1379 uint8_t i; 1380 for (i=0;i <= 19;i++){ 1381 if (bit_rate_index_bitmap & (1U << i)) { 1382 bit_rate_index = 18 - i; 1383 } 1384 } 1385 1386 event[pos++] = (uint8_t) version; 1387 event[pos++] = (uint8_t) channel_mode; 1388 event[pos++] = num_channels; 1389 little_endian_store_16(event, pos, sampling_frequency); 1390 pos += 2; 1391 event[pos++] = vbr; 1392 event[pos++] = bit_rate_index; 1393 little_endian_store_16(event, pos, maximum_sul); 1394 pos += 2; 1395 1396 btstack_assert(pos == AVDTP_MEDIA_CONFIG_ATRAC_EVENT_LEN); 1397 return pos; 1398 } 1399 1400 static uint16_t avdtp_signaling_setup_media_codec_other_config_event(uint8_t *event, uint16_t size, 1401 const avdtp_stream_endpoint_t *stream_endpoint, 1402 uint16_t avdtp_cid, uint8_t reconfigure, 1403 const adtvp_media_codec_capabilities_t *media_codec) { 1404 btstack_assert(size >= AVDTP_MEDIA_CONFIG_OTHER_EVENT_LEN); 1405 1406 uint8_t local_seid = avdtp_local_seid(stream_endpoint); 1407 uint8_t remote_seid = avdtp_remote_seid(stream_endpoint); 1408 1409 uint16_t pos = 0; 1410 event[pos++] = HCI_EVENT_AVDTP_META; 1411 pos++; // set later 1412 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION; 1413 little_endian_store_16(event, pos, avdtp_cid); 1414 pos += 2; 1415 event[pos++] = local_seid; 1416 event[pos++] = remote_seid; 1417 event[pos++] = reconfigure; 1418 event[pos++] = media_codec->media_type; 1419 little_endian_store_16(event, pos, media_codec->media_codec_type); 1420 pos += 2; 1421 little_endian_store_16(event, pos, media_codec->media_codec_information_len); 1422 pos += 2; 1423 1424 btstack_assert(pos == 13); 1425 1426 uint16_t media_codec_len = btstack_min(AVDTP_MAX_MEDIA_CODEC_INFORMATION_LENGTH, media_codec->media_codec_information_len); 1427 (void)memcpy(event + pos, media_codec->media_codec_information, media_codec_len); 1428 pos += media_codec_len; 1429 event[1] = pos - 2; 1430 return pos; 1431 } 1432 1433 void avdtp_signaling_emit_delay(uint16_t avdtp_cid, uint8_t local_seid, uint16_t delay) { 1434 uint8_t event[8]; 1435 int pos = 0; 1436 event[pos++] = HCI_EVENT_AVDTP_META; 1437 event[pos++] = sizeof(event) - 2; 1438 event[pos++] = AVDTP_SUBEVENT_SIGNALING_DELAY_REPORT; 1439 little_endian_store_16(event, pos, avdtp_cid); 1440 pos += 2; 1441 event[pos++] = local_seid; 1442 little_endian_store_16(event, pos, delay); 1443 pos += 2; 1444 avdtp_emit_source(event, pos); 1445 } 1446 1447 uint16_t avdtp_setup_media_codec_config_event(uint8_t *event, uint16_t size, const avdtp_stream_endpoint_t *stream_endpoint, 1448 uint16_t avdtp_cid, uint8_t reconfigure, 1449 const adtvp_media_codec_capabilities_t * media_codec) { 1450 switch (media_codec->media_codec_type){ 1451 case AVDTP_CODEC_SBC: 1452 return avdtp_signaling_setup_media_codec_sbc_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1453 media_codec->media_codec_information); 1454 case AVDTP_CODEC_MPEG_1_2_AUDIO: 1455 return avdtp_signaling_setup_media_codec_mpeg_audio_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1456 media_codec->media_codec_information); 1457 case AVDTP_CODEC_MPEG_2_4_AAC: 1458 return avdtp_signaling_setup_media_codec_mpec_aac_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1459 media_codec->media_codec_information); 1460 case AVDTP_CODEC_ATRAC_FAMILY: 1461 return avdtp_signaling_setup_media_codec_atrac_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1462 media_codec->media_codec_information); 1463 case AVDTP_CODEC_MPEG_D_USAC: 1464 return avdtp_signaling_setup_media_codec_mpegd_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1465 media_codec->media_codec_information); 1466 default: 1467 return avdtp_signaling_setup_media_codec_other_config_event(event, size, stream_endpoint, avdtp_cid, reconfigure, 1468 media_codec); 1469 } 1470 } 1471 1472 void avdtp_signaling_emit_configuration(avdtp_stream_endpoint_t *stream_endpoint, uint16_t avdtp_cid, uint8_t reconfigure, 1473 avdtp_capabilities_t *configuration, uint16_t configured_service_categories) { 1474 1475 if (get_bit16(configured_service_categories, AVDTP_MEDIA_CODEC)){ 1476 uint16_t pos = 0; 1477 // assume MEDIA_CONFIG_OTHER_EVENT_LEN is larger than all other events 1478 uint8_t event[AVDTP_MEDIA_CONFIG_OTHER_EVENT_LEN]; 1479 pos = avdtp_setup_media_codec_config_event(event, sizeof(event), stream_endpoint, avdtp_cid, reconfigure, 1480 &configuration->media_codec); 1481 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1482 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1483 } 1484 } 1485 1486 void avdtp_streaming_emit_connection_established(avdtp_stream_endpoint_t *stream_endpoint, uint8_t status) { 1487 uint8_t event[14]; 1488 int pos = 0; 1489 event[pos++] = HCI_EVENT_AVDTP_META; 1490 event[pos++] = sizeof(event) - 2; 1491 event[pos++] = AVDTP_SUBEVENT_STREAMING_CONNECTION_ESTABLISHED; 1492 little_endian_store_16(event, pos, stream_endpoint->connection->avdtp_cid); 1493 pos += 2; 1494 reverse_bd_addr(stream_endpoint->connection->remote_addr, &event[pos]); 1495 pos += 6; 1496 event[pos++] = avdtp_local_seid(stream_endpoint); 1497 event[pos++] = avdtp_remote_seid(stream_endpoint); 1498 event[pos++] = status; 1499 1500 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1501 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1502 } 1503 1504 void avdtp_streaming_emit_connection_released(avdtp_stream_endpoint_t *stream_endpoint, uint16_t avdtp_cid, uint8_t local_seid) { 1505 uint8_t event[6]; 1506 int pos = 0; 1507 event[pos++] = HCI_EVENT_AVDTP_META; 1508 event[pos++] = sizeof(event) - 2; 1509 event[pos++] = AVDTP_SUBEVENT_STREAMING_CONNECTION_RELEASED; 1510 little_endian_store_16(event, pos, avdtp_cid); 1511 pos += 2; 1512 event[pos++] = local_seid; 1513 1514 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1515 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1516 } 1517 1518 void avdtp_streaming_emit_can_send_media_packet_now(avdtp_stream_endpoint_t *stream_endpoint, uint16_t sequence_number) { 1519 uint8_t event[8]; 1520 int pos = 0; 1521 event[pos++] = HCI_EVENT_AVDTP_META; 1522 event[pos++] = sizeof(event) - 2; 1523 event[pos++] = AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW; 1524 little_endian_store_16(event, pos, stream_endpoint->connection->avdtp_cid); 1525 pos += 2; 1526 event[pos++] = avdtp_local_seid(stream_endpoint); 1527 little_endian_store_16(event, pos, sequence_number); 1528 pos += 2; 1529 event[1] = pos - 2; 1530 1531 btstack_packet_handler_t packet_handler = avdtp_packet_handler_for_stream_endpoint(stream_endpoint); 1532 (*packet_handler)(HCI_EVENT_PACKET, 0, event, pos); 1533 } 1534 1535 uint8_t avdtp_request_can_send_now_acceptor(avdtp_connection_t *connection) { 1536 if (!connection) return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1537 connection->wait_to_send_acceptor = true; 1538 l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); 1539 return ERROR_CODE_SUCCESS; 1540 } 1541 1542 uint8_t avdtp_request_can_send_now_initiator(avdtp_connection_t *connection) { 1543 if (!connection) return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; 1544 connection->wait_to_send_initiator = true; 1545 l2cap_request_can_send_now_event(connection->l2cap_signaling_cid); 1546 return ERROR_CODE_SUCCESS; 1547 } 1548 1549 uint8_t avdtp_local_seid(const avdtp_stream_endpoint_t * stream_endpoint){ 1550 if (!stream_endpoint) return 0; 1551 return stream_endpoint->sep.seid; 1552 1553 } 1554 1555 uint8_t avdtp_remote_seid(const avdtp_stream_endpoint_t * stream_endpoint){ 1556 if (!stream_endpoint) return AVDTP_INVALID_SEP_SEID; 1557 return stream_endpoint->remote_sep.seid; 1558 } 1559 1560 // helper to set/get configuration 1561 uint8_t avdtp_config_sbc_set_sampling_frequency(uint8_t * config, uint16_t sampling_frequency_hz){ 1562 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); 1563 if (sampling_frequency_bitmap == 0){ 1564 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1565 } 1566 config[0] = (config[0] & 0x0f) | (uint8_t)(sampling_frequency_bitmap << 4); 1567 return ERROR_CODE_SUCCESS; 1568 } 1569 1570 uint8_t avdtp_config_sbc_store(uint8_t * config, const avdtp_configuration_sbc_t * configuration){ 1571 if (configuration->channel_mode > AVDTP_CHANNEL_MODE_JOINT_STEREO){ 1572 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1573 } 1574 if (configuration->allocation_method > AVDTP_SBC_ALLOCATION_METHOD_SNR){ 1575 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1576 } 1577 switch (configuration->block_length){ 1578 case 4: 1579 case 8: 1580 case 12: 1581 case 16: 1582 break; 1583 default: 1584 return ERROR_CODE_PARAMETER_OUT_OF_MANDATORY_RANGE; 1585 } 1586 switch (configuration->subbands){ 1587 case 4: 1588 case 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 }