/****************************************************************************** * * Copyright 2018 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at: * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * ******************************************************************************/ #define LOG_TAG "bluetooth-asha" #include <base/functional/bind.h> #include <base/functional/callback.h> #include <base/strings/string_number_conversions.h> // HexEncode #include <bluetooth/log.h> #include <com_android_bluetooth_flags.h> #include <chrono> #include <cstdint> #include <mutex> #include <vector> #include "audio/asrc/asrc_resampler.h" #include "bta/include/bta_gatt_api.h" #include "bta/include/bta_gatt_queue.h" #include "bta/include/bta_hearing_aid_api.h" #include "btm_iso_api.h" #include "embdrv/g722/g722_enc_dec.h" #include "hal/link_clocker.h" #include "hardware/bt_gatt_types.h" #include "hci/controller_interface.h" #include "internal_include/bt_trace.h" #include "main/shim/entry.h" #include "os/log.h" #include "osi/include/allocator.h" #include "osi/include/properties.h" #include "stack/btm/btm_sec.h" #include "stack/include/acl_api.h" // BTM_ReadRSSI #include "stack/include/acl_api_types.h" // tBTM_RSSI_RESULT #include "stack/include/bt_hdr.h" #include "stack/include/bt_types.h" #include "stack/include/bt_uuid16.h" #include "stack/include/l2c_api.h" // L2CAP_MIN_OFFSET #include "stack/include/main_thread.h" #include "types/bluetooth/uuid.h" #include "types/bt_transport.h" #include "types/raw_address.h" using base::Closure; using bluetooth::Uuid; using bluetooth::hci::IsoManager; using bluetooth::hearing_aid::ConnectionState; using namespace bluetooth; // The MIN_CE_LEN parameter for Connection Parameters based on the current // Connection Interval constexpr uint16_t MIN_CE_LEN_10MS_CI = 0x0006; constexpr uint16_t MIN_CE_LEN_20MS_CI = 0x000C; constexpr uint16_t MAX_CE_LEN_20MS_CI = 0x000C; constexpr uint16_t CE_LEN_20MS_CI_ISO_RUNNING = 0x0000; constexpr uint16_t CONNECTION_INTERVAL_10MS_PARAM = 0x0008; constexpr uint16_t CONNECTION_INTERVAL_20MS_PARAM = 0x0010; void btif_storage_add_hearing_aid(const HearingDevice& dev_info); bool btif_storage_get_hearing_aid_prop( const RawAddress& address, uint8_t* capabilities, uint64_t* hi_sync_id, uint16_t* render_delay, uint16_t* preparation_delay, uint16_t* codecs); constexpr uint8_t CODEC_G722_16KHZ = 0x01; constexpr uint8_t CODEC_G722_24KHZ = 0x02; // audio control point opcodes constexpr uint8_t CONTROL_POINT_OP_START = 0x01; constexpr uint8_t CONTROL_POINT_OP_STOP = 0x02; constexpr uint8_t CONTROL_POINT_OP_STATE_CHANGE = 0x03; constexpr uint8_t STATE_CHANGE_OTHER_SIDE_DISCONNECTED = 0x00; constexpr uint8_t STATE_CHANGE_OTHER_SIDE_CONNECTED = 0x01; constexpr uint8_t STATE_CHANGE_CONN_UPDATE = 0x02; // used to mark current_volume as not yet known, or possibly old constexpr int8_t VOLUME_UNKNOWN = 127; constexpr int8_t VOLUME_MIN = -127; // audio type constexpr uint8_t AUDIOTYPE_UNKNOWN = 0x00; // Status of the other side Hearing Aids device constexpr uint8_t OTHER_SIDE_NOT_STREAMING = 0x00; constexpr uint8_t OTHER_SIDE_IS_STREAMING = 0x01; // This ADD_RENDER_DELAY_INTERVALS is the number of connection intervals when // the audio data packet is send by Audio Engine to when the Hearing Aids device // received it from the air. We assumed that there is 2 data buffer queued from // audio subsystem to bluetooth chip. Then the estimated OTA delay is two // connnection intervals. constexpr uint16_t ADD_RENDER_DELAY_INTERVALS = 4; namespace { // clang-format off Uuid HEARING_AID_UUID = Uuid::FromString("FDF0"); Uuid READ_ONLY_PROPERTIES_UUID = Uuid::FromString("6333651e-c481-4a3e-9169-7c902aad37bb"); Uuid AUDIO_CONTROL_POINT_UUID = Uuid::FromString("f0d4de7e-4a88-476c-9d9f-1937b0996cc0"); Uuid AUDIO_STATUS_UUID = Uuid::FromString("38663f1a-e711-4cac-b641-326b56404837"); Uuid VOLUME_UUID = Uuid::FromString("00e4ca9e-ab14-41e4-8823-f9e70c7e91df"); Uuid LE_PSM_UUID = Uuid::FromString("2d410339-82b6-42aa-b34e-e2e01df8cc1a"); // clang-format on static void read_rssi_callback(void* p_void); static void hearingaid_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data); static void encryption_callback(RawAddress, tBT_TRANSPORT, void*, tBTM_STATUS); inline BT_HDR* malloc_l2cap_buf(uint16_t len) { BT_HDR* msg = (BT_HDR*)osi_malloc(BT_HDR_SIZE + L2CAP_MIN_OFFSET + len /* LE-only, no need for FCS here */); msg->offset = L2CAP_MIN_OFFSET; msg->len = len; return msg; } inline uint8_t* get_l2cap_sdu_start_ptr(BT_HDR* msg) { return (uint8_t*)(msg) + BT_HDR_SIZE + L2CAP_MIN_OFFSET; } class HearingAidImpl; HearingAidImpl* instance; std::mutex instance_mutex; HearingAidAudioReceiver* audioReceiver; class HearingDevices { public: void Add(HearingDevice device) { if (FindByAddress(device.address) != nullptr) return; devices.push_back(device); } void Remove(const RawAddress& address) { for (auto it = devices.begin(); it != devices.end();) { if (it->address != address) { ++it; continue; } it = devices.erase(it); return; } } HearingDevice* FindByAddress(const RawAddress& address) { auto iter = std::find_if(devices.begin(), devices.end(), [&address](const HearingDevice& device) { return device.address == address; }); return (iter == devices.end()) ? nullptr : &(*iter); } HearingDevice* FindOtherConnectedDeviceFromSet(const HearingDevice& device) { auto iter = std::find_if( devices.begin(), devices.end(), [&device](const HearingDevice& other) { return &device != &other && device.hi_sync_id == other.hi_sync_id && other.conn_id != 0; }); return (iter == devices.end()) ? nullptr : &(*iter); } HearingDevice* FindByConnId(uint16_t conn_id) { auto iter = std::find_if(devices.begin(), devices.end(), [&conn_id](const HearingDevice& device) { return device.conn_id == conn_id; }); return (iter == devices.end()) ? nullptr : &(*iter); } HearingDevice* FindByGapHandle(uint16_t gap_handle) { auto iter = std::find_if(devices.begin(), devices.end(), [&gap_handle](const HearingDevice& device) { return device.gap_handle == gap_handle; }); return (iter == devices.end()) ? nullptr : &(*iter); } void StartRssiLog() { int read_rssi_start_interval_count = 0; for (auto& d : devices) { log::debug("bd_addr={} read_rssi_count={}", d.address, d.read_rssi_count); // Reset the count if (d.read_rssi_count <= 0) { d.read_rssi_count = READ_RSSI_NUM_TRIES; d.num_intervals_since_last_rssi_read = read_rssi_start_interval_count; // Spaced apart the Read RSSI commands to the BT controller. read_rssi_start_interval_count += PERIOD_TO_READ_RSSI_IN_INTERVALS / 2; read_rssi_start_interval_count %= PERIOD_TO_READ_RSSI_IN_INTERVALS; std::deque<rssi_log>& rssi_logs = d.audio_stats.rssi_history; if (rssi_logs.size() >= MAX_RSSI_HISTORY) { rssi_logs.pop_front(); } rssi_logs.emplace_back(); } } } size_t size() { return (devices.size()); } std::vector<HearingDevice> devices; }; static void write_rpt_ctl_cfg_cb(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, const uint8_t* value, void* data) { if (status != GATT_SUCCESS) { log::error("handle= {}, conn_id={}, status= 0x{:x}, length={}", handle, conn_id, static_cast<uint8_t>(status), len); } } g722_encode_state_t* encoder_state_left = nullptr; g722_encode_state_t* encoder_state_right = nullptr; inline void encoder_state_init() { if (encoder_state_left != nullptr) { log::warn("encoder already initialized"); return; } encoder_state_left = g722_encode_init(nullptr, 64000, G722_PACKED); encoder_state_right = g722_encode_init(nullptr, 64000, G722_PACKED); } inline void encoder_state_release() { if (encoder_state_left != nullptr) { g722_encode_release(encoder_state_left); encoder_state_left = nullptr; g722_encode_release(encoder_state_right); encoder_state_right = nullptr; } } class HearingAidImpl : public HearingAid { private: // Keep track of whether the Audio Service has resumed audio playback bool audio_running; bool is_iso_running = false; // For Testing: overwrite the MIN_CE_LEN and MAX_CE_LEN during connection // parameter updates int16_t overwrite_min_ce_len = -1; int16_t overwrite_max_ce_len = -1; const std::string PERSIST_MIN_CE_LEN_NAME = "persist.bluetooth.hearing_aid_min_ce_len"; const std::string PERSIST_MAX_CE_LEN_NAME = "persist.bluetooth.hearing_aid_max_ce_len"; // Record whether the connection parameter needs to update to a better one bool needs_parameter_update = false; std::chrono::time_point<std::chrono::steady_clock> last_drop_time_point = std::chrono::steady_clock::now(); // at most 1 packet DROP per DROP_FREQUENCY_THRESHOLD seconds const int DROP_FREQUENCY_THRESHOLD = bluetooth::common::init_flags::get_asha_packet_drop_frequency_threshold(); // Resampler context for audio stream. // Clock recovery uses L2CAP Flow Control Credit Ind acknowledgments // from either the left or right connection, whichever is first // connected. std::unique_ptr<bluetooth::audio::asrc::SourceAudioHalAsrc> asrc; public: ~HearingAidImpl() override = default; HearingAidImpl(bluetooth::hearing_aid::HearingAidCallbacks* callbacks, Closure initCb) : audio_running(false), overwrite_min_ce_len(-1), overwrite_max_ce_len(-1), gatt_if(0), seq_counter(0), current_volume(VOLUME_UNKNOWN), callbacks(callbacks), codec_in_use(0) { default_data_interval_ms = (uint16_t)osi_property_get_int32( "persist.bluetooth.hearingaid.interval", (int32_t)HA_INTERVAL_20_MS); if ((default_data_interval_ms != HA_INTERVAL_10_MS) && (default_data_interval_ms != HA_INTERVAL_20_MS)) { log::error("invalid interval={}ms. Overwrriting back to default", default_data_interval_ms); default_data_interval_ms = HA_INTERVAL_20_MS; } overwrite_min_ce_len = (int16_t)osi_property_get_int32(PERSIST_MIN_CE_LEN_NAME.c_str(), -1); overwrite_max_ce_len = (int16_t)osi_property_get_int32(PERSIST_MAX_CE_LEN_NAME.c_str(), -1); log::info( "default_data_interval_ms={} overwrite_min_ce_len={}" " overwrite_max_ce_len={}", default_data_interval_ms, overwrite_min_ce_len, overwrite_max_ce_len); BTA_GATTC_AppRegister( hearingaid_gattc_callback, base::Bind( [](Closure initCb, uint8_t client_id, uint8_t status) { if (status != GATT_SUCCESS) { log::error( "Can't start Hearing Aid profile - no gatt clients left!"); return; } instance->gatt_if = client_id; initCb.Run(); }, initCb), false); IsoManager::GetInstance()->Start(); IsoManager::GetInstance()->RegisterOnIsoTrafficActiveCallback( [](bool is_active) { if (!instance) { return; } instance->IsoTrafficEventCb(is_active); }); } void IsoTrafficEventCb(bool is_active) { if (is_active) { is_iso_running = true; needs_parameter_update = true; } else { is_iso_running = false; } log::info("is_iso_running={} needs_parameter_update={}", is_iso_running, needs_parameter_update); if (needs_parameter_update) { for (auto& device : hearingDevices.devices) { if (device.conn_id != 0) { device.connection_update_status = STARTED; device.requested_connection_interval = UpdateBleConnParams(device.address); } } } } // Reset and configure the ASHA resampling context using the input device // devices as reference for the BT clock estimation. void ConfigureAsrc() { if (!com::android::bluetooth::flags::asha_asrc()) { log::info("Asha resampling disabled: feature flag off"); return; } // Create a new ASRC context if required. if (asrc == nullptr) { log::info("Configuring Asha resampler"); asrc = std::make_unique<bluetooth::audio::asrc::SourceAudioHalAsrc>( /*thread*/ get_main_thread(), /*channels*/ 2, /*sample_rate*/ codec_in_use == CODEC_G722_24KHZ ? 24000 : 16000, /*bit_depth*/ 16, /*interval_us*/ default_data_interval_ms * 1000, /*num_burst_buffers*/ 0, /*burst_delay*/ 0); } } // Reset the ASHA resampling context. void ResetAsrc() { log::info("Resetting the Asha resampling context"); asrc = nullptr; } uint16_t UpdateBleConnParams(const RawAddress& address) { /* List of parameters that depends on the chosen Connection Interval */ uint16_t min_ce_len = MIN_CE_LEN_20MS_CI; uint16_t max_ce_len = MAX_CE_LEN_20MS_CI; uint16_t connection_interval; switch (default_data_interval_ms) { case HA_INTERVAL_10_MS: min_ce_len = MIN_CE_LEN_10MS_CI; connection_interval = CONNECTION_INTERVAL_10MS_PARAM; break; case HA_INTERVAL_20_MS: // When ISO is connected, the controller might not be able to // update the connection event length successfully. // So if ISO is running, we use a small ce length to connect first, // then update to a better value later on if (is_iso_running) { min_ce_len = CE_LEN_20MS_CI_ISO_RUNNING; max_ce_len = CE_LEN_20MS_CI_ISO_RUNNING; needs_parameter_update = true; } else { min_ce_len = MIN_CE_LEN_20MS_CI; max_ce_len = MAX_CE_LEN_20MS_CI; needs_parameter_update = false; } connection_interval = CONNECTION_INTERVAL_20MS_PARAM; break; default: log::error("invalid default_data_interval_ms={}", default_data_interval_ms); min_ce_len = MIN_CE_LEN_10MS_CI; connection_interval = CONNECTION_INTERVAL_10MS_PARAM; } if (overwrite_min_ce_len != -1) { log::warn("min_ce_len={} for device {} is overwritten to {}", min_ce_len, address, overwrite_min_ce_len); min_ce_len = overwrite_min_ce_len; } if (overwrite_max_ce_len != -1) { log::warn("max_ce_len={} for device {} is overwritten to {}", max_ce_len, address, overwrite_max_ce_len); max_ce_len = overwrite_max_ce_len; } log::info( "L2CA_UpdateBleConnParams for device {} min_ce_len:{} max_ce_len:{}", address, min_ce_len, max_ce_len); if (!L2CA_UpdateBleConnParams(address, connection_interval, connection_interval, 0x000A, 0x0064 /*1s*/, min_ce_len, max_ce_len)) { log::warn("Unable to update L2CAP ble connection parameters peer:{}", address); } return connection_interval; } bool IsBelowDropFrequency( std::chrono::time_point<std::chrono::steady_clock> tp) { auto duration = tp - last_drop_time_point; bool droppable = std::chrono::duration_cast<std::chrono::seconds>(duration).count() >= DROP_FREQUENCY_THRESHOLD; log::info("IsBelowDropFrequency {}", droppable); return droppable; } void Connect(const RawAddress& address) { log::info("bd_addr={}", address); hearingDevices.Add(HearingDevice(address, true)); BTA_GATTC_Open(gatt_if, address, BTM_BLE_DIRECT_CONNECTION, false); } void AddToAcceptlist(const RawAddress& address) { log::info("bd_addr={}", address); hearingDevices.Add(HearingDevice(address, true)); BTA_GATTC_Open(gatt_if, address, BTM_BLE_BKG_CONNECT_ALLOW_LIST, false); } void AddFromStorage(const HearingDevice& dev_info, bool is_acceptlisted) { log::info("bd_addr={} hi_sync_id=0x{:x} is_acceptlisted={}", dev_info.address, dev_info.hi_sync_id, is_acceptlisted); if (is_acceptlisted) { hearingDevices.Add(dev_info); // TODO: we should increase the scanning window for few seconds, to get // faster initial connection, same after hearing aid disconnects, i.e. // BTM_BleSetConnScanParams(2048, 1024); /* add device into BG connection to accept remote initiated connection */ BTA_GATTC_Open(gatt_if, dev_info.address, BTM_BLE_BKG_CONNECT_ALLOW_LIST, false); } callbacks->OnDeviceAvailable(dev_info.capabilities, dev_info.hi_sync_id, dev_info.address); } int GetDeviceCount() { return (hearingDevices.size()); } void OnGattConnected(tGATT_STATUS status, uint16_t conn_id, tGATT_IF client_if, RawAddress address, tBT_TRANSPORT transport, uint16_t mtu) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { /* When Hearing Aid is quickly disabled and enabled in settings, this case * might happen */ log::warn("Closing connection to non hearing-aid device: bd_addr={}", address); BTA_GATTC_Close(conn_id); return; } log::info("address={}, conn_id={}", address, conn_id); if (status != GATT_SUCCESS) { if (!hearingDevice->connecting_actively) { // acceptlist connection failed, that's ok. return; } if (hearingDevice->switch_to_background_connection_after_failure) { hearingDevice->connecting_actively = false; hearingDevice->switch_to_background_connection_after_failure = false; BTA_GATTC_Open(gatt_if, address, BTM_BLE_BKG_CONNECT_ALLOW_LIST, false); } else { log::info("Failed to connect to Hearing Aid device, bda={}", address); hearingDevices.Remove(address); callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); } return; } hearingDevice->conn_id = conn_id; uint64_t hi_sync_id = hearingDevice->hi_sync_id; // If there a background connection to the other device of a pair, promote // it to a direct connection to scan more agressively for it if (hi_sync_id != 0) { for (auto& device : hearingDevices.devices) { if (device.hi_sync_id == hi_sync_id && device.conn_id == 0 && !device.connecting_actively) { log::info( "Promoting device from the set from background to direct " "connection, bda={}", device.address); device.connecting_actively = true; device.switch_to_background_connection_after_failure = true; BTA_GATTC_Open(gatt_if, device.address, BTM_BLE_DIRECT_CONNECTION, false); } } } hearingDevice->connection_update_status = STARTED; hearingDevice->requested_connection_interval = UpdateBleConnParams(address); if (bluetooth::shim::GetController()->SupportsBle2mPhy()) { log::info("{} set preferred 2M PHY", address); BTM_BleSetPhy(address, PHY_LE_2M, PHY_LE_2M, 0); } // Set data length // TODO(jpawlowski: for 16khz only 87 is required, optimize BTM_SetBleDataLength(address, 167); if (BTM_SecIsSecurityPending(address)) { /* if security collision happened, wait for encryption done * (BTA_GATTC_ENC_CMPL_CB_EVT) */ return; } /* verify bond */ if (BTM_IsEncrypted(address, BT_TRANSPORT_LE)) { /* if link has been encrypted */ OnEncryptionComplete(address, true); return; } if (BTM_IsLinkKeyKnown(address, BT_TRANSPORT_LE)) { /* if bonded and link not encrypted */ BTM_SetEncryption(address, BT_TRANSPORT_LE, encryption_callback, nullptr, BTM_BLE_SEC_ENCRYPT); return; } /* otherwise let it go through */ OnEncryptionComplete(address, true); } void OnConnectionUpdateComplete(uint16_t conn_id, tBTA_GATTC* p_data) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } if (p_data) { if (p_data->conn_update.status == 0) { bool same_conn_interval = (hearingDevice->requested_connection_interval == p_data->conn_update.interval); switch (hearingDevice->connection_update_status) { case COMPLETED: if (!same_conn_interval) { log::warn( "Unexpected change. Redo. connection interval={}, " "expected={}, conn_id={}, connection_update_status={}", p_data->conn_update.interval, hearingDevice->requested_connection_interval, conn_id, hearingDevice->connection_update_status); // Redo this connection interval change. hearingDevice->connection_update_status = AWAITING; } break; case STARTED: if (same_conn_interval) { log::info("Connection update completed: conn_id={} bd_addr={}", conn_id, hearingDevice->address); hearingDevice->connection_update_status = COMPLETED; } else { log::warn( "Ignored. Different connection interval={}, expected={}, " "conn_id={}, connection_update_status={}", p_data->conn_update.interval, hearingDevice->requested_connection_interval, conn_id, hearingDevice->connection_update_status); // Wait for the right Connection Update Completion. return; } break; case AWAITING: case NONE: break; } // Inform this side and other side device (if any) of Connection // Updates. std::vector<uint8_t> conn_update( {CONTROL_POINT_OP_STATE_CHANGE, STATE_CHANGE_CONN_UPDATE, (uint8_t)p_data->conn_update.interval}); send_state_change_to_other_side(hearingDevice, conn_update); send_state_change(hearingDevice, conn_update); } else { log::info( "error status=0x{:x}, conn_id={} bd_addr={}, " "connection_update_status={}", static_cast<uint8_t>(p_data->conn_update.status), conn_id, hearingDevice->address, hearingDevice->connection_update_status); if (hearingDevice->connection_update_status == STARTED) { // Redo this connection interval change. log::error("Redo Connection Interval change"); hearingDevice->connection_update_status = AWAITING; } } } else { hearingDevice->connection_update_status = NONE; } if (!hearingDevice->accepting_audio && hearingDevice->connection_update_status == COMPLETED && hearingDevice->gap_opened) { OnDeviceReady(hearingDevice->address); } for (auto& device : hearingDevices.devices) { if (device.conn_id && (device.connection_update_status == AWAITING)) { device.connection_update_status = STARTED; device.requested_connection_interval = UpdateBleConnParams(device.address); return; } } } // Completion Callback for the RSSI read operation. void OnReadRssiComplete(const RawAddress& address, int8_t rssi_value) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::info("Skipping unknown device {}", address); return; } log::debug("bd_addr={} rssi={}", address, (int)rssi_value); if (hearingDevice->read_rssi_count <= 0) { log::error("bd_addr={}, invalid read_rssi_count={}", address, hearingDevice->read_rssi_count); return; } rssi_log& last_log_set = hearingDevice->audio_stats.rssi_history.back(); if (hearingDevice->read_rssi_count == READ_RSSI_NUM_TRIES) { // Store the timestamp only for the first one after packet flush clock_gettime(CLOCK_REALTIME, &last_log_set.timestamp); log::info("store time, bd_addr={}, rssi={}", address, (int)rssi_value); } last_log_set.rssi.emplace_back(rssi_value); hearingDevice->read_rssi_count--; } void OnEncryptionComplete(const RawAddress& address, bool success) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::error("unknown device: bd_addr={}", address); return; } if (!success) { log::error("encryption failed: bd_addr={}", address); BTA_GATTC_Close(hearingDevice->conn_id); if (hearingDevice->first_connection) { callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); } return; } log::info("encryption successful: bd_addr={}", address); if (hearingDevice->audio_control_point_handle && hearingDevice->audio_status_handle && hearingDevice->audio_status_ccc_handle && hearingDevice->volume_handle && hearingDevice->read_psm_handle) { // Use cached data, jump to read PSM ReadPSM(hearingDevice); } else { log::info("starting service search request for ASHA: bd_addr={}", address); hearingDevice->first_connection = true; BTA_GATTC_ServiceSearchRequest(hearingDevice->conn_id, HEARING_AID_UUID); } } // Just take care phy update successful case to avoid loop excuting. void OnPhyUpdateEvent(uint16_t conn_id, uint8_t tx_phys, uint8_t rx_phys, tGATT_STATUS status) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } if (status != GATT_SUCCESS) { log::warn("phy update failed: bd_addr={} status={}", hearingDevice->address, status); return; } if (tx_phys == PHY_LE_2M && rx_phys == PHY_LE_2M) { log::info("phy update to 2M successful: bd_addr={}", hearingDevice->address); hearingDevice->phy_update_retry_remain = PHY_UPDATE_RETRY_LIMIT; return; } if (hearingDevice->phy_update_retry_remain > 0) { log::info( "phy update successful with unexpected phys, retrying:" " bd_addr={} tx_phy=0x{:x} rx_phy=0x{:x}", hearingDevice->address, tx_phys, rx_phys); BTM_BleSetPhy(hearingDevice->address, PHY_LE_2M, PHY_LE_2M, 0); hearingDevice->phy_update_retry_remain--; } else { log::warn( "phy update successful with unexpected phys, exceeded retry count:" " bd_addr={} tx_phy=0x{:x} rx_phy=0x{:x}", hearingDevice->address, tx_phys, rx_phys); } } void OnServiceChangeEvent(const RawAddress& address) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::error("unknown device: bd_addr={}", address); return; } log::info("bd_addr={}", address); hearingDevice->first_connection = true; hearingDevice->service_changed_rcvd = true; BtaGattQueue::Clean(hearingDevice->conn_id); if (hearingDevice->gap_handle != GAP_INVALID_HANDLE) { GAP_ConnClose(hearingDevice->gap_handle); hearingDevice->gap_handle = GAP_INVALID_HANDLE; } } void OnServiceDiscDoneEvent(const RawAddress& address) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::error("unknown device: bd_addr={}", address); return; } log::info("bd_addr={}", address); if (hearingDevice->service_changed_rcvd || !(hearingDevice->audio_control_point_handle && hearingDevice->audio_status_handle && hearingDevice->audio_status_ccc_handle && hearingDevice->volume_handle && hearingDevice->read_psm_handle)) { log::info("starting service search request for ASHA: bd_addr={}", address); BTA_GATTC_ServiceSearchRequest(hearingDevice->conn_id, HEARING_AID_UUID); } } void OnServiceSearchComplete(uint16_t conn_id, tGATT_STATUS status) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } // Known device, nothing to do. if (!hearingDevice->first_connection) { log::info("service discovery result ignored: bd_addr={}", hearingDevice->address); return; } if (status != GATT_SUCCESS) { /* close connection and report service discovery complete with error */ log::error("service discovery failed: bd_addr={} status={}", hearingDevice->address, status); if (hearingDevice->first_connection) { callbacks->OnConnectionState(ConnectionState::DISCONNECTED, hearingDevice->address); } return; } log::info("service discovery successful: bd_addr={}", hearingDevice->address); const std::list<gatt::Service>* services = BTA_GATTC_GetServices(conn_id); const gatt::Service* service = nullptr; for (const gatt::Service& tmp : *services) { if (tmp.uuid == Uuid::From16Bit(UUID_SERVCLASS_GATT_SERVER)) { log::info("Found UUID_SERVCLASS_GATT_SERVER, handle=0x{:x}", tmp.handle); const gatt::Service* service_changed_service = &tmp; find_server_changed_ccc_handle(conn_id, service_changed_service); } else if (tmp.uuid == HEARING_AID_UUID) { log::info("Found Hearing Aid service, handle=0x{:x}", tmp.handle); service = &tmp; } } if (!service) { log::error("No Hearing Aid service found"); callbacks->OnConnectionState(ConnectionState::DISCONNECTED, hearingDevice->address); return; } for (const gatt::Characteristic& charac : service->characteristics) { if (charac.uuid == READ_ONLY_PROPERTIES_UUID) { if (!btif_storage_get_hearing_aid_prop( hearingDevice->address, &hearingDevice->capabilities, &hearingDevice->hi_sync_id, &hearingDevice->render_delay, &hearingDevice->preparation_delay, &hearingDevice->codecs)) { log::debug("Reading read only properties 0x{:x}", charac.value_handle); BtaGattQueue::ReadCharacteristic( conn_id, charac.value_handle, HearingAidImpl::OnReadOnlyPropertiesReadStatic, nullptr); } } else if (charac.uuid == AUDIO_CONTROL_POINT_UUID) { hearingDevice->audio_control_point_handle = charac.value_handle; // store audio control point! } else if (charac.uuid == AUDIO_STATUS_UUID) { hearingDevice->audio_status_handle = charac.value_handle; hearingDevice->audio_status_ccc_handle = find_ccc_handle(conn_id, charac.value_handle); if (!hearingDevice->audio_status_ccc_handle) { log::error("cannot find Audio Status CCC descriptor"); continue; } log::info("audio_status_handle=0x{:x}, ccc=0x{:x}", charac.value_handle, hearingDevice->audio_status_ccc_handle); } else if (charac.uuid == VOLUME_UUID) { hearingDevice->volume_handle = charac.value_handle; } else if (charac.uuid == LE_PSM_UUID) { hearingDevice->read_psm_handle = charac.value_handle; } else { log::warn("Unknown characteristic found:{}", charac.uuid.ToString()); } } if (hearingDevice->service_changed_rcvd) { hearingDevice->service_changed_rcvd = false; } ReadPSM(hearingDevice); } void ReadPSM(HearingDevice* hearingDevice) { if (hearingDevice->read_psm_handle) { log::info("bd_addr={} handle=0x{:x}", hearingDevice->address, hearingDevice->read_psm_handle); BtaGattQueue::ReadCharacteristic( hearingDevice->conn_id, hearingDevice->read_psm_handle, HearingAidImpl::OnPsmReadStatic, nullptr); } } void OnNotificationEvent(uint16_t conn_id, uint16_t handle, uint16_t len, uint8_t* value) { HearingDevice* device = hearingDevices.FindByConnId(conn_id); if (!device) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } if (device->audio_status_handle != handle) { log::warn( "unexpected handle: bd_addr={} audio_status_handle=0x{:x}" " handle=0x{:x}", device->address, device->audio_status_handle, handle); return; } if (len < 1) { log::warn("invalid data length (expected 1+ bytes): bd_addr={} len={}", device->address, len); return; } if (value[0] != 0) { log::warn("received error status: bd_addr={} status=0x{:x}", device->address, value[0]); return; } log::info("received success notification: bd_addr={} command_acked={}", device->address, device->command_acked); device->command_acked = true; } void OnReadOnlyPropertiesRead(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, uint8_t* value, void* data) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } uint8_t* p = value; uint8_t version; STREAM_TO_UINT8(version, p); if (version != 0x01) { log::warn("unsupported version: bd_addr={} version=0x{:x}", hearingDevice->address, version); return; } // version 0x01 of read only properties: if (len < 17) { log::warn("invalid data length (expected 17+ bytes): bd_addr={} len={}", hearingDevice->address, len); return; } uint8_t capabilities; STREAM_TO_UINT8(capabilities, p); STREAM_TO_UINT64(hearingDevice->hi_sync_id, p); uint8_t feature_map; STREAM_TO_UINT8(feature_map, p); STREAM_TO_UINT16(hearingDevice->render_delay, p); STREAM_TO_UINT16(hearingDevice->preparation_delay, p); uint16_t codecs; STREAM_TO_UINT16(codecs, p); hearingDevice->capabilities = capabilities; hearingDevice->codecs = codecs; bool side = capabilities & CAPABILITY_SIDE; bool binaural = capabilities & CAPABILITY_BINAURAL; bool csis_capable = capabilities & CAPABILITY_CSIS; if (capabilities & CAPABILITY_RESERVED) { log::warn( "reserved capabilities bits are set: bd_addr={} capabilities=0x{:x}", hearingDevice->address, capabilities); } bool g722_16khz_supported = codecs & (1 << CODEC_G722_16KHZ); bool g722_24khz_supported = codecs & (1 << CODEC_G722_24KHZ); if (!g722_16khz_supported) { log::warn("mandatory codec G722@16kHz not supported: bd_addr={}", hearingDevice->address); } log::info( "device capabilities: bd_addr={} side={} binaural={}" " CSIS_supported={} hi_sync_id=0x{:x} render_delay={}" " preparation_delay={} G722@16kHz_supported={} G722@24kHz_supported={}", hearingDevice->address, side ? "right" : "left", binaural, csis_capable, hearingDevice->hi_sync_id, hearingDevice->render_delay, hearingDevice->preparation_delay, g722_16khz_supported, g722_24khz_supported); } uint16_t CalcCompressedAudioPacketSize(uint16_t codec_type, int connection_interval) { int sample_rate; const int sample_bit_rate = 16; /* 16 bits per sample */ const int compression_ratio = 4; /* G.722 has a 4:1 compression ratio */ if (codec_type == CODEC_G722_24KHZ) { sample_rate = 24000; } else { sample_rate = 16000; } // compressed_data_packet_size is the size in bytes of the compressed audio // data buffer that is generated for each connection interval. uint32_t compressed_data_packet_size = (sample_rate * connection_interval * (sample_bit_rate / 8) / compression_ratio) / 1000; return ((uint16_t)compressed_data_packet_size); } void ChooseCodec(const HearingDevice& hearingDevice) { if (codec_in_use) return; // use the best codec available for this pair of devices. uint16_t codecs = hearingDevice.codecs; if (hearingDevice.hi_sync_id != 0) { for (const auto& device : hearingDevices.devices) { if (device.hi_sync_id != hearingDevice.hi_sync_id) continue; codecs &= device.codecs; } } if ((codecs & (1 << CODEC_G722_24KHZ)) && bluetooth::shim::GetController()->SupportsBle2mPhy() && default_data_interval_ms == HA_INTERVAL_10_MS) { codec_in_use = CODEC_G722_24KHZ; } else if (codecs & (1 << CODEC_G722_16KHZ)) { codec_in_use = CODEC_G722_16KHZ; } } void OnAudioStatus(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, uint8_t* value, void* data) { log::info("{}", base::HexEncode(value, len)); } void OnPsmRead(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, uint8_t* value, void* data) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } if (status != GATT_SUCCESS) { log::error("error reading PSM: bd_addr={} status={}", hearingDevice->address, status); return; } if (len < 2) { log::error("invalid PSM length: bd_addr={} len={}", hearingDevice->address, len); return; } uint16_t psm = 0; STREAM_TO_UINT16(psm, value); log::info("read PSM: bd_addr={} psm=0x{:x}", hearingDevice->address, psm); if (hearingDevice->gap_handle == GAP_INVALID_HANDLE && BTM_IsEncrypted(hearingDevice->address, BT_TRANSPORT_LE)) { ConnectSocket(hearingDevice, psm); } } void ConnectSocket(HearingDevice* hearingDevice, uint16_t psm) { tL2CAP_CFG_INFO cfg_info = tL2CAP_CFG_INFO{.mtu = 512}; log::info("bd_addr={} psm=0x{:x}", hearingDevice->address, psm); SendEnableServiceChangedInd(hearingDevice); uint8_t service_id = hearingDevice->isLeft() ? BTM_SEC_SERVICE_HEARING_AID_LEFT : BTM_SEC_SERVICE_HEARING_AID_RIGHT; uint16_t gap_handle = GAP_ConnOpen( "", service_id, false, &hearingDevice->address, psm, 514 /* MPS */, &cfg_info, nullptr, BTM_SEC_NONE /* TODO: request security ? */, HearingAidImpl::GapCallbackStatic, BT_TRANSPORT_LE); if (gap_handle == GAP_INVALID_HANDLE) { log::error("failed to open socket: bd_addr={}", hearingDevice->address); } else { hearingDevice->gap_handle = gap_handle; log::info("sent GAP connect request: bd_addr={}, gap_handle={}", hearingDevice->address, gap_handle); } } static void OnReadOnlyPropertiesReadStatic(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, uint8_t* value, void* data) { if (instance) instance->OnReadOnlyPropertiesRead(conn_id, status, handle, len, value, data); } static void OnAudioStatusStatic(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, uint8_t* value, void* data) { if (instance) instance->OnAudioStatus(conn_id, status, handle, len, value, data); } static void OnPsmReadStatic(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, uint8_t* value, void* data) { if (instance) instance->OnPsmRead(conn_id, status, handle, len, value, data); } /* CoC Socket, BLE connection parameter are ready */ void OnDeviceReady(const RawAddress& address) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::error("unknown device: bd_addr={}", address); return; } log::info("bd_addr={}", address); if (hearingDevice->first_connection) { btif_storage_add_hearing_aid(*hearingDevice); hearingDevice->first_connection = false; } /* Register and enable the Audio Status Notification */ tGATT_STATUS register_status = BTA_GATTC_RegisterForNotifications( gatt_if, address, hearingDevice->audio_status_handle); if (register_status != GATT_SUCCESS) { log::error( "failed to register for notifications:" " bd_addr={} status={} handle=0x{:x}", address, register_status, hearingDevice->audio_status_handle); return; } std::vector<uint8_t> value(2); uint8_t* ptr = value.data(); UINT16_TO_STREAM(ptr, GATT_CHAR_CLIENT_CONFIG_NOTIFICATION); BtaGattQueue::WriteDescriptor( hearingDevice->conn_id, hearingDevice->audio_status_ccc_handle, std::move(value), GATT_WRITE, write_rpt_ctl_cfg_cb, nullptr); ChooseCodec(*hearingDevice); SendStart(hearingDevice); if (audio_running) { // Inform the other side (if any) of this connection std::vector<uint8_t> inform_conn_state( {CONTROL_POINT_OP_STATE_CHANGE, STATE_CHANGE_OTHER_SIDE_CONNECTED}); send_state_change_to_other_side(hearingDevice, inform_conn_state); } hearingDevice->connecting_actively = false; hearingDevice->accepting_audio = true; StartSendingAudio(*hearingDevice); callbacks->OnDeviceAvailable(hearingDevice->capabilities, hearingDevice->hi_sync_id, address); callbacks->OnConnectionState(ConnectionState::CONNECTED, address); } void StartSendingAudio(const HearingDevice& hearingDevice) { log::info("bd_addr={}", hearingDevice.address); if (encoder_state_left == nullptr) { encoder_state_init(); seq_counter = 0; CodecConfiguration codec; if (codec_in_use == CODEC_G722_24KHZ) { codec.sample_rate = 24000; } else { codec.sample_rate = 16000; } codec.bit_rate = 16; codec.data_interval_ms = default_data_interval_ms; uint16_t delay_report_ms = 0; if (hearingDevice.render_delay != 0) { delay_report_ms = hearingDevice.render_delay + (ADD_RENDER_DELAY_INTERVALS * default_data_interval_ms); } HearingAidAudioSource::Start(codec, audioReceiver, delay_report_ms); } } void OnAudioSuspend(const std::function<void()>& stop_audio_ticks) { log::assert_that((bool)stop_audio_ticks, "stop_audio_ticks is empty"); if (!audio_running) { log::warn("Unexpected audio suspend"); } else { log::info("audio_running={}", audio_running); } // Close the ASRC context. ResetAsrc(); audio_running = false; stop_audio_ticks(); std::vector<uint8_t> stop({CONTROL_POINT_OP_STOP}); for (auto& device : hearingDevices.devices) { if (!device.accepting_audio) continue; if (!device.playback_started) { log::warn("Playback not started, skip send Stop cmd, bd_addr={}", device.address); } else { log::info("send Stop cmd, bd_addr={}", device.address); device.playback_started = false; device.command_acked = false; BtaGattQueue::WriteCharacteristic(device.conn_id, device.audio_control_point_handle, stop, GATT_WRITE, nullptr, nullptr); } } } void OnAudioResume(const std::function<void()>& start_audio_ticks) { log::assert_that((bool)start_audio_ticks, "start_audio_ticks is empty"); if (audio_running) { log::error("Unexpected Audio Resume"); } else { log::info("audio_running={}", audio_running); } for (auto& device : hearingDevices.devices) { if (!device.accepting_audio) continue; audio_running = true; SendStart(&device); } if (!audio_running) { log::info("No device (0/{}) ready to start", GetDeviceCount()); return; } // Open the ASRC context. ConfigureAsrc(); // TODO: shall we also reset the encoder ? encoder_state_release(); encoder_state_init(); seq_counter = 0; start_audio_ticks(); } uint8_t GetOtherSideStreamStatus(HearingDevice* this_side_device) { for (auto& device : hearingDevices.devices) { if ((device.address == this_side_device->address) || (device.hi_sync_id != this_side_device->hi_sync_id)) { continue; } if (audio_running && (device.conn_id != 0)) { return (OTHER_SIDE_IS_STREAMING); } else { return (OTHER_SIDE_NOT_STREAMING); } } return (OTHER_SIDE_NOT_STREAMING); } void SendEnableServiceChangedInd(HearingDevice* device) { log::info("bd_addr={}", device->address); std::vector<uint8_t> value(2); uint8_t* ptr = value.data(); UINT16_TO_STREAM(ptr, GATT_CHAR_CLIENT_CONFIG_INDICTION); BtaGattQueue::WriteDescriptor( device->conn_id, device->service_changed_ccc_handle, std::move(value), GATT_WRITE, nullptr, nullptr); } void SendStart(HearingDevice* device) { std::vector<uint8_t> start({CONTROL_POINT_OP_START, codec_in_use, AUDIOTYPE_UNKNOWN, (uint8_t)current_volume, OTHER_SIDE_NOT_STREAMING}); if (!audio_running) { if (!device->playback_started) { log::info("Skip Send Start since audio is not running, bd_addr={}", device->address); } else { log::error("Audio not running but Playback has started, bd_addr={}", device->address); } return; } if (current_volume == VOLUME_UNKNOWN) start[3] = (uint8_t)VOLUME_MIN; if (device->playback_started) { log::error("Playback already started, skip send Start cmd, bd_addr={}", device->address); } else { start[4] = GetOtherSideStreamStatus(device); log::info( "send Start cmd, volume=0x{:x}, audio type=0x{:x}, bd_addr={}, other " "side streaming=0x{:x}", start[3], start[2], device->address, start[4]); device->command_acked = false; BtaGattQueue::WriteCharacteristic( device->conn_id, device->audio_control_point_handle, start, GATT_WRITE, HearingAidImpl::StartAudioCtrlCallbackStatic, nullptr); } } static void StartAudioCtrlCallbackStatic(uint16_t conn_id, tGATT_STATUS status, uint16_t handle, uint16_t len, const uint8_t* value, void* data) { if (status != GATT_SUCCESS) { log::error("handle={}, conn_id={}, status=0x{:x}", handle, conn_id, static_cast<uint8_t>(status)); return; } if (!instance) { log::error("instance is null"); return; } instance->StartAudioCtrlCallback(conn_id); } void StartAudioCtrlCallback(uint16_t conn_id) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("Skipping unknown device, conn_id=0x{:x}", conn_id); return; } log::info("device: {}", hearingDevice->address); hearingDevice->playback_started = true; } /* Compare the two sides LE CoC credit and return true to drop two sides * packet on these situations. * 1) The credit is close * 2) Other side is disconnected * 3) Get one side current credit value failure. * * Otherwise, just flush audio packet one side. */ bool NeedToDropPacket(HearingDevice* target_side, HearingDevice* other_side) { // Just drop packet if the other side does not exist. if (!other_side) { log::debug("other side not connected to profile"); return true; } uint16_t diff_credit = 0; uint16_t target_current_credit = L2CA_GetPeerLECocCredit( target_side->address, GAP_ConnGetL2CAPCid(target_side->gap_handle)); if (target_current_credit == L2CAP_LE_CREDIT_MAX) { log::error("Get target side credit value fail."); return true; } uint16_t other_current_credit = L2CA_GetPeerLECocCredit( other_side->address, GAP_ConnGetL2CAPCid(other_side->gap_handle)); if (other_current_credit == L2CAP_LE_CREDIT_MAX) { log::error("Get other side credit value fail."); return true; } if (target_current_credit > other_current_credit) { diff_credit = target_current_credit - other_current_credit; } else { diff_credit = other_current_credit - target_current_credit; } log::debug("Target({}) Credit: {}, Other({}) Credit: {}, Init Credit: {}", target_side->address, target_current_credit, other_side->address, other_current_credit, init_credit); return diff_credit < (init_credit / 2 - 1); } void OnAudioDataReadyResample(const std::vector<uint8_t>& data) { if (asrc == nullptr) { return OnAudioDataReady(data); } for (auto const resampled_data : asrc->Run(data)) { OnAudioDataReady(*resampled_data); } } void OnAudioDataReady(const std::vector<uint8_t>& data) { /* For now we assume data comes in as 16bit per sample 16kHz PCM stereo */ bool need_drop = false; int num_samples = data.size() / (2 /*bytes_per_sample*/ * 2 /*number of channels*/); // The G.722 codec accept only even number of samples for encoding if (num_samples % 2 != 0) log::fatal("num_samples is not even: {}", num_samples); // TODO: we should cache left/right and current state, instad of recomputing // it for each packet, 100 times a second. HearingDevice* left = nullptr; HearingDevice* right = nullptr; for (auto& device : hearingDevices.devices) { if (!device.accepting_audio) continue; if (device.isLeft()) left = &device; else right = &device; } if (left == nullptr && right == nullptr) { log::warn("No more (0/{}) devices ready", GetDeviceCount()); DoDisconnectAudioStop(); return; } std::vector<uint16_t> chan_left; std::vector<uint16_t> chan_right; if (left == nullptr || right == nullptr) { for (int i = 0; i < num_samples; i++) { const uint8_t* sample = data.data() + i * 4; int16_t left = (int16_t)((*(sample + 1) << 8) + *sample) >> 1; sample += 2; int16_t right = (int16_t)((*(sample + 1) << 8) + *sample) >> 1; uint16_t mono_data = (int16_t)(((uint32_t)left + (uint32_t)right) >> 1); chan_left.push_back(mono_data); chan_right.push_back(mono_data); } } else { for (int i = 0; i < num_samples; i++) { const uint8_t* sample = data.data() + i * 4; uint16_t left = (int16_t)((*(sample + 1) << 8) + *sample) >> 1; chan_left.push_back(left); sample += 2; uint16_t right = (int16_t)((*(sample + 1) << 8) + *sample) >> 1; chan_right.push_back(right); } } uint16_t l2cap_flush_threshold = 0; if (com::android::bluetooth::flags::higher_l2cap_flush_threshold()) { l2cap_flush_threshold = 1; } // Skipping packets completely messes up the resampler context. // The condition for skipping packets seems to be easily triggered, // causing dropouts that could have been avoided. // // When the resampler is enabled, the flush threshold is set // to the number of credits specified for the ASHA l2cap streaming // channel. This will ensure it is only triggered in case of // critical failure. if (com::android::bluetooth::flags::asha_asrc()) { l2cap_flush_threshold = 8; } // TODO: monural, binarual check // divide encoded data into packets, add header, send. // TODO: make those buffers static and global to prevent constant // reallocations // TODO: this should basically fit the encoded data, tune the size later std::vector<uint8_t> encoded_data_left; auto time_point = std::chrono::steady_clock::now(); if (left) { // TODO: instead of a magic number, we need to figure out the correct // buffer size encoded_data_left.resize(4000); int encoded_size = g722_encode(encoder_state_left, encoded_data_left.data(), (const int16_t*)chan_left.data(), chan_left.size()); encoded_data_left.resize(encoded_size); uint16_t cid = GAP_ConnGetL2CAPCid(left->gap_handle); uint16_t packets_in_chans = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_GET); if (packets_in_chans > l2cap_flush_threshold) { // Compare the two sides LE CoC credit value to confirm need to drop or // skip audio packet. if (NeedToDropPacket(left, right) && IsBelowDropFrequency(time_point)) { log::info("{} triggers dropping, {} packets in channel", left->address, packets_in_chans); need_drop = true; left->audio_stats.trigger_drop_count++; } else { log::info("{} skipping {} packets", left->address, packets_in_chans); left->audio_stats.packet_flush_count += packets_in_chans; left->audio_stats.frame_flush_count++; const uint16_t buffers_left = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_ALL); if (buffers_left) { log::warn( "Unable to flush L2CAP ALL (left HA) channel peer:{} cid:{} " "buffers_left:{}", left->address, cid, buffers_left); } } hearingDevices.StartRssiLog(); } check_and_do_rssi_read(left); } std::vector<uint8_t> encoded_data_right; if (right) { // TODO: instead of a magic number, we need to figure out the correct // buffer size encoded_data_right.resize(4000); int encoded_size = g722_encode(encoder_state_right, encoded_data_right.data(), (const int16_t*)chan_right.data(), chan_right.size()); encoded_data_right.resize(encoded_size); uint16_t cid = GAP_ConnGetL2CAPCid(right->gap_handle); uint16_t packets_in_chans = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_GET); if (packets_in_chans > l2cap_flush_threshold) { // Compare the two sides LE CoC credit value to confirm need to drop or // skip audio packet. if (NeedToDropPacket(right, left) && IsBelowDropFrequency(time_point)) { log::info("{} triggers dropping, {} packets in channel", right->address, packets_in_chans); need_drop = true; right->audio_stats.trigger_drop_count++; } else { log::info("{} skipping {} packets", right->address, packets_in_chans); right->audio_stats.packet_flush_count += packets_in_chans; right->audio_stats.frame_flush_count++; const uint16_t buffers_left = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_ALL); if (buffers_left) { log::warn( "Unable to flush L2CAP ALL (right HA) channel peer:{} cid:{} " "buffers_left:{}", right->address, cid, buffers_left); } } hearingDevices.StartRssiLog(); } check_and_do_rssi_read(right); } size_t encoded_data_size = std::max(encoded_data_left.size(), encoded_data_right.size()); uint16_t packet_size = CalcCompressedAudioPacketSize(codec_in_use, default_data_interval_ms); if (need_drop) { last_drop_time_point = time_point; if (left) { left->audio_stats.packet_drop_count++; } if (right) { right->audio_stats.packet_drop_count++; } return; } for (size_t i = 0; i < encoded_data_size; i += packet_size) { if (left) { left->audio_stats.packet_send_count++; SendAudio(encoded_data_left.data() + i, packet_size, left); } if (right) { right->audio_stats.packet_send_count++; SendAudio(encoded_data_right.data() + i, packet_size, right); } seq_counter++; } if (left) left->audio_stats.frame_send_count++; if (right) right->audio_stats.frame_send_count++; } void SendAudio(uint8_t* encoded_data, uint16_t packet_size, HearingDevice* hearingAid) { if (!hearingAid->playback_started || !hearingAid->command_acked) { log::warn("Playback stalled: bd_addr={} cmd send={} cmd acked={}", hearingAid->address, hearingAid->playback_started, hearingAid->command_acked); return; } BT_HDR* audio_packet = malloc_l2cap_buf(packet_size + 1); uint8_t* p = get_l2cap_sdu_start_ptr(audio_packet); *p = seq_counter; p++; memcpy(p, encoded_data, packet_size); log::verbose("bd_addr={} packet_size={}", hearingAid->address, packet_size); uint16_t result = GAP_ConnWriteData(hearingAid->gap_handle, audio_packet); if (result != BT_PASS) { log::error("Error sending data: 0x{:x}", result); } } void GapCallback(uint16_t gap_handle, uint16_t event, tGAP_CB_DATA* data) { HearingDevice* hearingDevice = hearingDevices.FindByGapHandle(gap_handle); if (!hearingDevice) { log::error("unknown device: gap_handle={} event=0x{:x}", gap_handle, event); return; } switch (event) { case GAP_EVT_CONN_OPENED: { RawAddress address = *GAP_ConnGetRemoteAddr(gap_handle); uint16_t tx_mtu = GAP_ConnGetRemMtuSize(gap_handle); init_credit = L2CA_GetPeerLECocCredit(address, GAP_ConnGetL2CAPCid(gap_handle)); log::info("GAP_EVT_CONN_OPENED: bd_addr={} tx_mtu={} init_credit={}", address, tx_mtu, init_credit); HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::error("unknown device: bd_addr={}", address); return; } hearingDevice->gap_opened = true; if (hearingDevice->connection_update_status == COMPLETED) { OnDeviceReady(address); } break; } case GAP_EVT_CONN_CLOSED: log::info("GAP_EVT_CONN_CLOSED: bd_addr={} accepting_audio={}", hearingDevice->address, hearingDevice->accepting_audio); if (!hearingDevice->accepting_audio) { /* Disconnect connection when data channel is not available */ BTA_GATTC_Close(hearingDevice->conn_id); } else { /* Just clean data channel related parameter when data channel is * available */ hearingDevice->gap_handle = GAP_INVALID_HANDLE; hearingDevice->accepting_audio = false; hearingDevice->playback_started = false; hearingDevice->command_acked = false; hearingDevice->gap_opened = false; } break; case GAP_EVT_CONN_DATA_AVAIL: { log::verbose("GAP_EVT_CONN_DATA_AVAIL: bd_addr={}", hearingDevice->address); // only data we receive back from hearing aids are some stats, not // really important, but useful now for debugging. uint32_t bytes_to_read = 0; GAP_GetRxQueueCnt(gap_handle, &bytes_to_read); std::vector<uint8_t> buffer(bytes_to_read); uint16_t bytes_read = 0; // TODO:GAP_ConnReadData should accpet uint32_t for length! GAP_ConnReadData(gap_handle, buffer.data(), buffer.size(), &bytes_read); if (bytes_read < 4) { log::warn("Wrong data length"); return; } uint8_t* p = buffer.data(); log::verbose("stats from the hearing aid:"); for (size_t i = 0; i + 4 <= buffer.size(); i += 4) { uint16_t event_counter, frame_index; STREAM_TO_UINT16(event_counter, p); STREAM_TO_UINT16(frame_index, p); log::verbose("event_counter={} frame_index: {}", event_counter, frame_index); } break; } case GAP_EVT_TX_EMPTY: log::info("GAP_EVT_TX_EMPTY: bd_addr={}", hearingDevice->address); break; case GAP_EVT_CONN_CONGESTED: log::info("GAP_EVT_CONN_CONGESTED: bd_addr={}", hearingDevice->address); // TODO: make it into function HearingAidAudioSource::Stop(); // TODO: kill the encoder only if all hearing aids are down. // g722_encode_release(encoder_state); // encoder_state_left = nulllptr; // encoder_state_right = nulllptr; break; case GAP_EVT_CONN_UNCONGESTED: log::info("GAP_EVT_CONN_UNCONGESTED: bd_addr={}", hearingDevice->address); break; } } static void GapCallbackStatic(uint16_t gap_handle, uint16_t event, tGAP_CB_DATA* data) { if (instance) instance->GapCallback(gap_handle, event, data); } void DumpRssi(int fd, const HearingDevice& device) { const struct AudioStats* stats = &device.audio_stats; if (stats->rssi_history.size() <= 0) { dprintf(fd, " No RSSI history for %s:\n", ADDRESS_TO_LOGGABLE_CSTR(device.address)); return; } dprintf(fd, " RSSI history for %s:\n", ADDRESS_TO_LOGGABLE_CSTR(device.address)); dprintf(fd, " Time of RSSI 0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9\n"); for (auto& rssi_logs : stats->rssi_history) { if (rssi_logs.rssi.size() <= 0) { break; } char eventtime[20]; char temptime[20]; struct tm* tstamp = localtime(&rssi_logs.timestamp.tv_sec); if (!strftime(temptime, sizeof(temptime), "%H:%M:%S", tstamp)) { log::error("strftime fails. tm_sec={}, tm_min={}, tm_hour={}", tstamp->tm_sec, tstamp->tm_min, tstamp->tm_hour); strlcpy(temptime, "UNKNOWN TIME", sizeof(temptime)); } snprintf(eventtime, sizeof(eventtime), "%s.%03ld", temptime, rssi_logs.timestamp.tv_nsec / 1000000); dprintf(fd, " %s: ", eventtime); for (auto rssi_value : rssi_logs.rssi) { dprintf(fd, " %04d", rssi_value); } dprintf(fd, "\n"); } } void Dump(int fd) { std::stringstream stream; for (const auto& device : hearingDevices.devices) { bool side = device.capabilities & CAPABILITY_SIDE; bool standalone = device.capabilities & CAPABILITY_BINAURAL; stream << " " << device.address.ToString() << " " << (device.accepting_audio ? "" : "not ") << "connected" << "\n " << (standalone ? "binaural" : "monaural") << " " << (side ? "right" : "left") << " " << loghex(device.hi_sync_id) << std::endl; stream << " Trigger dropped counts : " << device.audio_stats.trigger_drop_count << "\n Packet dropped counts : " << device.audio_stats.packet_drop_count << "\n Packet counts (send/flush) : " << device.audio_stats.packet_send_count << " / " << device.audio_stats.packet_flush_count << "\n Frame counts (sent/flush) : " << device.audio_stats.frame_send_count << " / " << device.audio_stats.frame_flush_count << std::endl; DumpRssi(fd, device); } dprintf(fd, "%s", stream.str().c_str()); } void Disconnect(const RawAddress& address) { HearingDevice* hearingDevice = hearingDevices.FindByAddress(address); if (!hearingDevice) { log::error("unknown device: bd_addr={}", address); return; } bool connected = hearingDevice->accepting_audio; bool connecting_by_user = hearingDevice->connecting_actively; log::info("bd_addr={} playback_started={} accepting_audio={}", hearingDevice->address, hearingDevice->playback_started, hearingDevice->accepting_audio); if (hearingDevice->connecting_actively) { // cancel pending direct connect BTA_GATTC_CancelOpen(gatt_if, address, true); } // Removes all registrations for connection. BTA_GATTC_CancelOpen(0, address, false); // Inform the other side (if any) of this disconnection std::vector<uint8_t> inform_disconn_state( {CONTROL_POINT_OP_STATE_CHANGE, STATE_CHANGE_OTHER_SIDE_DISCONNECTED}); send_state_change_to_other_side(hearingDevice, inform_disconn_state); DoDisconnectCleanUp(hearingDevice); if (!connected) { /* In case user wanted to connect, sent DISCONNECTED state */ if (connecting_by_user) { callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); } /* Do remove device when the address is useless. */ hearingDevices.Remove(address); return; } callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address); /* Do remove device when the address is useless. */ hearingDevices.Remove(address); for (const auto& device : hearingDevices.devices) { if (device.accepting_audio) return; } log::info("No more (0/{}) devices ready", GetDeviceCount()); DoDisconnectAudioStop(); } void OnGattDisconnected(uint16_t conn_id, tGATT_IF client_if, RawAddress remote_bda) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x} bd_addr={}", conn_id, remote_bda); return; } log::info("conn_id=0x{:x} bd_addr={}", conn_id, remote_bda); // Inform the other side (if any) of this disconnection std::vector<uint8_t> inform_disconn_state( {CONTROL_POINT_OP_STATE_CHANGE, STATE_CHANGE_OTHER_SIDE_DISCONNECTED}); send_state_change_to_other_side(hearingDevice, inform_disconn_state); DoDisconnectCleanUp(hearingDevice); HearingDevice* other_connected_device_from_set = hearingDevices.FindOtherConnectedDeviceFromSet(*hearingDevice); if (other_connected_device_from_set != nullptr) { log::info( "Another device from the set is still connected, issuing a direct " "connection, other_device_bda={}", other_connected_device_from_set->address); } // If another device from the pair is still connected, do a direct // connection to scan more aggressively and connect as fast as possible hearingDevice->connecting_actively = other_connected_device_from_set != nullptr; auto connection_type = hearingDevice->connecting_actively ? BTM_BLE_DIRECT_CONNECTION : BTM_BLE_BKG_CONNECT_ALLOW_LIST; hearingDevice->switch_to_background_connection_after_failure = connection_type == BTM_BLE_DIRECT_CONNECTION; // This is needed just for the first connection. After stack is restarted, // code that loads device will add them to acceptlist. BTA_GATTC_Open(gatt_if, hearingDevice->address, connection_type, false); callbacks->OnConnectionState(ConnectionState::DISCONNECTED, remote_bda); for (const auto& device : hearingDevices.devices) { if (device.accepting_audio) return; } log::info("No more (0/{}) devices ready", GetDeviceCount()); DoDisconnectAudioStop(); } void DoDisconnectCleanUp(HearingDevice* hearingDevice) { if (hearingDevice->connection_update_status != COMPLETED) { log::info("connection update not completed: status={}, bd_addr={}", hearingDevice->connection_update_status, hearingDevice->address); if (hearingDevice->connection_update_status == STARTED) { OnConnectionUpdateComplete(hearingDevice->conn_id, NULL); } } hearingDevice->connection_update_status = NONE; hearingDevice->gap_opened = false; if (hearingDevice->conn_id) { BtaGattQueue::Clean(hearingDevice->conn_id); BTA_GATTC_Close(hearingDevice->conn_id); hearingDevice->conn_id = 0; } if (hearingDevice->gap_handle != GAP_INVALID_HANDLE) { GAP_ConnClose(hearingDevice->gap_handle); hearingDevice->gap_handle = GAP_INVALID_HANDLE; } hearingDevice->accepting_audio = false; log::info("bd_addr={} playback_started={}", hearingDevice->address, hearingDevice->playback_started); hearingDevice->playback_started = false; hearingDevice->command_acked = false; } void DoDisconnectAudioStop() { HearingAidAudioSource::Stop(); audio_running = false; encoder_state_release(); current_volume = VOLUME_UNKNOWN; } void SetVolume(int8_t volume) { log::debug("{}", volume); current_volume = volume; for (HearingDevice& device : hearingDevices.devices) { if (!device.accepting_audio) continue; std::vector<uint8_t> volume_value({static_cast<unsigned char>(volume)}); BtaGattQueue::WriteCharacteristic(device.conn_id, device.volume_handle, volume_value, GATT_WRITE_NO_RSP, nullptr, nullptr); } } void CleanUp() { BTA_GATTC_AppDeregister(gatt_if); for (HearingDevice& device : hearingDevices.devices) { DoDisconnectCleanUp(&device); } hearingDevices.devices.clear(); encoder_state_release(); } private: uint8_t gatt_if; uint8_t seq_counter; /* current volume gain for the hearing aids*/ int8_t current_volume; bluetooth::hearing_aid::HearingAidCallbacks* callbacks; /* currently used codec */ uint8_t codec_in_use; uint16_t default_data_interval_ms; uint16_t init_credit; HearingDevices hearingDevices; void find_server_changed_ccc_handle(uint16_t conn_id, const gatt::Service* service) { HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id); if (!hearingDevice) { log::error("unknown device: conn_id=0x{:x}", conn_id); return; } for (const gatt::Characteristic& charac : service->characteristics) { if (charac.uuid == Uuid::From16Bit(GATT_UUID_GATT_SRV_CHGD)) { hearingDevice->service_changed_ccc_handle = find_ccc_handle(conn_id, charac.value_handle); if (!hearingDevice->service_changed_ccc_handle) { log::error( "failed to find service changed CCC descriptor: bd_addr={}", hearingDevice->address); continue; } log::info("bd_addr={} service_changed_ccc=0x{:x}", hearingDevice->address, hearingDevice->service_changed_ccc_handle); break; } } } // Find the handle for the client characteristics configuration of a given // characteristics uint16_t find_ccc_handle(uint16_t conn_id, uint16_t char_handle) { const gatt::Characteristic* p_char = BTA_GATTC_GetCharacteristic(conn_id, char_handle); if (!p_char) { log::warn("No such characteristic: {}", char_handle); return 0; } for (const gatt::Descriptor& desc : p_char->descriptors) { if (desc.uuid == Uuid::From16Bit(GATT_UUID_CHAR_CLIENT_CONFIG)) return desc.handle; } return 0; } void send_state_change(HearingDevice* device, std::vector<uint8_t> payload) { if (device->conn_id != 0) { if (device->service_changed_rcvd) { log::info( "service discover is in progress, skip send State Change cmd."); return; } // Send the data packet log::info("Send State Change: bd_addr={} status=0x{:x}", device->address, payload[1]); BtaGattQueue::WriteCharacteristic( device->conn_id, device->audio_control_point_handle, payload, GATT_WRITE_NO_RSP, nullptr, nullptr); } } void send_state_change_to_other_side(HearingDevice* this_side_device, std::vector<uint8_t> payload) { for (auto& device : hearingDevices.devices) { if ((device.address == this_side_device->address) || (device.hi_sync_id != this_side_device->hi_sync_id)) { continue; } send_state_change(&device, payload); } } void check_and_do_rssi_read(HearingDevice* device) { if (device->read_rssi_count > 0) { device->num_intervals_since_last_rssi_read++; if (device->num_intervals_since_last_rssi_read >= PERIOD_TO_READ_RSSI_IN_INTERVALS) { device->num_intervals_since_last_rssi_read = 0; log::debug("bd_addr={}", device->address); BTM_ReadRSSI(device->address, read_rssi_callback); } } } }; static void read_rssi_callback(void* p_void) { tBTM_RSSI_RESULT* p_result = (tBTM_RSSI_RESULT*)p_void; if (!p_result) return; if ((instance) && (p_result->status == BTM_SUCCESS)) { instance->OnReadRssiComplete(p_result->rem_bda, p_result->rssi); } } static void hearingaid_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data) { if (p_data == nullptr) return; switch (event) { case BTA_GATTC_DEREG_EVT: log::info(""); break; case BTA_GATTC_OPEN_EVT: { if (!instance) return; tBTA_GATTC_OPEN& o = p_data->open; instance->OnGattConnected(o.status, o.conn_id, o.client_if, o.remote_bda, o.transport, o.mtu); break; } case BTA_GATTC_CLOSE_EVT: { if (!instance) return; tBTA_GATTC_CLOSE& c = p_data->close; instance->OnGattDisconnected(c.conn_id, c.client_if, c.remote_bda); } break; case BTA_GATTC_SEARCH_CMPL_EVT: if (!instance) return; instance->OnServiceSearchComplete(p_data->search_cmpl.conn_id, p_data->search_cmpl.status); break; case BTA_GATTC_NOTIF_EVT: if (!instance) return; if (!p_data->notify.is_notify || p_data->notify.len > GATT_MAX_ATTR_LEN) { log::error("rejected BTA_GATTC_NOTIF_EVT. is_notify={}, len={}", p_data->notify.is_notify, p_data->notify.len); break; } instance->OnNotificationEvent(p_data->notify.conn_id, p_data->notify.handle, p_data->notify.len, p_data->notify.value); break; case BTA_GATTC_ENC_CMPL_CB_EVT: if (!instance) return; instance->OnEncryptionComplete( p_data->enc_cmpl.remote_bda, BTM_IsEncrypted(p_data->enc_cmpl.remote_bda, BT_TRANSPORT_LE)); break; case BTA_GATTC_CONN_UPDATE_EVT: if (!instance) return; instance->OnConnectionUpdateComplete(p_data->conn_update.conn_id, p_data); break; case BTA_GATTC_SRVC_CHG_EVT: if (!instance) return; instance->OnServiceChangeEvent(p_data->remote_bda); break; case BTA_GATTC_SRVC_DISC_DONE_EVT: if (!instance) return; instance->OnServiceDiscDoneEvent(p_data->service_changed.remote_bda); break; case BTA_GATTC_PHY_UPDATE_EVT: { if (!instance) return; tBTA_GATTC_PHY_UPDATE& p = p_data->phy_update; instance->OnPhyUpdateEvent(p.conn_id, p.tx_phy, p.rx_phy, p.status); break; } default: break; } } static void encryption_callback(RawAddress address, tBT_TRANSPORT, void*, tBTM_STATUS status) { if (instance) { instance->OnEncryptionComplete(address, status == BTM_SUCCESS ? true : false); } } class HearingAidAudioReceiverImpl : public HearingAidAudioReceiver { public: void OnAudioDataReady(const std::vector<uint8_t>& data) override { if (instance) instance->OnAudioDataReadyResample(data); } void OnAudioSuspend(const std::function<void()>& stop_audio_ticks) override { if (instance) instance->OnAudioSuspend(stop_audio_ticks); } void OnAudioResume(const std::function<void()>& start_audio_ticks) override { if (instance) instance->OnAudioResume(start_audio_ticks); } }; HearingAidAudioReceiverImpl audioReceiverImpl; } // namespace void HearingAid::Initialize( bluetooth::hearing_aid::HearingAidCallbacks* callbacks, Closure initCb) { std::scoped_lock<std::mutex> lock(instance_mutex); if (instance) { log::error("Already initialized!"); return; } audioReceiver = &audioReceiverImpl; instance = new HearingAidImpl(callbacks, initCb); HearingAidAudioSource::Initialize(); } bool HearingAid::IsHearingAidRunning() { return instance; } void HearingAid::Connect(const RawAddress& address) { if (!instance) { log::error("Hearing Aid instance is not available"); return; } instance->Connect(address); } void HearingAid::Disconnect(const RawAddress& address) { if (!instance) { log::error("Hearing Aid instance is not available"); return; } instance->Disconnect(address); } void HearingAid::AddToAcceptlist(const RawAddress& address) { if (!instance) { log::error("Hearing Aid instance is not available"); return; } instance->AddToAcceptlist(address); } void HearingAid::SetVolume(int8_t volume) { if (!instance) { log::error("Hearing Aid instance is not available"); return; } instance->SetVolume(volume); } void HearingAid::AddFromStorage(const HearingDevice& dev_info, bool is_acceptlisted) { if (!instance) { log::error("Not initialized yet"); } instance->AddFromStorage(dev_info, is_acceptlisted); }; int HearingAid::GetDeviceCount() { if (!instance) { log::info("Not initialized yet"); return 0; } return (instance->GetDeviceCount()); } void HearingAid::CleanUp() { std::scoped_lock<std::mutex> lock(instance_mutex); // Must stop audio source to make sure it doesn't call any of callbacks on our // soon to be null instance HearingAidAudioSource::Stop(); HearingAidImpl* ptr = instance; instance = nullptr; HearingAidAudioSource::CleanUp(); ptr->CleanUp(); delete ptr; }; void HearingAid::DebugDump(int fd) { std::scoped_lock<std::mutex> lock(instance_mutex); dprintf(fd, "Hearing Aid Manager:\n"); if (instance) instance->Dump(fd); HearingAidAudioSource::DebugDump(fd); dprintf(fd, "\n"); }