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