blob: 87795422daebbb18df0b3df87684ebbfba801582 [file] [log] [blame]
/******************************************************************************
*
* 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.
*
******************************************************************************/
#include "bta_hearing_aid_api.h"
#include "bta_gatt_api.h"
#include "bta_gatt_queue.h"
#include "btm_int.h"
#include "device/include/controller.h"
#include "embdrv/g722/g722_enc_dec.h"
#include "gap_api.h"
#include "gatt_api.h"
#include <base/bind.h>
#include <base/logging.h>
#include <base/strings/string_number_conversions.h>
#include <hardware/bt_hearing_aid.h>
#include <vector>
using base::Closure;
using bluetooth::Uuid;
using bluetooth::hearing_aid::ConnectionState;
void btif_storage_add_hearing_aid(const RawAddress& address, uint16_t psm,
uint8_t capabilities, uint16_t codecs,
uint16_t audio_control_point_handle,
uint16_t volume_handle, uint64_t hiSyncId,
uint16_t render_delay,
uint16_t preparation_delay);
constexpr uint8_t CODEC_G722_16KHZ = 0x01;
constexpr uint8_t CODEC_G722_24KHZ = 0x02;
// Masks for checking capability support
constexpr uint8_t CAPABILITY_SIDE = 0x01;
constexpr uint8_t CAPABILITY_BINAURAL = 0x02;
constexpr uint8_t CAPABILITY_RESERVED = 0xFC;
// audio control point opcodes
constexpr uint8_t CONTROL_POINT_OP_START = 0x01;
constexpr uint8_t CONTROL_POINT_OP_STOP = 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;
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
constexpr uint16_t MIN_CE_LEN_1M = 0x0006;
void hearingaid_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data);
void encryption_callback(const RawAddress*, tGATT_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;
}
struct AudioStats {
size_t packet_flush_count;
size_t packet_send_count;
size_t frame_flush_count;
size_t frame_send_count;
AudioStats() { Reset(); }
void Reset() {
packet_flush_count = 0;
packet_send_count = 0;
frame_flush_count = 0;
frame_send_count = 0;
}
};
class HearingAidImpl;
HearingAidImpl* instance;
HearingAidAudioReceiver* audioReceiver;
struct HearingDevice {
RawAddress address;
/* This is true only during first connection to profile, until we store the
* device */
bool first_connection;
/* we are making active attempt to connect to this device, 'direct connect'.
* This is true only during initial phase of first connection. */
bool connecting_actively;
/* For two hearing aids, you must update their parameters one after another,
* not simulteanously, to ensure start of connection events for both devices
* are far from each other. This flag means that this device is waiting for
* update of parameters, that should happen after "LE Connection Update
* Complete" event
*/
bool connection_update_pending;
/* if true, we are connected, L2CAP socket is open, we can stream audio*/
bool accepting_audio;
uint16_t conn_id;
uint16_t gap_handle;
uint16_t audio_control_point_handle;
uint16_t volume_handle;
uint16_t psm;
uint8_t capabilities;
uint64_t hi_sync_id;
uint16_t render_delay;
uint16_t preparation_delay;
uint16_t codecs;
AudioStats audio_stats;
HearingDevice(const RawAddress& address, uint16_t psm, uint8_t capabilities,
uint16_t codecs, uint16_t audio_control_point_handle,
uint16_t volume_handle, uint64_t hiSyncId,
uint16_t render_delay, uint16_t preparation_delay)
: address(address),
first_connection(false),
connecting_actively(false),
connection_update_pending(false),
accepting_audio(false),
conn_id(0),
gap_handle(0),
audio_control_point_handle(audio_control_point_handle),
volume_handle(volume_handle),
psm(psm),
capabilities(capabilities),
hi_sync_id(hiSyncId),
render_delay(render_delay),
preparation_delay(preparation_delay),
codecs(codecs) {}
HearingDevice(const RawAddress& address, bool first_connection)
: address(address),
first_connection(first_connection),
connecting_actively(first_connection),
connection_update_pending(false),
accepting_audio(false),
conn_id(0),
gap_handle(0),
psm(0) {}
HearingDevice() { HearingDevice(RawAddress::kEmpty, false); }
/* return true if this device represents left Hearing Aid. Returned value is
* valid only after capabilities are discovered */
bool isLeft() const { return !(capabilities & CAPABILITY_SIDE); }
};
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* 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);
}
bool IsAnyConnectionUpdatePending() {
for (const auto& d : devices) {
if (d.connection_update_pending) return true;
}
return false;
}
size_t size() { return (devices.size()); }
std::vector<HearingDevice> devices;
};
g722_encode_state_t* encoder_state_left = nullptr;
g722_encode_state_t* encoder_state_right = nullptr;
class HearingAidImpl : public HearingAid {
public:
virtual ~HearingAidImpl() = default;
HearingAidImpl(bluetooth::hearing_aid::HearingAidCallbacks* callbacks,
Closure initCb)
: gatt_if(0),
seq_counter(0),
current_volume(VOLUME_UNKNOWN),
callbacks(callbacks) {
DVLOG(2) << __func__;
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));
}
void Connect(const RawAddress& address) override {
DVLOG(2) << __func__ << " " << address;
hearingDevices.Add(HearingDevice(address, true));
BTA_GATTC_Open(gatt_if, address, true, GATT_TRANSPORT_LE, false);
}
void AddFromStorage(const RawAddress& address, uint16_t psm,
uint8_t capabilities, uint16_t codecs,
uint16_t audio_control_point_handle,
uint16_t volume_handle, uint64_t hiSyncId,
uint16_t render_delay, uint16_t preparation_delay,
uint16_t is_white_listed) {
DVLOG(2) << __func__ << " " << address << ", hiSyncId=" << loghex(hiSyncId)
<< ", isWhiteListed=" << is_white_listed;
if (is_white_listed) {
hearingDevices.Add(HearingDevice(
address, psm, capabilities, codecs, audio_control_point_handle,
volume_handle, hiSyncId, render_delay, preparation_delay));
// 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, address, false, GATT_TRANSPORT_LE, false);
BTA_DmBleStartAutoConn();
}
callbacks->OnDeviceAvailable(capabilities, hiSyncId, address);
}
int GetDeviceCount() { return (hearingDevices.size()); }
void OnGattConnected(tGATT_STATUS status, uint16_t conn_id,
tGATT_IF client_if, RawAddress address,
tBTA_TRANSPORT transport, uint16_t mtu) {
VLOG(2) << __func__ << " " << address;
HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
if (!hearingDevice) {
DVLOG(2) << "Skipping unknown device, address=" << address;
return;
}
if (status != GATT_SUCCESS) {
if (!hearingDevice->connecting_actively) {
// whitelist connection failed, that's ok.
return;
}
LOG(INFO) << "Failed to connect to Hearing Aid device";
hearingDevices.Remove(address);
callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address);
return;
}
hearingDevice->connecting_actively = false;
hearingDevice->conn_id = conn_id;
/* We must update connection parameters one at a time, otherwise anchor
* point (start of connection event) for two devices can be too close to
* each other. Here, by setting min_ce_len=max_ce_len=X, we force controller
* to move anchor point of both connections away from each other, to make
* sure we'll be able to fit all the data we want in one connection event.
*/
bool any_update_pending = hearingDevices.IsAnyConnectionUpdatePending();
// mark the device as pending connection update. If we don't start the
// update now, it'll be started once current device finishes.
hearingDevice->connection_update_pending = true;
if (!any_update_pending) {
L2CA_UpdateBleConnParams(address, 0x0008, 0x0008, 0x000A, 0x0064 /*1s*/,
MIN_CE_LEN_1M, MIN_CE_LEN_1M);
}
// Set data length
// TODO(jpawlowski: for 16khz only 87 is required, optimize
BTM_SetBleDataLength(address, 147);
tBTM_SEC_DEV_REC* p_dev_rec = btm_find_dev(address);
if (p_dev_rec) {
if (p_dev_rec->sec_state == BTM_SEC_STATE_ENCRYPTING ||
p_dev_rec->sec_state == BTM_SEC_STATE_AUTHENTICATING) {
/* if security collision happened, wait for encryption done
* (BTA_GATTC_ENC_CMPL_CB_EVT) */
return;
}
}
/* verify bond */
uint8_t sec_flag = 0;
BTM_GetSecurityFlagsByTransport(address, &sec_flag, BT_TRANSPORT_LE);
if (sec_flag & BTM_SEC_FLAG_ENCRYPTED) {
/* if link has been encrypted */
OnEncryptionComplete(address, true);
return;
}
if (sec_flag & BTM_SEC_FLAG_LKEY_KNOWN) {
/* if bonded and link not encrypted */
sec_flag = BTM_BLE_SEC_ENCRYPT;
BTM_SetEncryption(address, BTA_TRANSPORT_LE, encryption_callback, nullptr,
sec_flag);
return;
}
/* otherwise let it go through */
OnEncryptionComplete(address, true);
}
void OnConnectionUpdateComplete(uint16_t conn_id) {
HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id);
if (!hearingDevice) {
DVLOG(2) << "Skipping unknown device, conn_id=" << loghex(conn_id);
return;
}
hearingDevice->connection_update_pending = false;
for (auto& device : hearingDevices.devices) {
if (device.conn_id && device.connection_update_pending) {
L2CA_UpdateBleConnParams(device.address, 0x0008, 0x0008, 0x000A,
0x0064 /*1s*/, MIN_CE_LEN_1M, MIN_CE_LEN_1M);
return;
}
}
}
void OnEncryptionComplete(const RawAddress& address, bool success) {
HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
if (!hearingDevice) {
DVLOG(2) << "Skipping unknown device" << address;
return;
}
if (!success) {
LOG(ERROR) << "encryption failed";
BTA_GATTC_Close(hearingDevice->conn_id);
if (hearingDevice->first_connection) {
callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address);
}
return;
}
DVLOG(2) << __func__ << " " << address;
if (!hearingDevice->first_connection) {
// Use cached data, jump to connecting socket
ConnectSocket(hearingDevice);
return;
}
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) {
DVLOG(2) << "Skipping unknown device, conn_id=" << loghex(conn_id);
return;
}
// Known device, nothing to do.
if (!hearingDevice->first_connection) return;
if (status != GATT_SUCCESS) {
/* close connection and report service discovery complete with error */
LOG(ERROR) << "Service discovery failed";
if (hearingDevice->first_connection) {
callbacks->OnConnectionState(ConnectionState::DISCONNECTED,
hearingDevice->address);
}
return;
}
const std::vector<tBTA_GATTC_SERVICE>* services =
BTA_GATTC_GetServices(conn_id);
const tBTA_GATTC_SERVICE* service = nullptr;
for (const tBTA_GATTC_SERVICE& tmp : *services) {
if (tmp.uuid != HEARING_AID_UUID) continue;
LOG(INFO) << "Found Hearing Aid service, handle=" << loghex(tmp.handle);
service = &tmp;
break;
}
if (!service) {
LOG(ERROR) << "No Hearing Aid service found";
callbacks->OnConnectionState(ConnectionState::DISCONNECTED,
hearingDevice->address);
return;
}
uint16_t psm_handle = 0x0000;
for (const tBTA_GATTC_CHARACTERISTIC& charac : service->characteristics) {
if (charac.uuid == READ_ONLY_PROPERTIES_UUID) {
DVLOG(2) << "Reading read only properties "
<< loghex(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) {
DVLOG(2) << "Reading Audio status " << loghex(charac.value_handle);
BtaGattQueue::ReadCharacteristic(conn_id, charac.value_handle,
HearingAidImpl::OnAudioStatusStatic,
nullptr);
} else if (charac.uuid == VOLUME_UUID) {
hearingDevice->volume_handle = charac.value_handle;
} else if (charac.uuid == LE_PSM_UUID) {
psm_handle = charac.value_handle;
} else {
LOG(WARNING) << "Unknown characteristic found:" << charac.uuid;
}
}
if (psm_handle) {
DVLOG(2) << "Reading PSM " << loghex(psm_handle);
BtaGattQueue::ReadCharacteristic(
conn_id, psm_handle, HearingAidImpl::OnPsmReadStatic, nullptr);
}
}
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) {
DVLOG(2) << __func__ << "unknown conn_id=" << loghex(conn_id);
return;
}
VLOG(2) << __func__ << " " << base::HexEncode(value, len);
uint8_t* p = value;
uint8_t version;
STREAM_TO_UINT8(version, p);
if (version != 0x01) {
LOG(WARNING) << "Unknown version: " << loghex(version);
return;
}
// version 0x01 of read only properties:
if (len < 17) {
LOG(WARNING) << "Read only properties too short: " << loghex(len);
return;
}
uint8_t capabilities;
STREAM_TO_UINT8(capabilities, p);
hearingDevice->capabilities = capabilities;
bool side = capabilities & CAPABILITY_SIDE;
bool standalone = capabilities & CAPABILITY_BINAURAL;
VLOG(2) << __func__ << " capabilities: " << (side ? "right" : "left")
<< ", " << (standalone ? "binaural" : "monaural");
if (capabilities & CAPABILITY_RESERVED) {
LOG(WARNING) << __func__ << " reserved capabilities are set";
}
STREAM_TO_UINT64(hearingDevice->hi_sync_id, p);
VLOG(2) << __func__ << " hiSyncId: " << loghex(hearingDevice->hi_sync_id);
uint8_t feature_map;
STREAM_TO_UINT8(feature_map, p);
STREAM_TO_UINT16(hearingDevice->render_delay, p);
VLOG(2) << __func__
<< " render delay: " << loghex(hearingDevice->render_delay);
STREAM_TO_UINT16(hearingDevice->preparation_delay, p);
VLOG(2) << __func__ << " preparation delay: "
<< loghex(hearingDevice->preparation_delay);
uint16_t codecs;
STREAM_TO_UINT16(codecs, p);
hearingDevice->codecs = codecs;
VLOG(2) << __func__ << " supported codecs: " << loghex(codecs);
if (codecs & (1 << CODEC_G722_16KHZ)) VLOG(2) << "\tG722@16kHz";
if (codecs & (1 << CODEC_G722_24KHZ)) VLOG(2) << "\tG722@24kHz";
if (!(codecs & (1 << CODEC_G722_16KHZ))) {
LOG(WARNING) << __func__ << " Mandatory codec, G722@16kHz not supported";
}
}
void OnAudioStatus(uint16_t conn_id, tGATT_STATUS status, uint16_t handle,
uint16_t len, uint8_t* value, void* data) {
DVLOG(2) << __func__ << " " << 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) {
DVLOG(2) << "Skipping unknown read event, conn_id=" << loghex(conn_id);
return;
}
if (status != GATT_SUCCESS) {
LOG(ERROR) << "Error reading PSM for device" << hearingDevice->address;
return;
}
if (len > 2) {
LOG(ERROR) << "Bad PSM length";
return;
}
uint16_t psm_val = *((uint16_t*)value);
hearingDevice->psm = psm_val;
VLOG(2) << "read psm:" << loghex(hearingDevice->psm);
ConnectSocket(hearingDevice);
}
void ConnectSocket(HearingDevice* hearingDevice) {
tL2CAP_CFG_INFO cfg_info = tL2CAP_CFG_INFO{.mtu = 512};
uint16_t gap_handle = GAP_ConnOpen(
"", 0, false, &hearingDevice->address, hearingDevice->psm,
514 /* MPS */, &cfg_info, nullptr,
BTM_SEC_NONE /* TODO: request security ? */, L2CAP_FCR_LE_COC_MODE,
HearingAidImpl::GapCallbackStatic, BT_TRANSPORT_LE);
if (gap_handle == GAP_INVALID_HANDLE) {
LOG(ERROR) << "UNABLE TO GET gap_handle";
return;
}
hearingDevice->gap_handle = gap_handle;
LOG(INFO) << "Successfully sent GAP connect request";
}
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 is ready */
void OnGapConnection(const RawAddress& address) {
HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
if (!hearingDevice) {
LOG(INFO) << "Device not connected to profile" << address;
return;
}
if (hearingDevice->first_connection) {
/* add device into BG connection to accept remote initiated connection */
BTA_GATTC_Open(gatt_if, address, false, GATT_TRANSPORT_LE, false);
BTA_DmBleStartAutoConn();
btif_storage_add_hearing_aid(
address, hearingDevice->psm, hearingDevice->capabilities,
hearingDevice->codecs, hearingDevice->audio_control_point_handle,
hearingDevice->volume_handle, hearingDevice->hi_sync_id,
hearingDevice->render_delay, hearingDevice->preparation_delay);
hearingDevice->first_connection = false;
}
SendStart(*hearingDevice);
hearingDevice->accepting_audio = true;
LOG(INFO) << __func__ << ": address=" << address
<< ", hi_sync_id=" << loghex(hearingDevice->hi_sync_id);
callbacks->OnDeviceAvailable(hearingDevice->capabilities,
hearingDevice->hi_sync_id, address);
callbacks->OnConnectionState(ConnectionState::CONNECTED, address);
StartSendingAudio(*hearingDevice);
}
void StartSendingAudio(const HearingDevice& hearingDevice) {
VLOG(0) << __func__ << hearingDevice.address;
if (encoder_state_left == nullptr) {
encoder_state_left = g722_encode_init(nullptr, 64000, G722_PACKED);
encoder_state_right = g722_encode_init(nullptr, 64000, G722_PACKED);
seq_counter = 0;
// use the best codec avaliable 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)) &&
controller_get_interface()->supports_ble_2m_phy()) {
codec_in_use = CODEC_G722_24KHZ;
codec.sample_rate = 24000;
codec.bit_rate = 16;
codec.data_interval_ms = 10;
} else if (codecs & (1 << CODEC_G722_16KHZ)) {
codec_in_use = CODEC_G722_16KHZ;
codec.sample_rate = 16000;
codec.bit_rate = 16;
codec.data_interval_ms = 10;
}
// TODO: remove once we implement support for other codecs
codec_in_use = CODEC_G722_16KHZ;
HearingAidAudioSource::Start(codec, audioReceiver);
}
}
void OnAudioSuspend() {
DVLOG(2) << __func__;
std::vector<uint8_t> stop({CONTROL_POINT_OP_STOP});
for (const auto& device : hearingDevices.devices) {
if (!device.accepting_audio) continue;
BtaGattQueue::WriteCharacteristic(device.conn_id,
device.audio_control_point_handle, stop,
GATT_WRITE, nullptr, nullptr);
}
}
void OnAudioResume() {
DVLOG(2) << __func__;
// TODO: shall we also reset the encoder ?
if (encoder_state_left != nullptr) {
g722_encode_release(encoder_state_left);
g722_encode_release(encoder_state_right);
encoder_state_left = g722_encode_init(nullptr, 64000, G722_PACKED);
encoder_state_right = g722_encode_init(nullptr, 64000, G722_PACKED);
}
seq_counter = 0;
for (const auto& device : hearingDevices.devices) {
if (!device.accepting_audio) continue;
SendStart(device);
}
}
void SendStart(const HearingDevice& device) {
std::vector<uint8_t> start({CONTROL_POINT_OP_START, codec_in_use,
0x02 /* media */, (uint8_t)current_volume});
if (current_volume == VOLUME_UNKNOWN) start[3] = (uint8_t)VOLUME_MIN;
BtaGattQueue::WriteCharacteristic(device.conn_id,
device.audio_control_point_handle, start,
GATT_WRITE, nullptr, nullptr);
// TODO(jpawlowski): this will be removed, once test devices get volume
// from start reqest
if (current_volume != VOLUME_UNKNOWN) {
std::vector<uint8_t> volume_value(
{static_cast<unsigned char>(current_volume)});
BtaGattQueue::WriteCharacteristic(device.conn_id, device.volume_handle,
volume_value, GATT_WRITE_NO_RSP,
nullptr, nullptr);
}
}
void OnAudioDataReady(const std::vector<uint8_t>& data) {
/* For now we assume data comes in as 16bit per sample 16kHz PCM stereo */
DVLOG(2) << __func__;
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;
std::vector<uint16_t> chan_left;
std::vector<uint16_t> chan_right;
// TODO: encode data into G.722 left/right or mono.
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);
}
// 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) {
HearingAidAudioSource::Stop();
current_volume = VOLUME_UNKNOWN;
return;
}
// 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;
if (left) {
encoded_data_left.resize(2000);
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_to_flush = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_GET);
if (packets_to_flush) {
VLOG(2) << left->address << " skipping " << packets_to_flush
<< " packets";
left->audio_stats.packet_flush_count += packets_to_flush;
left->audio_stats.frame_flush_count++;
}
// flush all packets stuck in queue
L2CA_FlushChannel(cid, 0xffff);
}
std::vector<uint8_t> encoded_data_right;
if (right) {
encoded_data_right.resize(2000);
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_to_flush = L2CA_FlushChannel(cid, L2CAP_FLUSH_CHANS_GET);
if (packets_to_flush) {
VLOG(2) << right->address << " skipping " << packets_to_flush
<< " packets";
right->audio_stats.packet_flush_count += packets_to_flush;
right->audio_stats.frame_flush_count++;
}
// flush all packets stuck in queue
L2CA_FlushChannel(cid, 0xffff);
}
size_t encoded_data_size =
std::max(encoded_data_left.size(), encoded_data_right.size());
// TODO: make it also dependent on the interval, when we support intervals
// different than 10ms
uint16_t packet_size;
if (codec_in_use == CODEC_G722_24KHZ) {
packet_size = 120;
} else /* if (codec_in_use == CODEC_G722_16KHZ) */ {
packet_size = 80;
}
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) {
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);
DVLOG(2) << hearingAid->address << " : " << base::HexEncode(p, packet_size);
uint16_t result = GAP_ConnWriteData(hearingAid->gap_handle, audio_packet);
if (result != BT_PASS) {
LOG(ERROR) << " Error sending data: " << loghex(result);
}
}
void GapCallback(uint16_t gap_handle, uint16_t event, tGAP_CB_DATA* data) {
HearingDevice* hearingDevice = hearingDevices.FindByGapHandle(gap_handle);
if (!hearingDevice) {
DVLOG(2) << "Skipping unknown device, gap_handle=" << gap_handle;
return;
}
switch (event) {
case GAP_EVT_CONN_OPENED: {
RawAddress address = *GAP_ConnGetRemoteAddr(gap_handle);
uint16_t tx_mtu = GAP_ConnGetRemMtuSize(gap_handle);
LOG(INFO) << "GAP_EVT_CONN_OPENED " << address << ", tx_mtu=" << tx_mtu;
OnGapConnection(address);
break;
}
// TODO: handle properly!
case GAP_EVT_CONN_CLOSED:
DVLOG(2) << "GAP_EVT_CONN_CLOSED";
hearingDevice->accepting_audio = false;
hearingDevice->gap_handle = 0;
break;
case GAP_EVT_CONN_DATA_AVAIL: {
DVLOG(2) << "GAP_EVT_CONN_DATA_AVAIL";
// 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(WARNING) << " Wrong data length";
return;
}
uint8_t* p = buffer.data();
DVLOG(1) << "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);
DVLOG(1) << "event_counter=" << event_counter
<< " frame_index: " << frame_index;
}
break;
}
case GAP_EVT_TX_EMPTY:
DVLOG(2) << "GAP_EVT_TX_EMPTY";
break;
case GAP_EVT_CONN_CONGESTED:
DVLOG(2) << "GAP_EVT_CONN_CONGESTED";
// 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:
DVLOG(2) << "GAP_EVT_CONN_UNCONGESTED";
break;
case GAP_EVT_LE_COC_CREDITS: {
auto& tmp = data->coc_credits;
DVLOG(2) << "GAP_EVT_LE_COC_CREDITS, for device: "
<< hearingDevice->address << " added" << tmp.credits_received
<< " credit_count: " << tmp.credit_count;
break;
}
}
}
static void GapCallbackStatic(uint16_t gap_handle, uint16_t event,
tGAP_CB_DATA* data) {
if (instance) instance->GapCallback(gap_handle, event, data);
}
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
<< " Packet counts (enqueued/flushed) : "
<< device.audio_stats.packet_send_count << " / "
<< device.audio_stats.packet_flush_count
<< "\n Frame counts (enqueued/flushed) : "
<< device.audio_stats.frame_send_count << " / "
<< device.audio_stats.frame_flush_count << std::endl;
}
dprintf(fd, "%s", stream.str().c_str());
}
void Disconnect(const RawAddress& address) override {
DVLOG(2) << __func__;
HearingDevice* hearingDevice = hearingDevices.FindByAddress(address);
if (!hearingDevice) {
LOG(INFO) << "Device not connected to profile" << address;
return;
}
VLOG(2) << __func__ << ": " << address;
bool connected = hearingDevice->accepting_audio;
hearingDevice->accepting_audio = false;
if (hearingDevice->connecting_actively) {
// cancel pending direct connect
BTA_GATTC_CancelOpen(gatt_if, address, true);
}
if (hearingDevice->conn_id) {
BTA_GATTC_Close(hearingDevice->conn_id);
}
if (hearingDevice->gap_handle) {
GAP_ConnClose(hearingDevice->gap_handle);
hearingDevice->gap_handle = 0;
}
// cancel autoconnect
BTA_GATTC_CancelOpen(gatt_if, address, false);
hearingDevices.Remove(address);
if (connected)
callbacks->OnConnectionState(ConnectionState::DISCONNECTED, address);
}
void OnGattDisconnected(tGATT_STATUS status, uint16_t conn_id,
tGATT_IF client_if, RawAddress remote_bda,
tBTA_GATT_REASON reason) {
HearingDevice* hearingDevice = hearingDevices.FindByConnId(conn_id);
if (!hearingDevice) {
VLOG(2) << "Skipping unknown device disconnect, conn_id=" << conn_id;
return;
}
hearingDevice->accepting_audio = false;
hearingDevice->conn_id = 0;
BtaGattQueue::Clean(conn_id);
callbacks->OnConnectionState(ConnectionState::DISCONNECTED, remote_bda);
}
void SetVolume(int8_t volume) override {
VLOG(2) << __func__ << ": " << +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) {
if (!device.gap_handle) continue;
GAP_ConnClose(device.gap_handle);
device.gap_handle = 0;
}
hearingDevices.devices.clear();
HearingAidAudioSource::Stop();
}
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;
CodecConfiguration codec;
HearingDevices hearingDevices;
};
void hearingaid_gattc_callback(tBTA_GATTC_EVT event, tBTA_GATTC* p_data) {
VLOG(2) << __func__ << " event = " << +event;
if (p_data == nullptr) return;
switch (event) {
case BTA_GATTC_DEREG_EVT:
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.status, c.conn_id, c.client_if,
c.remote_bda, c.reason);
} 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:
break;
case BTA_GATTC_ENC_CMPL_CB_EVT:
if (!instance) return;
instance->OnEncryptionComplete(p_data->enc_cmpl.remote_bda, true);
break;
case BTA_GATTC_CONN_UPDATE_EVT:
if (!instance) return;
instance->OnConnectionUpdateComplete(p_data->conn_update.conn_id);
break;
default:
break;
}
}
void encryption_callback(const RawAddress* address, tGATT_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->OnAudioDataReady(data);
}
void OnAudioSuspend() override {
if (instance) instance->OnAudioSuspend();
}
void OnAudioResume() override {
if (instance) instance->OnAudioResume();
}
};
HearingAidAudioReceiverImpl audioReceiverImpl;
} // namespace
void HearingAid::Initialize(
bluetooth::hearing_aid::HearingAidCallbacks* callbacks, Closure initCb) {
if (instance) {
LOG(ERROR) << "Already initialized!";
}
audioReceiver = &audioReceiverImpl;
instance = new HearingAidImpl(callbacks, initCb);
HearingAidAudioSource::Initialize();
}
bool HearingAid::IsInitialized() { return instance; }
HearingAid* HearingAid::Get() {
CHECK(instance);
return instance;
};
void HearingAid::AddFromStorage(const RawAddress& address, uint16_t psm,
uint8_t capabilities, uint16_t codecs,
uint16_t audio_control_point_handle,
uint16_t volume_handle, uint64_t hiSyncId,
uint16_t render_delay,
uint16_t preparation_delay,
uint16_t is_white_listed) {
if (!instance) {
LOG(ERROR) << "Not initialized yet";
}
instance->AddFromStorage(address, psm, capabilities, codecs,
audio_control_point_handle, volume_handle, hiSyncId,
render_delay, preparation_delay, is_white_listed);
};
int HearingAid::GetDeviceCount() {
if (!instance) {
LOG(INFO) << __func__ << ": Not initialized yet";
return 0;
}
return (instance->GetDeviceCount());
}
void HearingAid::CleanUp() {
// Must stop audio source to make sure it doesn't call any of callbacks on our
// soon to be null instance
HearingAidAudioSource::Stop();
HearingAidAudioSource::CleanUp();
instance->CleanUp();
HearingAidImpl* ptr = instance;
instance = nullptr;
delete ptr;
};
void HearingAid::DebugDump(int fd) {
dprintf(fd, "\nHearing Aid Manager:\n");
if (instance) instance->Dump(fd);
HearingAidAudioSource::DebugDump(fd);
}