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