1 /****************************************************************************** 2 * 3 * Copyright 2018 The Android Open Source Project 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at: 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 * 17 ******************************************************************************/ 18 19 #include "bta_hearing_aid_api.h" 20 21 #include "bta_gatt_api.h" 22 #include "bta_gatt_queue.h" 23 #include "btm_int.h" 24 #include "device/include/controller.h" 25 #include "embdrv/g722/g722_enc_dec.h" 26 #include "gap_api.h" 27 #include "gatt_api.h" 28 29 #include <base/bind.h> 30 #include <base/logging.h> 31 #include <base/strings/string_number_conversions.h> 32 #include <hardware/bt_hearing_aid.h> 33 #include <vector> 34 35 using base::Closure; 36 using bluetooth::Uuid; 37 using bluetooth::hearing_aid::ConnectionState; 38 39 void btif_storage_add_hearing_aid(const RawAddress& address, uint16_t psm, 40 uint8_t capabilities, uint16_t codecs, 41 uint16_t audio_control_point_handle, 42 uint16_t volume_handle, uint64_t hiSyncId, 43 uint16_t render_delay, 44 uint16_t preparation_delay); 45 46 constexpr uint8_t CODEC_G722_16KHZ = 0x01; 47 constexpr uint8_t CODEC_G722_24KHZ = 0x02; 48 49 // Masks for checking capability support 50 constexpr uint8_t CAPABILITY_SIDE = 0x01; 51 constexpr uint8_t CAPABILITY_BINAURAL = 0x02; 52 constexpr uint8_t CAPABILITY_RESERVED = 0xFC; 53 54 // audio control point opcodes 55 constexpr uint8_t CONTROL_POINT_OP_START = 0x01; 56 constexpr uint8_t CONTROL_POINT_OP_STOP = 0x02; 57 58 // used to mark current_volume as not yet known, or possibly old 59 constexpr int8_t VOLUME_UNKNOWN = 127; 60 constexpr int8_t VOLUME_MIN = -127; 61 62 namespace { 63 64 // clang-format off 65 Uuid HEARING_AID_UUID = Uuid::FromString("FDF0"); 66 Uuid READ_ONLY_PROPERTIES_UUID = Uuid::FromString("6333651e-c481-4a3e-9169-7c902aad37bb"); 67 Uuid AUDIO_CONTROL_POINT_UUID = Uuid::FromString("f0d4de7e-4a88-476c-9d9f-1937b0996cc0"); 68 Uuid AUDIO_STATUS_UUID = Uuid::FromString("38663f1a-e711-4cac-b641-326b56404837"); 69 Uuid VOLUME_UUID = Uuid::FromString("00e4ca9e-ab14-41e4-8823-f9e70c7e91df"); 70 Uuid LE_PSM_UUID = Uuid::FromString("2d410339-82b6-42aa-b34e-e2e01df8cc1a"); 71 // clang-format on 72 73 constexpr uint16_t MIN_CE_LEN_1M = 0x0006; 74 75 void hearingaid_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data); 76 void encryption_callback(const RawAddress*, tGATT_TRANSPORT, void*, 77 tBTM_STATUS); 78 79 inline BT_HDR* malloc_l2cap_buf(uint16_t len) { 80 BT_HDR* msg = (BT_HDR*)osi_malloc(BT_HDR_SIZE + L2CAP_MIN_OFFSET + 81 len /* LE-only, no need for FCS here */); 82 msg->offset = L2CAP_MIN_OFFSET; 83 msg->len = len; 84 return msg; 85 } 86 87 inline uint8_t* get_l2cap_sdu_start_ptr(BT_HDR* msg) { 88 return (uint8_t*)(msg) + BT_HDR_SIZE + L2CAP_MIN_OFFSET; 89 } 90 91 struct AudioStats { 92 size_t packet_flush_count; 93 size_t packet_send_count; 94 size_t frame_flush_count; 95 size_t frame_send_count; 96 97 AudioStats() { Reset(); } 98 99 void Reset() { 100 packet_flush_count = 0; 101 packet_send_count = 0; 102 frame_flush_count = 0; 103 frame_send_count = 0; 104 } 105 }; 106 107 class HearingAidImpl; 108 HearingAidImpl* instance; 109 HearingAidAudioReceiver* audioReceiver; 110 111 struct HearingDevice { 112 RawAddress address; 113 /* This is true only during first connection to profile, until we store the 114 * device */ 115 bool first_connection; 116 117 /* we are making active attempt to connect to this device, 'direct connect'. 118 * This is true only during initial phase of first connection. */ 119 bool connecting_actively; 120 121 /* For two hearing aids, you must update their parameters one after another, 122 * not simulteanously, to ensure start of connection events for both devices 123 * are far from each other. This flag means that this device is waiting for 124 * update of parameters, that should happen after "LE Connection Update 125 * Complete" event 126 */ 127 bool connection_update_pending; 128 129 /* if true, we are connected, L2CAP socket is open, we can stream audio*/ 130 bool accepting_audio; 131 132 uint16_t conn_id; 133 uint16_t gap_handle; 134 uint16_t audio_control_point_handle; 135 uint16_t volume_handle; 136 uint16_t psm; 137 138 uint8_t capabilities; 139 uint64_t hi_sync_id; 140 uint16_t render_delay; 141 uint16_t preparation_delay; 142 uint16_t codecs; 143 144 AudioStats audio_stats; 145 146 HearingDevice(const RawAddress& address, uint16_t psm, uint8_t capabilities, 147 uint16_t codecs, uint16_t audio_control_point_handle, 148 uint16_t volume_handle, uint64_t hiSyncId, 149 uint16_t render_delay, uint16_t preparation_delay) 150 : address(address), 151 first_connection(false), 152 connecting_actively(false), 153 connection_update_pending(false), 154 accepting_audio(false), 155 conn_id(0), 156 gap_handle(0), 157 audio_control_point_handle(audio_control_point_handle), 158 volume_handle(volume_handle), 159 psm(psm), 160 capabilities(capabilities), 161 hi_sync_id(hiSyncId), 162 render_delay(render_delay), 163 preparation_delay(preparation_delay), 164 codecs(codecs) {} 165 166 HearingDevice(const RawAddress& address, bool first_connection) 167 : address(address), 168 first_connection(first_connection), 169 connecting_actively(first_connection), 170 connection_update_pending(false), 171 accepting_audio(false), 172 conn_id(0), 173 gap_handle(0), 174 psm(0) {} 175 176 HearingDevice() { HearingDevice(RawAddress::kEmpty, false); } 177 178 /* return true if this device represents left Hearing Aid. Returned value is 179 * valid only after capabilities are discovered */ 180 bool isLeft() const { return !(capabilities & CAPABILITY_SIDE); } 181 }; 182 183 class HearingDevices { 184 public: 185 void Add(HearingDevice device) { 186 if (FindByAddress(device.address) != nullptr) return; 187 188 devices.push_back(device); 189 } 190 191 void Remove(const RawAddress& address) { 192 for (auto it = devices.begin(); it != devices.end();) { 193 if (it->address != address) { 194 ++it; 195 continue; 196 } 197 198 it = devices.erase(it); 199 return; 200 } 201 } 202 203 HearingDevice* FindByAddress(const RawAddress& address) { 204 auto iter = std::find_if(devices.begin(), devices.end(), 205 [&address](const HearingDevice& device) { 206 return device.address == address; 207 }); 208 209 return (iter == devices.end()) ? nullptr : &(*iter); 210 } 211 212 HearingDevice* FindByConnId(uint16_t conn_id) { 213 auto iter = std::find_if(devices.begin(), devices.end(), 214 [&conn_id](const HearingDevice& device) { 215 return device.conn_id == conn_id; 216 }); 217 218 return (iter == devices.end()) ? nullptr : &(*iter); 219 } 220 221 HearingDevice* FindByGapHandle(uint16_t gap_handle) { 222 auto iter = std::find_if(devices.begin(), devices.end(), 223 [&gap_handle](const HearingDevice& device) { 224 return device.gap_handle == gap_handle; 225 }); 226 227 return (iter == devices.end()) ? nullptr : &(*iter); 228 } 229 230 bool IsAnyConnectionUpdatePending() { 231 for (const auto& d : devices) { 232 if (d.connection_update_pending) return true; 233 } 234 235 return false; 236 } 237 238 size_t size() { return (devices.size()); } 239 240 std::vector<HearingDevice> devices; 241 }; 242 243 g722_encode_state_t* encoder_state_left = nullptr; 244 g722_encode_state_t* encoder_state_right = nullptr; 245 246 class HearingAidImpl : public HearingAid { 247 public: 248 virtual ~HearingAidImpl() = default; 249 250 HearingAidImpl(bluetooth::hearing_aid::HearingAidCallbacks* callbacks, 251 Closure initCb) 252 : gatt_if(0), 253 seq_counter(0), 254 current_volume(VOLUME_UNKNOWN), 255 callbacks(callbacks) { 256 DVLOG(2) << __func__; 257 BTA_GATTC_AppRegister( 258 hearingaid_gattc_callback, 259 base::Bind( 260 [](Closure initCb, uint8_t client_id, uint8_t status) { 261 if (status != GATT_SUCCESS) { 262 LOG(ERROR) << "Can't start Hearing Aid profile - no gatt " 263 "clients left!"; 264 return; 265 } 266 instance->gatt_if = client_id; 267 initCb.Run(); 268 }, 269 initCb)); 270 } 271 272 void Connect(const RawAddress& address) override { 273 DVLOG(2) << __func__ << " " << address; 274 hearingDevices.Add(HearingDevice(address, true)); 275 BTA_GATTC_Open(gatt_if, address, true, GATT_TRANSPORT_LE, false); 276 } 277 278 void AddFromStorage(const RawAddress& address, uint16_t psm, 279 uint8_t capabilities, uint16_t codecs, 280 uint16_t audio_control_point_handle, 281 uint16_t volume_handle, uint64_t hiSyncId, 282 uint16_t render_delay, uint16_t preparation_delay, 283 uint16_t is_white_listed) { 284 DVLOG(2) << __func__ << " " << address << ", hiSyncId=" << loghex(hiSyncId) 285 << ", isWhiteListed=" << is_white_listed; 286 if (is_white_listed) { 287 hearingDevices.Add(HearingDevice( 288 address, psm, capabilities, codecs, audio_control_point_handle, 289 volume_handle, hiSyncId, render_delay, preparation_delay)); 290 291 // TODO: we should increase the scanning window for few seconds, to get 292 // faster initial connection, same after hearing aid disconnects, i.e. 293 // BTM_BleSetConnScanParams(2048, 1024); 294 295 /* add device into BG connection to accept remote initiated connection */ 296 BTA_GATTC_Open(gatt_if, address, false, GATT_TRANSPORT_LE, false); 297 BTA_DmBleStartAutoConn(); 298 } 299 300 callbacks->OnDeviceAvailable(capabilities, hiSyncId, address); 301 } 302 303 int GetDeviceCount() { return (hearingDevices.size()); } 304 305 void OnGattConnected(tGATT_STATUS status, uint16_t conn_id, 306 tGATT_IF client_if, RawAddress address, 307 tBTA_TRANSPORT transport, uint16_t mtu) { 308 VLOG(2) << __func__ << " " << address; 309 310 HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); 311 if (!hearingDevice) { 312 DVLOG(2) << "Skipping unknown device, address=" << address; 313 return; 314 } 315 316 if (status != GATT_SUCCESS) { 317 if (!hearingDevice->connecting_actively) { 318 // whitelist connection failed, that's ok. 319 return; 320 } 321 322 LOG(INFO) << "Failed to connect to Hearing Aid device"; 323 hearingDevices.Remove(address); 324 callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); 325 return; 326 } 327 328 hearingDevice->connecting_actively = false; 329 hearingDevice->conn_id = conn_id; 330 331 /* We must update connection parameters one at a time, otherwise anchor 332 * point (start of connection event) for two devices can be too close to 333 * each other. Here, by setting min_ce_len=max_ce_len=X, we force controller 334 * to move anchor point of both connections away from each other, to make 335 * sure we'll be able to fit all the data we want in one connection event. 336 */ 337 bool any_update_pending = hearingDevices.IsAnyConnectionUpdatePending(); 338 // mark the device as pending connection update. If we don't start the 339 // update now, it'll be started once current device finishes. 340 hearingDevice->connection_update_pending = true; 341 if (!any_update_pending) { 342 L2CA_UpdateBleConnParams(address, 0x0008, 0x0008, 0x000A, 0x0064 /*1s*/, 343 MIN_CE_LEN_1M, MIN_CE_LEN_1M); 344 } 345 346 // Set data length 347 // TODO(jpawlowski: for 16khz only 87 is required, optimize 348 BTM_SetBleDataLength(address, 147); 349 350 tBTM_SEC_DEV_REC* p_dev_rec = btm_find_dev(address); 351 if (p_dev_rec) { 352 if (p_dev_rec->sec_state == BTM_SEC_STATE_ENCRYPTING || 353 p_dev_rec->sec_state == BTM_SEC_STATE_AUTHENTICATING) { 354 /* if security collision happened, wait for encryption done 355 * (BTA_GATTC_ENC_CMPL_CB_EVT) */ 356 return; 357 } 358 } 359 360 /* verify bond */ 361 uint8_t sec_flag = 0; 362 BTM_GetSecurityFlagsByTransport(address, &sec_flag, BT_TRANSPORT_LE); 363 364 if (sec_flag & BTM_SEC_FLAG_ENCRYPTED) { 365 /* if link has been encrypted */ 366 OnEncryptionComplete(address, true); 367 return; 368 } 369 370 if (sec_flag & BTM_SEC_FLAG_LKEY_KNOWN) { 371 /* if bonded and link not encrypted */ 372 sec_flag = BTM_BLE_SEC_ENCRYPT; 373 BTM_SetEncryption(address, BTA_TRANSPORT_LE, encryption_callback, nullptr, 374 sec_flag); 375 return; 376 } 377 378 /* otherwise let it go through */ 379 OnEncryptionComplete(address, true); 380 } 381 382 void OnConnectionUpdateComplete(uint16_t conn_id) { 383 HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); 384 if (!hearingDevice) { 385 DVLOG(2) << "Skipping unknown device, conn_id=" << loghex(conn_id); 386 return; 387 } 388 389 hearingDevice->connection_update_pending = false; 390 391 for (auto& device : hearingDevices.devices) { 392 if (device.conn_id && device.connection_update_pending) { 393 L2CA_UpdateBleConnParams(device.address, 0x0008, 0x0008, 0x000A, 394 0x0064 /*1s*/, MIN_CE_LEN_1M, MIN_CE_LEN_1M); 395 return; 396 } 397 } 398 } 399 400 void OnEncryptionComplete(const RawAddress& address, bool success) { 401 HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); 402 if (!hearingDevice) { 403 DVLOG(2) << "Skipping unknown device" << address; 404 return; 405 } 406 407 if (!success) { 408 LOG(ERROR) << "encryption failed"; 409 BTA_GATTC_Close(hearingDevice->conn_id); 410 if (hearingDevice->first_connection) { 411 callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); 412 } 413 return; 414 } 415 416 DVLOG(2) << __func__ << " " << address; 417 418 if (!hearingDevice->first_connection) { 419 // Use cached data, jump to connecting socket 420 ConnectSocket(hearingDevice); 421 return; 422 } 423 424 BTA_GATTC_ServiceSearchRequest(hearingDevice->conn_id, &HEARING_AID_UUID); 425 } 426 427 void OnServiceSearchComplete(uint16_t conn_id, tGATT_STATUS status) { 428 HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); 429 if (!hearingDevice) { 430 DVLOG(2) << "Skipping unknown device, conn_id=" << loghex(conn_id); 431 return; 432 } 433 434 // Known device, nothing to do. 435 if (!hearingDevice->first_connection) return; 436 437 if (status != GATT_SUCCESS) { 438 /* close connection and report service discovery complete with error */ 439 LOG(ERROR) << "Service discovery failed"; 440 if (hearingDevice->first_connection) { 441 callbacks->OnConnectionState(ConnectionState::DISCONNECTED, 442 hearingDevice->address); 443 } 444 return; 445 } 446 447 const std::vector<tBTA_GATTC_SERVICE>* services = 448 BTA_GATTC_GetServices(conn_id); 449 450 const tBTA_GATTC_SERVICE* service = nullptr; 451 for (const tBTA_GATTC_SERVICE& tmp : *services) { 452 if (tmp.uuid != HEARING_AID_UUID) continue; 453 LOG(INFO) << "Found Hearing Aid service, handle=" << loghex(tmp.handle); 454 service = &tmp; 455 break; 456 } 457 458 if (!service) { 459 LOG(ERROR) << "No Hearing Aid service found"; 460 callbacks->OnConnectionState(ConnectionState::DISCONNECTED, 461 hearingDevice->address); 462 return; 463 } 464 465 uint16_t psm_handle = 0x0000; 466 for (const tBTA_GATTC_CHARACTERISTIC& charac : service->characteristics) { 467 if (charac.uuid == READ_ONLY_PROPERTIES_UUID) { 468 DVLOG(2) << "Reading read only properties " 469 << loghex(charac.value_handle); 470 BtaGattQueue::ReadCharacteristic( 471 conn_id, charac.value_handle, 472 HearingAidImpl::OnReadOnlyPropertiesReadStatic, nullptr); 473 } else if (charac.uuid == AUDIO_CONTROL_POINT_UUID) { 474 hearingDevice->audio_control_point_handle = charac.value_handle; 475 // store audio control point! 476 } else if (charac.uuid == AUDIO_STATUS_UUID) { 477 DVLOG(2) << "Reading Audio status " << loghex(charac.value_handle); 478 BtaGattQueue::ReadCharacteristic(conn_id, charac.value_handle, 479 HearingAidImpl::OnAudioStatusStatic, 480 nullptr); 481 } else if (charac.uuid == VOLUME_UUID) { 482 hearingDevice->volume_handle = charac.value_handle; 483 } else if (charac.uuid == LE_PSM_UUID) { 484 psm_handle = charac.value_handle; 485 } else { 486 LOG(WARNING) << "Unknown characteristic found:" << charac.uuid; 487 } 488 } 489 490 if (psm_handle) { 491 DVLOG(2) << "Reading PSM " << loghex(psm_handle); 492 BtaGattQueue::ReadCharacteristic( 493 conn_id, psm_handle, HearingAidImpl::OnPsmReadStatic, nullptr); 494 } 495 } 496 497 void OnReadOnlyPropertiesRead(uint16_t conn_id, tGATT_STATUS status, 498 uint16_t handle, uint16_t len, uint8_t* value, 499 void* data) { 500 HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); 501 if (!hearingDevice) { 502 DVLOG(2) << __func__ << "unknown conn_id=" << loghex(conn_id); 503 return; 504 } 505 506 VLOG(2) << __func__ << " " << base::HexEncode(value, len); 507 508 uint8_t* p = value; 509 510 uint8_t version; 511 STREAM_TO_UINT8(version, p); 512 513 if (version != 0x01) { 514 LOG(WARNING) << "Unknown version: " << loghex(version); 515 return; 516 } 517 518 // version 0x01 of read only properties: 519 if (len < 17) { 520 LOG(WARNING) << "Read only properties too short: " << loghex(len); 521 return; 522 } 523 uint8_t capabilities; 524 STREAM_TO_UINT8(capabilities, p); 525 hearingDevice->capabilities = capabilities; 526 bool side = capabilities & CAPABILITY_SIDE; 527 bool standalone = capabilities & CAPABILITY_BINAURAL; 528 VLOG(2) << __func__ << " capabilities: " << (side ? "right" : "left") 529 << ", " << (standalone ? "binaural" : "monaural"); 530 531 if (capabilities & CAPABILITY_RESERVED) { 532 LOG(WARNING) << __func__ << " reserved capabilities are set"; 533 } 534 535 STREAM_TO_UINT64(hearingDevice->hi_sync_id, p); 536 VLOG(2) << __func__ << " hiSyncId: " << loghex(hearingDevice->hi_sync_id); 537 uint8_t feature_map; 538 STREAM_TO_UINT8(feature_map, p); 539 540 STREAM_TO_UINT16(hearingDevice->render_delay, p); 541 VLOG(2) << __func__ 542 << " render delay: " << loghex(hearingDevice->render_delay); 543 544 STREAM_TO_UINT16(hearingDevice->preparation_delay, p); 545 VLOG(2) << __func__ << " preparation delay: " 546 << loghex(hearingDevice->preparation_delay); 547 548 uint16_t codecs; 549 STREAM_TO_UINT16(codecs, p); 550 hearingDevice->codecs = codecs; 551 VLOG(2) << __func__ << " supported codecs: " << loghex(codecs); 552 if (codecs & (1 << CODEC_G722_16KHZ)) VLOG(2) << "\tG722@16kHz"; 553 if (codecs & (1 << CODEC_G722_24KHZ)) VLOG(2) << "\tG722@24kHz"; 554 555 if (!(codecs & (1 << CODEC_G722_16KHZ))) { 556 LOG(WARNING) << __func__ << " Mandatory codec, G722@16kHz not supported"; 557 } 558 } 559 560 void OnAudioStatus(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, 561 uint16_t len, uint8_t* value, void* data) { 562 DVLOG(2) << __func__ << " " << base::HexEncode(value, len); 563 } 564 565 void OnPsmRead(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, 566 uint16_t len, uint8_t* value, void* data) { 567 HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); 568 if (!hearingDevice) { 569 DVLOG(2) << "Skipping unknown read event, conn_id=" << loghex(conn_id); 570 return; 571 } 572 573 if (status != GATT_SUCCESS) { 574 LOG(ERROR) << "Error reading PSM for device" << hearingDevice->address; 575 return; 576 } 577 578 if (len > 2) { 579 LOG(ERROR) << "Bad PSM length"; 580 return; 581 } 582 583 uint16_t psm_val = *((uint16_t*)value); 584 hearingDevice->psm = psm_val; 585 VLOG(2) << "read psm:" << loghex(hearingDevice->psm); 586 587 ConnectSocket(hearingDevice); 588 } 589 590 void ConnectSocket(HearingDevice* hearingDevice) { 591 tL2CAP_CFG_INFO cfg_info = tL2CAP_CFG_INFO{.mtu = 512}; 592 593 uint16_t gap_handle = GAP_ConnOpen( 594 "", 0, false, &hearingDevice->address, hearingDevice->psm, 595 514 /* MPS */, &cfg_info, nullptr, 596 BTM_SEC_NONE /* TODO: request security ? */, L2CAP_FCR_LE_COC_MODE, 597 HearingAidImpl::GapCallbackStatic, BT_TRANSPORT_LE); 598 if (gap_handle == GAP_INVALID_HANDLE) { 599 LOG(ERROR) << "UNABLE TO GET gap_handle"; 600 return; 601 } 602 603 hearingDevice->gap_handle = gap_handle; 604 LOG(INFO) << "Successfully sent GAP connect request"; 605 } 606 607 static void OnReadOnlyPropertiesReadStatic(uint16_t conn_id, 608 tGATT_STATUS status, 609 uint16_t handle, uint16_t len, 610 uint8_t* value, void* data) { 611 if (instance) 612 instance->OnReadOnlyPropertiesRead(conn_id, status, handle, len, value, 613 data); 614 } 615 static void OnAudioStatusStatic(uint16_t conn_id, tGATT_STATUS status, 616 uint16_t handle, uint16_t len, uint8_t* value, 617 void* data) { 618 if (instance) 619 instance->OnAudioStatus(conn_id, status, handle, len, value, data); 620 } 621 622 static void OnPsmReadStatic(uint16_t conn_id, tGATT_STATUS status, 623 uint16_t handle, uint16_t len, uint8_t* value, 624 void* data) { 625 if (instance) 626 instance->OnPsmRead(conn_id, status, handle, len, value, data); 627 } 628 629 /* CoC Socket is ready */ 630 void OnGapConnection(const RawAddress& address) { 631 HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); 632 if (!hearingDevice) { 633 LOG(INFO) << "Device not connected to profile" << address; 634 return; 635 } 636 637 if (hearingDevice->first_connection) { 638 /* add device into BG connection to accept remote initiated connection */ 639 BTA_GATTC_Open(gatt_if, address, false, GATT_TRANSPORT_LE, false); 640 BTA_DmBleStartAutoConn(); 641 642 btif_storage_add_hearing_aid( 643 address, hearingDevice->psm, hearingDevice->capabilities, 644 hearingDevice->codecs, hearingDevice->audio_control_point_handle, 645 hearingDevice->volume_handle, hearingDevice->hi_sync_id, 646 hearingDevice->render_delay, hearingDevice->preparation_delay); 647 648 hearingDevice->first_connection = false; 649 } 650 651 SendStart(*hearingDevice); 652 653 hearingDevice->accepting_audio = true; 654 LOG(INFO) << __func__ << ": address=" << address 655 << ", hi_sync_id=" << loghex(hearingDevice->hi_sync_id); 656 callbacks->OnDeviceAvailable(hearingDevice->capabilities, 657 hearingDevice->hi_sync_id, address); 658 callbacks->OnConnectionState(ConnectionState::CONNECTED, address); 659 660 StartSendingAudio(*hearingDevice); 661 } 662 663 void StartSendingAudio(const HearingDevice& hearingDevice) { 664 VLOG(0) << __func__ << hearingDevice.address; 665 666 if (encoder_state_left == nullptr) { 667 encoder_state_left = g722_encode_init(nullptr, 64000, G722_PACKED); 668 encoder_state_right = g722_encode_init(nullptr, 64000, G722_PACKED); 669 seq_counter = 0; 670 671 // use the best codec avaliable for this pair of devices. 672 uint16_t codecs = hearingDevice.codecs; 673 if (hearingDevice.hi_sync_id != 0) { 674 for (const auto& device : hearingDevices.devices) { 675 if (device.hi_sync_id != hearingDevice.hi_sync_id) continue; 676 677 codecs &= device.codecs; 678 } 679 } 680 681 if ((codecs & (1 << CODEC_G722_24KHZ)) && 682 controller_get_interface()->supports_ble_2m_phy()) { 683 codec_in_use = CODEC_G722_24KHZ; 684 codec.sample_rate = 24000; 685 codec.bit_rate = 16; 686 codec.data_interval_ms = 10; 687 } else if (codecs & (1 << CODEC_G722_16KHZ)) { 688 codec_in_use = CODEC_G722_16KHZ; 689 codec.sample_rate = 16000; 690 codec.bit_rate = 16; 691 codec.data_interval_ms = 10; 692 } 693 694 // TODO: remove once we implement support for other codecs 695 codec_in_use = CODEC_G722_16KHZ; 696 HearingAidAudioSource::Start(codec, audioReceiver); 697 } 698 } 699 700 void OnAudioSuspend() { 701 DVLOG(2) << __func__; 702 703 std::vector<uint8_t> stop({CONTROL_POINT_OP_STOP}); 704 for (const auto& device : hearingDevices.devices) { 705 if (!device.accepting_audio) continue; 706 707 BtaGattQueue::WriteCharacteristic(device.conn_id, 708 device.audio_control_point_handle, stop, 709 GATT_WRITE, nullptr, nullptr); 710 } 711 } 712 713 void OnAudioResume() { 714 DVLOG(2) << __func__; 715 716 // TODO: shall we also reset the encoder ? 717 if (encoder_state_left != nullptr) { 718 g722_encode_release(encoder_state_left); 719 g722_encode_release(encoder_state_right); 720 encoder_state_left = g722_encode_init(nullptr, 64000, G722_PACKED); 721 encoder_state_right = g722_encode_init(nullptr, 64000, G722_PACKED); 722 } 723 seq_counter = 0; 724 725 for (const auto& device : hearingDevices.devices) { 726 if (!device.accepting_audio) continue; 727 SendStart(device); 728 } 729 } 730 731 void SendStart(const HearingDevice& device) { 732 std::vector<uint8_t> start({CONTROL_POINT_OP_START, codec_in_use, 733 0x02 /* media */, (uint8_t)current_volume}); 734 735 if (current_volume == VOLUME_UNKNOWN) start[3] = (uint8_t)VOLUME_MIN; 736 737 BtaGattQueue::WriteCharacteristic(device.conn_id, 738 device.audio_control_point_handle, start, 739 GATT_WRITE, nullptr, nullptr); 740 741 // TODO(jpawlowski): this will be removed, once test devices get volume 742 // from start reqest 743 if (current_volume != VOLUME_UNKNOWN) { 744 std::vector<uint8_t> volume_value( 745 {static_cast<unsigned char>(current_volume)}); 746 BtaGattQueue::WriteCharacteristic(device.conn_id, device.volume_handle, 747 volume_value, GATT_WRITE_NO_RSP, 748 nullptr, nullptr); 749 } 750 } 751 752 void OnAudioDataReady(const std::vector<uint8_t>& data) { 753 /* For now we assume data comes in as 16bit per sample 16kHz PCM stereo */ 754 DVLOG(2) << __func__; 755 756 int num_samples = 757 data.size() / (2 /*bytes_per_sample*/ * 2 /*number of channels*/); 758 759 // The G.722 codec accept only even number of samples for encoding 760 if (num_samples % 2 != 0) 761 LOG(FATAL) << "num_samples is not even: " << num_samples; 762 763 std::vector<uint16_t> chan_left; 764 std::vector<uint16_t> chan_right; 765 // TODO: encode data into G.722 left/right or mono. 766 for (int i = 0; i < num_samples; i++) { 767 const uint8_t* sample = data.data() + i * 4; 768 769 uint16_t left = (int16_t)((*(sample + 1) << 8) + *sample) >> 1; 770 chan_left.push_back(left); 771 772 sample += 2; 773 uint16_t right = (int16_t)((*(sample + 1) << 8) + *sample) >> 1; 774 chan_right.push_back(right); 775 } 776 777 // TODO: we should cache left/right and current state, instad of recomputing 778 // it for each packet, 100 times a second. 779 HearingDevice* left = nullptr; 780 HearingDevice* right = nullptr; 781 for (auto& device : hearingDevices.devices) { 782 if (!device.accepting_audio) continue; 783 784 if (device.isLeft()) 785 left = &device; 786 else 787 right = &device; 788 } 789 790 if (left == nullptr && right == nullptr) { 791 HearingAidAudioSource::Stop(); 792 current_volume = VOLUME_UNKNOWN; 793 return; 794 } 795 796 // TODO: monural, binarual check 797 798 // divide encoded data into packets, add header, send. 799 800 // TODO: make those buffers static and global to prevent constant 801 // reallocations 802 // TODO: this should basically fit the encoded data, tune the size later 803 std::vector<uint8_t> encoded_data_left; 804 if (left) { 805 encoded_data_left.resize(2000); 806 int encoded_size = 807 g722_encode(encoder_state_left, encoded_data_left.data(), 808 (const int16_t*)chan_left.data(), chan_left.size()); 809 encoded_data_left.resize(encoded_size); 810 811 uint16_t cid = GAP_ConnGetL2CAPCid(left->gap_handle); 812 uint16_t packets_to_flush = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_GET); 813 if (packets_to_flush) { 814 VLOG(2) << left->address << " skipping " << packets_to_flush 815 << " packets"; 816 left->audio_stats.packet_flush_count += packets_to_flush; 817 left->audio_stats.frame_flush_count++; 818 } 819 // flush all packets stuck in queue 820 L2CA_FlushChannel(cid, 0xffff); 821 } 822 823 std::vector<uint8_t> encoded_data_right; 824 if (right) { 825 encoded_data_right.resize(2000); 826 int encoded_size = 827 g722_encode(encoder_state_right, encoded_data_right.data(), 828 (const int16_t*)chan_right.data(), chan_right.size()); 829 encoded_data_right.resize(encoded_size); 830 831 uint16_t cid = GAP_ConnGetL2CAPCid(right->gap_handle); 832 uint16_t packets_to_flush = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_GET); 833 if (packets_to_flush) { 834 VLOG(2) << right->address << " skipping " << packets_to_flush 835 << " packets"; 836 right->audio_stats.packet_flush_count += packets_to_flush; 837 right->audio_stats.frame_flush_count++; 838 } 839 // flush all packets stuck in queue 840 L2CA_FlushChannel(cid, 0xffff); 841 } 842 843 size_t encoded_data_size = 844 std::max(encoded_data_left.size(), encoded_data_right.size()); 845 846 // TODO: make it also dependent on the interval, when we support intervals 847 // different than 10ms 848 uint16_t packet_size; 849 850 if (codec_in_use == CODEC_G722_24KHZ) { 851 packet_size = 120; 852 } else /* if (codec_in_use == CODEC_G722_16KHZ) */ { 853 packet_size = 80; 854 } 855 856 for (size_t i = 0; i < encoded_data_size; i += packet_size) { 857 if (left) { 858 left->audio_stats.packet_send_count++; 859 SendAudio(encoded_data_left.data() + i, packet_size, left); 860 } 861 if (right) { 862 right->audio_stats.packet_send_count++; 863 SendAudio(encoded_data_right.data() + i, packet_size, right); 864 } 865 seq_counter++; 866 } 867 if (left) left->audio_stats.frame_send_count++; 868 if (right) right->audio_stats.frame_send_count++; 869 } 870 871 void SendAudio(uint8_t* encoded_data, uint16_t packet_size, 872 HearingDevice* hearingAid) { 873 BT_HDR* audio_packet = malloc_l2cap_buf(packet_size + 1); 874 uint8_t* p = get_l2cap_sdu_start_ptr(audio_packet); 875 *p = seq_counter; 876 p++; 877 memcpy(p, encoded_data, packet_size); 878 879 DVLOG(2) << hearingAid->address << " : " << base::HexEncode(p, packet_size); 880 881 uint16_t result = GAP_ConnWriteData(hearingAid->gap_handle, audio_packet); 882 883 if (result != BT_PASS) { 884 LOG(ERROR) << " Error sending data: " << loghex(result); 885 } 886 } 887 888 void GapCallback(uint16_t gap_handle, uint16_t event, tGAP_CB_DATA* data) { 889 HearingDevice* hearingDevice = hearingDevices.FindByGapHandle(gap_handle); 890 if (!hearingDevice) { 891 DVLOG(2) << "Skipping unknown device, gap_handle=" << gap_handle; 892 return; 893 } 894 895 switch (event) { 896 case GAP_EVT_CONN_OPENED: { 897 RawAddress address = *GAP_ConnGetRemoteAddr(gap_handle); 898 uint16_t tx_mtu = GAP_ConnGetRemMtuSize(gap_handle); 899 900 LOG(INFO) << "GAP_EVT_CONN_OPENED " << address << ", tx_mtu=" << tx_mtu; 901 OnGapConnection(address); 902 break; 903 } 904 905 // TODO: handle properly! 906 case GAP_EVT_CONN_CLOSED: 907 DVLOG(2) << "GAP_EVT_CONN_CLOSED"; 908 hearingDevice->accepting_audio = false; 909 hearingDevice->gap_handle = 0; 910 break; 911 case GAP_EVT_CONN_DATA_AVAIL: { 912 DVLOG(2) << "GAP_EVT_CONN_DATA_AVAIL"; 913 914 // only data we receive back from hearing aids are some stats, not 915 // really important, but useful now for debugging. 916 uint32_t bytes_to_read = 0; 917 GAP_GetRxQueueCnt(gap_handle, &bytes_to_read); 918 std::vector<uint8_t> buffer(bytes_to_read); 919 920 uint16_t bytes_read = 0; 921 // TODO:GAP_ConnReadData should accpet uint32_t for length! 922 GAP_ConnReadData(gap_handle, buffer.data(), buffer.size(), &bytes_read); 923 924 if (bytes_read < 4) { 925 LOG(WARNING) << " Wrong data length"; 926 return; 927 } 928 929 uint8_t* p = buffer.data(); 930 931 DVLOG(1) << "stats from the hearing aid:"; 932 for (size_t i = 0; i + 4 <= buffer.size(); i += 4) { 933 uint16_t event_counter, frame_index; 934 STREAM_TO_UINT16(event_counter, p); 935 STREAM_TO_UINT16(frame_index, p); 936 DVLOG(1) << "event_counter=" << event_counter 937 << " frame_index: " << frame_index; 938 } 939 break; 940 } 941 942 case GAP_EVT_TX_EMPTY: 943 DVLOG(2) << "GAP_EVT_TX_EMPTY"; 944 break; 945 case GAP_EVT_CONN_CONGESTED: 946 DVLOG(2) << "GAP_EVT_CONN_CONGESTED"; 947 948 // TODO: make it into function 949 HearingAidAudioSource::Stop(); 950 // TODO: kill the encoder only if all hearing aids are down. 951 // g722_encode_release(encoder_state); 952 // encoder_state_left = nulllptr; 953 // encoder_state_right = nulllptr; 954 break; 955 case GAP_EVT_CONN_UNCONGESTED: 956 DVLOG(2) << "GAP_EVT_CONN_UNCONGESTED"; 957 break; 958 959 case GAP_EVT_LE_COC_CREDITS: { 960 auto& tmp = data->coc_credits; 961 DVLOG(2) << "GAP_EVT_LE_COC_CREDITS, for device: " 962 << hearingDevice->address << " added" << tmp.credits_received 963 << " credit_count: " << tmp.credit_count; 964 break; 965 } 966 } 967 } 968 969 static void GapCallbackStatic(uint16_t gap_handle, uint16_t event, 970 tGAP_CB_DATA* data) { 971 if (instance) instance->GapCallback(gap_handle, event, data); 972 } 973 974 void Dump(int fd) { 975 std::stringstream stream; 976 for (const auto& device : hearingDevices.devices) { 977 bool side = device.capabilities & CAPABILITY_SIDE; 978 bool standalone = device.capabilities & CAPABILITY_BINAURAL; 979 stream << " " << device.address.ToString() << " " 980 << (device.accepting_audio ? "" : "not ") << "connected" 981 << "\n " << (standalone ? "binaural" : "monaural") << " " 982 << (side ? "right" : "left") << " " << loghex(device.hi_sync_id) 983 << std::endl; 984 stream 985 << " Packet counts (enqueued/flushed) : " 986 << device.audio_stats.packet_send_count << " / " 987 << device.audio_stats.packet_flush_count 988 << "\n Frame counts (enqueued/flushed) : " 989 << device.audio_stats.frame_send_count << " / " 990 << device.audio_stats.frame_flush_count << std::endl; 991 } 992 dprintf(fd, "%s", stream.str().c_str()); 993 } 994 995 void Disconnect(const RawAddress& address) override { 996 DVLOG(2) << __func__; 997 HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); 998 if (!hearingDevice) { 999 LOG(INFO) << "Device not connected to profile" << address; 1000 return; 1001 } 1002 1003 VLOG(2) << __func__ << ": " << address; 1004 1005 bool connected = hearingDevice->accepting_audio; 1006 hearingDevice->accepting_audio = false; 1007 1008 if (hearingDevice->connecting_actively) { 1009 // cancel pending direct connect 1010 BTA_GATTC_CancelOpen(gatt_if, address, true); 1011 } 1012 1013 if (hearingDevice->conn_id) { 1014 BTA_GATTC_Close(hearingDevice->conn_id); 1015 } 1016 1017 if (hearingDevice->gap_handle) { 1018 GAP_ConnClose(hearingDevice->gap_handle); 1019 hearingDevice->gap_handle = 0; 1020 } 1021 1022 // cancel autoconnect 1023 BTA_GATTC_CancelOpen(gatt_if, address, false); 1024 1025 hearingDevices.Remove(address); 1026 1027 if (connected) 1028 callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); 1029 } 1030 1031 void OnGattDisconnected(tGATT_STATUS status, uint16_t conn_id, 1032 tGATT_IF client_if, RawAddress remote_bda, 1033 tBTA_GATT_REASON reason) { 1034 HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); 1035 if (!hearingDevice) { 1036 VLOG(2) << "Skipping unknown device disconnect, conn_id=" << conn_id; 1037 return; 1038 } 1039 1040 hearingDevice->accepting_audio = false; 1041 hearingDevice->conn_id = 0; 1042 1043 BtaGattQueue::Clean(conn_id); 1044 1045 callbacks->OnConnectionState(ConnectionState::DISCONNECTED, remote_bda); 1046 } 1047 1048 void SetVolume(int8_t volume) override { 1049 VLOG(2) << __func__ << ": " << +volume; 1050 current_volume = volume; 1051 for (HearingDevice& device : hearingDevices.devices) { 1052 if (!device.accepting_audio) continue; 1053 1054 std::vector<uint8_t> volume_value({static_cast<unsigned char>(volume)}); 1055 BtaGattQueue::WriteCharacteristic(device.conn_id, device.volume_handle, 1056 volume_value, GATT_WRITE_NO_RSP, 1057 nullptr, nullptr); 1058 } 1059 } 1060 1061 void CleanUp() { 1062 BTA_GATTC_AppDeregister(gatt_if); 1063 for (HearingDevice& device : hearingDevices.devices) { 1064 if (!device.gap_handle) continue; 1065 1066 GAP_ConnClose(device.gap_handle); 1067 device.gap_handle = 0; 1068 } 1069 1070 hearingDevices.devices.clear(); 1071 HearingAidAudioSource::Stop(); 1072 } 1073 1074 private: 1075 uint8_t gatt_if; 1076 uint8_t seq_counter; 1077 /* current volume gain for the hearing aids*/ 1078 int8_t current_volume; 1079 bluetooth::hearing_aid::HearingAidCallbacks* callbacks; 1080 1081 /* currently used codec */ 1082 uint8_t codec_in_use; 1083 CodecConfiguration codec; 1084 1085 HearingDevices hearingDevices; 1086 }; 1087 1088 void hearingaid_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data) { 1089 VLOG(2) << __func__ << " event = " << +event; 1090 1091 if (p_data == nullptr) return; 1092 1093 switch (event) { 1094 case BTA_GATTC_DEREG_EVT: 1095 break; 1096 1097 case BTA_GATTC_OPEN_EVT: { 1098 if (!instance) return; 1099 tBTA_GATTC_OPEN& o = p_data->open; 1100 instance->OnGattConnected(o.status, o.conn_id, o.client_if, o.remote_bda, 1101 o.transport, o.mtu); 1102 break; 1103 } 1104 1105 case BTA_GATTC_CLOSE_EVT: { 1106 if (!instance) return; 1107 tBTA_GATTC_CLOSE& c = p_data->close; 1108 instance->OnGattDisconnected(c.status, c.conn_id, c.client_if, 1109 c.remote_bda, c.reason); 1110 } break; 1111 1112 case BTA_GATTC_SEARCH_CMPL_EVT: 1113 if (!instance) return; 1114 instance->OnServiceSearchComplete(p_data->search_cmpl.conn_id, 1115 p_data->search_cmpl.status); 1116 break; 1117 1118 case BTA_GATTC_NOTIF_EVT: 1119 break; 1120 1121 case BTA_GATTC_ENC_CMPL_CB_EVT: 1122 if (!instance) return; 1123 instance->OnEncryptionComplete(p_data->enc_cmpl.remote_bda, true); 1124 break; 1125 1126 case BTA_GATTC_CONN_UPDATE_EVT: 1127 if (!instance) return; 1128 instance->OnConnectionUpdateComplete(p_data->conn_update.conn_id); 1129 break; 1130 1131 default: 1132 break; 1133 } 1134 } 1135 1136 void encryption_callback(const RawAddress* address, tGATT_TRANSPORT, void*, 1137 tBTM_STATUS status) { 1138 if (instance) { 1139 instance->OnEncryptionComplete(*address, 1140 status == BTM_SUCCESS ? true : false); 1141 } 1142 } 1143 1144 class HearingAidAudioReceiverImpl : public HearingAidAudioReceiver { 1145 public: 1146 void OnAudioDataReady(const std::vector<uint8_t>& data) override { 1147 if (instance) instance->OnAudioDataReady(data); 1148 } 1149 void OnAudioSuspend() override { 1150 if (instance) instance->OnAudioSuspend(); 1151 } 1152 1153 void OnAudioResume() override { 1154 if (instance) instance->OnAudioResume(); 1155 } 1156 }; 1157 1158 HearingAidAudioReceiverImpl audioReceiverImpl; 1159 1160 } // namespace 1161 1162 void HearingAid::Initialize( 1163 bluetooth::hearing_aid::HearingAidCallbacks* callbacks, Closure initCb) { 1164 if (instance) { 1165 LOG(ERROR) << "Already initialized!"; 1166 } 1167 1168 audioReceiver = &audioReceiverImpl; 1169 instance = new HearingAidImpl(callbacks, initCb); 1170 HearingAidAudioSource::Initialize(); 1171 } 1172 1173 bool HearingAid::IsInitialized() { return instance; } 1174 1175 HearingAid* HearingAid::Get() { 1176 CHECK(instance); 1177 return instance; 1178 }; 1179 1180 void HearingAid::AddFromStorage(const RawAddress& address, uint16_t psm, 1181 uint8_t capabilities, uint16_t codecs, 1182 uint16_t audio_control_point_handle, 1183 uint16_t volume_handle, uint64_t hiSyncId, 1184 uint16_t render_delay, 1185 uint16_t preparation_delay, 1186 uint16_t is_white_listed) { 1187 if (!instance) { 1188 LOG(ERROR) << "Not initialized yet"; 1189 } 1190 1191 instance->AddFromStorage(address, psm, capabilities, codecs, 1192 audio_control_point_handle, volume_handle, hiSyncId, 1193 render_delay, preparation_delay, is_white_listed); 1194 }; 1195 1196 int HearingAid::GetDeviceCount() { 1197 if (!instance) { 1198 LOG(INFO) << __func__ << ": Not initialized yet"; 1199 return 0; 1200 } 1201 1202 return (instance->GetDeviceCount()); 1203 } 1204 1205 void HearingAid::CleanUp() { 1206 // Must stop audio source to make sure it doesn't call any of callbacks on our 1207 // soon to be null instance 1208 HearingAidAudioSource::Stop(); 1209 HearingAidAudioSource::CleanUp(); 1210 1211 instance->CleanUp(); 1212 HearingAidImpl* ptr = instance; 1213 instance = nullptr; 1214 delete ptr; 1215 }; 1216 1217 void HearingAid::DebugDump(int fd) { 1218 dprintf(fd, "\nHearing Aid Manager:\n"); 1219 if (instance) instance->Dump(fd); 1220 HearingAidAudioSource::DebugDump(fd); 1221 }
