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 MATTHIAS 24 * RINGWALD 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 39 #include <stdint.h> 40 #include <stdio.h> 41 #include <stdlib.h> 42 #include <string.h> 43 #include <unistd.h> 44 45 #include "btstack.h" 46 #include "avdtp.h" 47 #include "avdtp_util.h" 48 49 inline uint8_t avdtp_header(uint8_t tr_label, avdtp_packet_type_t packet_type, avdtp_message_type_t msg_type){ 50 return (tr_label<<4) | ((uint8_t)packet_type<<2) | (uint8_t)msg_type; 51 } 52 53 avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_with_seid(uint8_t seid){ 54 btstack_linked_list_iterator_t it; 55 btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); 56 while (btstack_linked_list_iterator_has_next(&it)){ 57 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 58 if (stream_endpoint->sep.seid == seid){ 59 return stream_endpoint; 60 } 61 } 62 return NULL; 63 } 64 65 avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_associated_with_acp_seid(uint16_t acp_seid){ 66 btstack_linked_list_iterator_t it; 67 btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); 68 while (btstack_linked_list_iterator_has_next(&it)){ 69 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 70 if (stream_endpoint->remote_sep_index >= 0 && stream_endpoint->remote_sep_index < MAX_NUM_SEPS){ 71 if (stream_endpoint->remote_seps[stream_endpoint->remote_sep_index].seid == acp_seid){ 72 return stream_endpoint; 73 } 74 } 75 } 76 return NULL; 77 } 78 79 int get_bit16(uint16_t bitmap, int position){ 80 return (bitmap >> position) & 1; 81 } 82 83 uint8_t store_bit16(uint16_t bitmap, int position, uint8_t value){ 84 if (value){ 85 bitmap |= 1 << position; 86 } else { 87 bitmap &= ~ (1 << position); 88 } 89 return bitmap; 90 } 91 92 int avdtp_read_signaling_header(avdtp_signaling_packet_t * signaling_header, uint8_t * packet, uint16_t size){ 93 int pos = 0; 94 if (size < 2) return pos; 95 signaling_header->transaction_label = packet[pos] >> 4; 96 signaling_header->packet_type = (avdtp_packet_type_t)((packet[pos] >> 2) & 0x03); 97 signaling_header->message_type = (avdtp_message_type_t) (packet[pos] & 0x03); 98 pos++; 99 memset(signaling_header->command, 0, sizeof(signaling_header->command)); 100 switch (signaling_header->packet_type){ 101 case AVDTP_SINGLE_PACKET: 102 signaling_header->num_packets = 0; 103 signaling_header->offset = 0; 104 signaling_header->size = 0; 105 break; 106 case AVDTP_END_PACKET: 107 signaling_header->num_packets = 0; 108 break; 109 case AVDTP_START_PACKET: 110 signaling_header->num_packets = packet[pos++]; 111 signaling_header->size = 0; 112 signaling_header->offset = 0; 113 break; 114 case AVDTP_CONTINUE_PACKET: 115 if (signaling_header->num_packets <= 0) { 116 printf(" ERROR: wrong num fragmented packets\n"); 117 break; 118 } 119 signaling_header->num_packets--; 120 break; 121 } 122 signaling_header->signal_identifier = packet[pos++] & 0x3f; 123 return pos; 124 } 125 126 int avdtp_pack_service_capabilities(uint8_t * buffer, int size, avdtp_capabilities_t caps, avdtp_service_category_t category, uint8_t pack_all_capabilities){ 127 UNUSED(size); 128 129 int i; 130 // pos = 0 reserved for length 131 int pos = 1; 132 switch(category){ 133 case AVDTP_MEDIA_TRANSPORT: 134 case AVDTP_REPORTING: 135 break; 136 case AVDTP_DELAY_REPORTING: 137 if (!pack_all_capabilities) break; 138 break; 139 case AVDTP_RECOVERY: 140 buffer[pos++] = caps.recovery.recovery_type; // 0x01=RFC2733 141 buffer[pos++] = caps.recovery.maximum_recovery_window_size; 142 buffer[pos++] = caps.recovery.maximum_number_media_packets; 143 break; 144 case AVDTP_CONTENT_PROTECTION: 145 buffer[pos++] = caps.content_protection.cp_type_value_len + 2; 146 big_endian_store_16(buffer, pos, caps.content_protection.cp_type); 147 pos += 2; 148 memcpy(buffer+pos, caps.content_protection.cp_type_value, caps.content_protection.cp_type_value_len); 149 break; 150 case AVDTP_HEADER_COMPRESSION: 151 buffer[pos++] = (caps.header_compression.back_ch << 7) | (caps.header_compression.media << 6) | (caps.header_compression.recovery << 5); 152 break; 153 case AVDTP_MULTIPLEXING: 154 buffer[pos++] = caps.multiplexing_mode.fragmentation << 7; 155 for (i=0; i<caps.multiplexing_mode.transport_identifiers_num; i++){ 156 buffer[pos++] = caps.multiplexing_mode.transport_session_identifiers[i] << 7; 157 buffer[pos++] = caps.multiplexing_mode.tcid[i] << 7; 158 // media, reporting. recovery 159 } 160 break; 161 case AVDTP_MEDIA_CODEC: 162 buffer[pos++] = ((uint8_t)caps.media_codec.media_type) << 4; 163 buffer[pos++] = (uint8_t)caps.media_codec.media_codec_type; 164 for (i = 0; i<caps.media_codec.media_codec_information_len; i++){ 165 buffer[pos++] = caps.media_codec.media_codec_information[i]; 166 } 167 break; 168 default: 169 break; 170 } 171 buffer[0] = pos - 1; // length 172 return pos; 173 } 174 175 static int avdtp_unpack_service_capabilities_has_errors(avdtp_connection_t * connection, avdtp_service_category_t category, uint8_t cap_len){ 176 connection->error_code = 0; 177 178 if (category == AVDTP_SERVICE_CATEGORY_INVALID_0 || 179 (category == AVDTP_SERVICE_CATEGORY_INVALID_FF && connection->signaling_packet.signal_identifier == AVDTP_SI_RECONFIGURE)){ 180 printf(" ERROR: BAD SERVICE CATEGORY %d\n", category); 181 connection->reject_service_category = category; 182 connection->error_code = BAD_SERV_CATEGORY; 183 return 1; 184 } 185 186 if (connection->signaling_packet.signal_identifier == AVDTP_SI_RECONFIGURE){ 187 if (category != AVDTP_CONTENT_PROTECTION && category != AVDTP_MEDIA_CODEC){ 188 printf(" ERROR: REJECT CATEGORY, INVALID_CAPABILITIES\n"); 189 connection->reject_service_category = category; 190 connection->error_code = INVALID_CAPABILITIES; 191 return 1; 192 } 193 } 194 195 switch(category){ 196 case AVDTP_MEDIA_TRANSPORT: 197 if (cap_len != 0){ 198 printf(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); 199 connection->reject_service_category = category; 200 connection->error_code = BAD_MEDIA_TRANSPORT_FORMAT; 201 return 1; 202 } 203 break; 204 case AVDTP_REPORTING: 205 case AVDTP_DELAY_REPORTING: 206 if (cap_len != 0){ 207 printf(" ERROR: REJECT CATEGORY, BAD_LENGTH\n"); 208 connection->reject_service_category = category; 209 connection->error_code = BAD_LENGTH; 210 return 1; 211 } 212 break; 213 case AVDTP_RECOVERY: 214 if (cap_len < 3){ 215 printf(" ERROR: REJECT CATEGORY, BAD_MEDIA_TRANSPORT\n"); 216 connection->reject_service_category = category; 217 connection->error_code = BAD_RECOVERY_FORMAT; 218 return 1; 219 } 220 break; 221 case AVDTP_CONTENT_PROTECTION: 222 if (cap_len < 2){ 223 printf(" ERROR: REJECT CATEGORY, BAD_CP_FORMAT\n"); 224 connection->reject_service_category = category; 225 connection->error_code = BAD_CP_FORMAT; 226 return 1; 227 } 228 break; 229 case AVDTP_HEADER_COMPRESSION: 230 break; 231 case AVDTP_MULTIPLEXING: 232 break; 233 case AVDTP_MEDIA_CODEC: 234 break; 235 default: 236 break; 237 } 238 return 0; 239 } 240 241 uint16_t avdtp_unpack_service_capabilities(avdtp_connection_t * connection, avdtp_capabilities_t * caps, uint8_t * packet, uint16_t size){ 242 if (size == 0) return 0; 243 244 uint16_t registered_service_categories = 0; 245 int pos = 0; 246 int i; 247 avdtp_service_category_t category = (avdtp_service_category_t)packet[pos++]; 248 uint8_t cap_len = packet[pos++]; 249 250 if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; 251 int processed_cap_len = 0; 252 int rfa = 0; 253 //printf(" size %d, cat size %d\n", size, cap_len); 254 255 while (pos < size){ 256 if (cap_len > size - pos){ 257 connection->reject_service_category = category; 258 connection->error_code = BAD_LENGTH; 259 return 0; 260 } 261 rfa = 0; 262 processed_cap_len = pos; 263 switch(category){ 264 case AVDTP_RECOVERY: 265 caps->recovery.recovery_type = packet[pos++]; 266 caps->recovery.maximum_recovery_window_size = packet[pos++]; 267 caps->recovery.maximum_number_media_packets = packet[pos++]; 268 break; 269 case AVDTP_CONTENT_PROTECTION: 270 caps->content_protection.cp_type = big_endian_read_16(packet, pos); 271 pos+=2; 272 273 caps->content_protection.cp_type_value_len = cap_len - 2; 274 pos += caps->content_protection.cp_type_value_len; 275 276 // connection->reject_service_category = category; 277 // connection->error_code = UNSUPPORTED_CONFIGURATION; 278 // support for content protection goes here 279 break; 280 281 case AVDTP_HEADER_COMPRESSION: 282 caps->header_compression.back_ch = packet[pos] >> 7; 283 caps->header_compression.media = packet[pos] >> 6; 284 caps->header_compression.recovery = packet[pos] >> 5; 285 pos++; 286 break; 287 case AVDTP_MULTIPLEXING: 288 caps->multiplexing_mode.fragmentation = packet[pos++] >> 7; 289 // read [tsid, tcid] for media, reporting. recovery respectively 290 caps->multiplexing_mode.transport_identifiers_num = 3; 291 for (i=0; i<caps->multiplexing_mode.transport_identifiers_num; i++){ 292 caps->multiplexing_mode.transport_session_identifiers[i] = packet[pos++] >> 7; 293 caps->multiplexing_mode.tcid[i] = packet[pos++] >> 7; 294 } 295 break; 296 case AVDTP_MEDIA_CODEC: 297 caps->media_codec.media_type = packet[pos++] >> 4; 298 caps->media_codec.media_codec_type = packet[pos++]; 299 caps->media_codec.media_codec_information_len = cap_len - 2; 300 caps->media_codec.media_codec_information = &packet[pos]; 301 pos += caps->media_codec.media_codec_information_len; 302 break; 303 case AVDTP_MEDIA_TRANSPORT: 304 case AVDTP_REPORTING: 305 case AVDTP_DELAY_REPORTING: 306 pos += cap_len; 307 break; 308 default: 309 pos += cap_len; 310 rfa = 1; 311 break; 312 } 313 processed_cap_len = pos - processed_cap_len; 314 315 if (cap_len == processed_cap_len){ 316 if (!rfa) { 317 registered_service_categories = store_bit16(registered_service_categories, category, 1); 318 } 319 if (pos < size-2){ 320 //int old_pos = pos; 321 category = (avdtp_service_category_t)packet[pos++]; 322 cap_len = packet[pos++]; 323 if (avdtp_unpack_service_capabilities_has_errors(connection, category, cap_len)) return 0; 324 //printf("category %d, pos %d + 2 + %d -> %d\n", category, old_pos, cap_len, pos + cap_len); 325 //printf_hexdump(packet+old_pos, size-old_pos); 326 } 327 } 328 } 329 return registered_service_categories; 330 } 331 332 void avdtp_prepare_capabilities(avdtp_signaling_packet_t * signaling_packet, uint8_t transaction_label, uint16_t registered_service_categories, avdtp_capabilities_t capabilities, uint8_t identifier){ 333 if (signaling_packet->offset) return; 334 uint8_t pack_all_capabilities = 1; 335 signaling_packet->message_type = AVDTP_RESPONSE_ACCEPT_MSG; 336 signaling_packet->size = 0; 337 int i; 338 signaling_packet->command[signaling_packet->size++] = signaling_packet->acp_seid << 2; 339 340 switch (identifier) { 341 case AVDTP_SI_GET_CAPABILITIES: 342 pack_all_capabilities = 0; 343 break; 344 case AVDTP_SI_GET_ALL_CAPABILITIES: 345 pack_all_capabilities = 1; 346 break; 347 case AVDTP_SI_SET_CONFIGURATION: 348 signaling_packet->command[signaling_packet->size++] = signaling_packet->int_seid << 2; 349 signaling_packet->message_type = AVDTP_CMD_MSG; 350 break; 351 case AVDTP_SI_RECONFIGURE: 352 signaling_packet->message_type = AVDTP_CMD_MSG; 353 break; 354 default: 355 log_error("avdtp_prepare_capabilities wrong identifier %d", identifier); 356 break; 357 } 358 359 for (i = 1; i < 9; i++){ 360 if (get_bit16(registered_service_categories, i)){ 361 // service category 362 signaling_packet->command[signaling_packet->size++] = i; 363 signaling_packet->size += avdtp_pack_service_capabilities(signaling_packet->command+signaling_packet->size, sizeof(signaling_packet->command)-signaling_packet->size, capabilities, (avdtp_service_category_t)i, pack_all_capabilities); 364 } 365 } 366 // signaling_packet->command[signaling_packet->size++] = 0x04; 367 // signaling_packet->command[signaling_packet->size++] = 0x02; 368 // signaling_packet->command[signaling_packet->size++] = 0x02; 369 // signaling_packet->command[signaling_packet->size++] = 0x00; 370 371 signaling_packet->signal_identifier = identifier; 372 signaling_packet->transaction_label = transaction_label; 373 } 374 375 int avdtp_signaling_create_fragment(uint16_t cid, avdtp_signaling_packet_t * signaling_packet, uint8_t * out_buffer) { 376 int mtu = l2cap_get_remote_mtu_for_local_cid(cid); 377 // hack for test 378 // int mtu = 6; 379 int data_len = 0; 380 381 uint16_t offset = signaling_packet->offset; 382 uint16_t pos = 1; 383 // printf(" avdtp_signaling_create_fragment offset %d, packet type %d\n", signaling_packet->offset, signaling_packet->packet_type); 384 385 if (offset == 0){ 386 if (signaling_packet->size <= mtu - 2){ 387 // printf(" AVDTP_SINGLE_PACKET\n"); 388 signaling_packet->packet_type = AVDTP_SINGLE_PACKET; 389 out_buffer[pos++] = signaling_packet->signal_identifier; 390 data_len = signaling_packet->size; 391 } else { 392 signaling_packet->packet_type = AVDTP_START_PACKET; 393 out_buffer[pos++] = (mtu + signaling_packet->size)/ (mtu-1); 394 out_buffer[pos++] = signaling_packet->signal_identifier; 395 data_len = mtu - 3; 396 signaling_packet->offset = data_len; 397 // printf(" AVDTP_START_PACKET len %d, offset %d\n", signaling_packet->size, signaling_packet->offset); 398 } 399 } else { 400 int remaining_bytes = signaling_packet->size - offset; 401 if (remaining_bytes <= mtu - 1){ 402 //signaling_packet->fragmentation = 1; 403 signaling_packet->packet_type = AVDTP_END_PACKET; 404 data_len = remaining_bytes; 405 signaling_packet->offset = 0; 406 // printf(" AVDTP_END_PACKET len %d, offset %d\n", signaling_packet->size, signaling_packet->offset); 407 } else{ 408 signaling_packet->packet_type = AVDTP_CONTINUE_PACKET; 409 data_len = mtu - 1; 410 signaling_packet->offset += data_len; 411 // printf(" AVDTP_CONTINUE_PACKET len %d, offset %d\n", signaling_packet->size, signaling_packet->offset); 412 } 413 } 414 out_buffer[0] = avdtp_header(signaling_packet->transaction_label, signaling_packet->packet_type, signaling_packet->message_type); 415 memcpy(out_buffer+pos, signaling_packet->command + offset, data_len); 416 pos += data_len; 417 return pos; 418 } 419 420 421 void avdtp_signaling_emit_connection_established(btstack_packet_handler_t callback, uint16_t con_handle, bd_addr_t addr, uint8_t status){ 422 if (!callback) return; 423 uint8_t event[12]; 424 int pos = 0; 425 event[pos++] = HCI_EVENT_AVDTP_META; 426 event[pos++] = sizeof(event) - 2; 427 event[pos++] = AVDTP_SUBEVENT_SIGNALING_CONNECTION_ESTABLISHED; 428 little_endian_store_16(event, pos, con_handle); 429 pos += 2; 430 reverse_bd_addr(addr,&event[pos]); 431 pos += 6; 432 event[pos++] = status; 433 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 434 } 435 436 void avdtp_signaling_emit_sep(btstack_packet_handler_t callback, uint16_t con_handle, avdtp_sep_t sep){ 437 if (!callback) return; 438 uint8_t event[9]; 439 int pos = 0; 440 event[pos++] = HCI_EVENT_AVDTP_META; 441 event[pos++] = sizeof(event) - 2; 442 event[pos++] = AVDTP_SUBEVENT_SIGNALING_SEP_FOUND; 443 little_endian_store_16(event, pos, con_handle); 444 pos += 2; 445 event[pos++] = sep.seid; 446 event[pos++] = sep.in_use; 447 event[pos++] = sep.media_type; 448 event[pos++] = sep.type; 449 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 450 } 451 452 void avdtp_signaling_emit_accept(btstack_packet_handler_t callback, uint16_t con_handle, avdtp_signal_identifier_t identifier, uint8_t status){ 453 if (!callback) return; 454 uint8_t event[7]; 455 int pos = 0; 456 event[pos++] = HCI_EVENT_AVDTP_META; 457 event[pos++] = sizeof(event) - 2; 458 event[pos++] = AVDTP_SUBEVENT_SIGNALING_ACCEPT; 459 little_endian_store_16(event, pos, con_handle); 460 pos += 2; 461 event[pos++] = identifier; 462 event[pos++] = status; 463 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 464 } 465 466 void avdtp_signaling_emit_reject(btstack_packet_handler_t callback, uint16_t con_handle, avdtp_signal_identifier_t identifier){ 467 if (!callback) return; 468 uint8_t event[6]; 469 int pos = 0; 470 event[pos++] = HCI_EVENT_AVDTP_META; 471 event[pos++] = sizeof(event) - 2; 472 event[pos++] = AVDTP_SUBEVENT_SIGNALING_REJECT; 473 little_endian_store_16(event, pos, con_handle); 474 pos += 2; 475 event[pos++] = identifier; 476 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 477 } 478 479 void avdtp_signaling_emit_general_reject(btstack_packet_handler_t callback, uint16_t con_handle, avdtp_signal_identifier_t identifier){ 480 if (!callback) return; 481 uint8_t event[6]; 482 int pos = 0; 483 event[pos++] = HCI_EVENT_AVDTP_META; 484 event[pos++] = sizeof(event) - 2; 485 event[pos++] = AVDTP_SUBEVENT_SIGNALING_GENERAL_REJECT; 486 little_endian_store_16(event, pos, con_handle); 487 pos += 2; 488 event[pos++] = identifier; 489 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 490 } 491 492 void avdtp_signaling_emit_media_codec_sbc_capability(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec){ 493 if (!callback) return; 494 uint8_t event[13]; 495 int pos = 0; 496 event[pos++] = HCI_EVENT_AVDTP_META; 497 event[pos++] = sizeof(event) - 2; 498 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CAPABILITY; 499 little_endian_store_16(event, pos, con_handle); 500 pos += 2; 501 event[pos++] = media_codec.media_type; 502 event[pos++] = media_codec.media_codec_information[0] >> 4; 503 event[pos++] = media_codec.media_codec_information[0] & 0x0F; 504 event[pos++] = media_codec.media_codec_information[1] >> 4; 505 event[pos++] = (media_codec.media_codec_information[1] & 0x0F) >> 2; 506 event[pos++] = media_codec.media_codec_information[1] & 0x03; 507 event[pos++] = media_codec.media_codec_information[2]; 508 event[pos++] = media_codec.media_codec_information[3]; 509 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 510 } 511 512 void avdtp_signaling_emit_media_codec_other_capability(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec){ 513 if (!callback) return; 514 uint8_t event[109]; 515 int pos = 0; 516 event[pos++] = HCI_EVENT_AVDTP_META; 517 event[pos++] = sizeof(event) - 2; 518 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CAPABILITY; 519 little_endian_store_16(event, pos, con_handle); 520 pos += 2; 521 event[pos++] = media_codec.media_type; 522 little_endian_store_16(event, pos, media_codec.media_codec_type); 523 pos += 2; 524 little_endian_store_16(event, pos, media_codec.media_codec_information_len); 525 pos += 2; 526 if (media_codec.media_codec_information_len < 100){ 527 memcpy(event+pos, media_codec.media_codec_information, media_codec.media_codec_information_len); 528 } else { 529 memcpy(event+pos, media_codec.media_codec_information, 100); 530 } 531 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 532 } 533 534 static inline void avdtp_signaling_emit_media_codec_sbc(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec, uint8_t reconfigure){ 535 if (!callback) return; 536 uint8_t event[14+2]; 537 int pos = 0; 538 event[pos++] = HCI_EVENT_AVDTP_META; 539 event[pos++] = sizeof(event) - 2; 540 541 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_SBC_CONFIGURATION; 542 little_endian_store_16(event, pos, con_handle); 543 pos += 2; 544 event[pos++] = reconfigure; 545 546 uint8_t num_channels = 0; 547 uint16_t sampling_frequency = 0; 548 uint8_t subbands = 0; 549 uint8_t block_length = 0; 550 551 uint8_t sampling_frequency_bitmap = media_codec.media_codec_information[0] >> 4; 552 uint8_t channel_mode_bitmap = media_codec.media_codec_information[0] & 0x0F; 553 uint8_t block_length_bitmap = media_codec.media_codec_information[1] >> 4; 554 uint8_t subbands_bitmap = (media_codec.media_codec_information[1] & 0x0F) >> 2; 555 556 if (channel_mode_bitmap & AVDTP_SBC_MONO){ 557 num_channels = 1; 558 } 559 if ( (channel_mode_bitmap & AVDTP_SBC_JOINT_STEREO) || 560 (channel_mode_bitmap & AVDTP_SBC_STEREO) || 561 (channel_mode_bitmap & AVDTP_SBC_DUAL_CHANNEL) ){ 562 num_channels = 2; 563 } 564 565 if (sampling_frequency_bitmap & AVDTP_SBC_16000){ 566 sampling_frequency = 16000; 567 } 568 if (sampling_frequency_bitmap & AVDTP_SBC_32000){ 569 sampling_frequency = 32000; 570 } 571 if (sampling_frequency_bitmap & AVDTP_SBC_44100){ 572 sampling_frequency = 44100; 573 } 574 if (sampling_frequency_bitmap & AVDTP_SBC_48000){ 575 sampling_frequency = 48000; 576 } 577 578 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_4){ 579 subbands = 4; 580 } 581 if (subbands_bitmap & AVDTP_SBC_SUBBANDS_8){ 582 subbands = 8; 583 } 584 585 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_4){ 586 block_length = 4; 587 } 588 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_8){ 589 block_length = 8; 590 } 591 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_12){ 592 block_length = 12; 593 } 594 if (block_length_bitmap & AVDTP_SBC_BLOCK_LENGTH_16){ 595 block_length = 16; 596 } 597 598 event[pos++] = media_codec.media_type; 599 little_endian_store_16(event, pos, sampling_frequency); 600 pos += 2; 601 602 event[pos++] = channel_mode_bitmap; 603 event[pos++] = num_channels; 604 event[pos++] = block_length; 605 event[pos++] = subbands; 606 event[pos++] = media_codec.media_codec_information[1] & 0x03; 607 event[pos++] = media_codec.media_codec_information[2]; 608 event[pos++] = media_codec.media_codec_information[3]; 609 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 610 } 611 612 void avdtp_signaling_emit_media_codec_sbc_configuration(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec){ 613 if (!callback) return; 614 avdtp_signaling_emit_media_codec_sbc(callback, con_handle, media_codec, 0); 615 } 616 617 void avdtp_signaling_emit_media_codec_sbc_reconfiguration(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec){ 618 if (!callback) return; 619 avdtp_signaling_emit_media_codec_sbc(callback, con_handle, media_codec, 1); 620 } 621 622 static inline void avdtp_signaling_emit_media_codec_other(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec, uint8_t reconfigure){ 623 uint8_t event[110]; 624 int pos = 0; 625 event[pos++] = HCI_EVENT_AVDTP_META; 626 event[pos++] = sizeof(event) - 2; 627 event[pos++] = AVDTP_SUBEVENT_SIGNALING_MEDIA_CODEC_OTHER_CONFIGURATION; 628 little_endian_store_16(event, pos, con_handle); 629 pos += 2; 630 631 event[pos++] = reconfigure; 632 633 event[pos++] = media_codec.media_type; 634 little_endian_store_16(event, pos, media_codec.media_codec_type); 635 pos += 2; 636 little_endian_store_16(event, pos, media_codec.media_codec_information_len); 637 pos += 2; 638 639 if (media_codec.media_codec_information_len < 100){ 640 memcpy(event+pos, media_codec.media_codec_information, media_codec.media_codec_information_len); 641 } else { 642 memcpy(event+pos, media_codec.media_codec_information, 100); 643 } 644 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 645 } 646 647 void avdtp_signaling_emit_media_codec_other_configuration(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec){ 648 if (!callback) return; 649 avdtp_signaling_emit_media_codec_other(callback, con_handle, media_codec, 0); 650 } 651 652 void avdtp_signaling_emit_media_codec_other_reconfiguration(btstack_packet_handler_t callback, uint16_t con_handle, adtvp_media_codec_capabilities_t media_codec){ 653 if (!callback) return; 654 avdtp_signaling_emit_media_codec_other(callback, con_handle, media_codec, 1); 655 } 656 657 658 void avdtp_sink_request_can_send_now_acceptor(avdtp_connection_t * connection, uint16_t l2cap_cid){ 659 connection->wait_to_send_acceptor = 1; 660 l2cap_request_can_send_now_event(l2cap_cid); 661 } 662 void avdtp_sink_request_can_send_now_initiator(avdtp_connection_t * connection, uint16_t l2cap_cid){ 663 connection->wait_to_send_initiator = 1; 664 l2cap_request_can_send_now_event(l2cap_cid); 665 } 666 void avdtp_sink_request_can_send_now_self(avdtp_connection_t * connection, uint16_t l2cap_cid){ 667 connection->wait_to_send_self = 1; 668 l2cap_request_can_send_now_event(l2cap_cid); 669 } 670 671 avdtp_stream_endpoint_t * get_avdtp_stream_endpoint_for_seid(uint16_t seid){ 672 btstack_linked_list_iterator_t it; 673 btstack_linked_list_iterator_init(&it, (btstack_linked_list_t *) &stream_endpoints); 674 while (btstack_linked_list_iterator_has_next(&it)){ 675 avdtp_stream_endpoint_t * stream_endpoint = (avdtp_stream_endpoint_t *)btstack_linked_list_iterator_next(&it); 676 if (stream_endpoint->sep.seid == seid){ 677 return stream_endpoint; 678 } 679 } 680 return NULL; 681 } 682 683 uint8_t avdtp_get_index_of_remote_stream_endpoint_with_seid(avdtp_stream_endpoint_t * stream_endpoint, uint16_t seid){ 684 if (stream_endpoint->remote_seps[stream_endpoint->remote_sep_index].seid == seid){ 685 return stream_endpoint->remote_sep_index; 686 } 687 int i; 688 for (i=0; i < stream_endpoint->remote_seps_num; i++){ 689 if (stream_endpoint->remote_seps[i].seid == seid){ 690 return i; 691 } 692 } 693 return 0xFF; 694 } 695