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