struct bt_keys *bt_keys_get_addr(const bt_addr_le_t *addr)
{
struct bt_keys *keys;
int i;
BT_DBG("%s", bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
keys = &key_pool[i];
if (!bt_addr_le_cmp(&keys->addr, addr)) {
return keys;
}
if (!bt_addr_le_cmp(&keys->addr, BT_ADDR_LE_ANY)) {
bt_addr_le_copy(&keys->addr, addr);
BT_DBG("created %p for %s", keys, bt_addr_le_str(addr));
return keys;
}
}
BT_DBG("unable to create keys for %s", bt_addr_le_str(addr));
return NULL;
}
static int cmd_init(int argc, char *argv[])
{
static const struct bt_storage storage = {
.read = storage_read,
.write = storage_write,
.clear = storage_clear,
};
int err;
if (argc > 1) {
if (argc < 3) {
printk("Invalid address\n");
return -EINVAL;
}
err = str2bt_addr_le(argv[1], argv[2], &id_addr);
if (err) {
printk("Invalid address (err %d)\n", err);
bt_addr_le_cmp(&id_addr, BT_ADDR_LE_ANY);
return -EINVAL;
}
bt_storage_register(&storage);
}
err = bt_enable(bt_ready);
if (err) {
printk("Bluetooth init failed (err %d)\n", err);
}
return 0;
}
static uint8_t connected_cb(const struct bt_gatt_attr *attr, void *user_data)
{
struct bt_conn *conn = user_data;
struct _bt_gatt_ccc *ccc;
size_t i;
/* Check attribute user_data must be of type struct _bt_gatt_ccc */
if (attr->write != bt_gatt_attr_write_ccc) {
return BT_GATT_ITER_CONTINUE;
}
ccc = attr->user_data;
/* If already enabled skip */
if (ccc->value) {
return BT_GATT_ITER_CONTINUE;
}
for (i = 0; i < ccc->cfg_len; i++) {
/* Ignore configuration for different peer */
if (bt_addr_le_cmp(&conn->le.dst, &ccc->cfg[i].peer)) {
continue;
}
if (ccc->cfg[i].value) {
gatt_ccc_changed(ccc);
return BT_GATT_ITER_CONTINUE;
}
}
return BT_GATT_ITER_CONTINUE;
}
int bt_gatt_attr_read_ccc(struct bt_conn *conn,
const struct bt_gatt_attr *attr, void *buf,
uint16_t len, uint16_t offset)
{
struct _bt_gatt_ccc *ccc = attr->user_data;
uint16_t value;
size_t i;
for (i = 0; i < ccc->cfg_len; i++) {
if (bt_addr_le_cmp(&ccc->cfg[i].peer, &conn->le.dst)) {
continue;
}
value = sys_cpu_to_le16(ccc->cfg[i].value);
break;
}
/* Default to disable if there is no cfg for the peer */
if (i == ccc->cfg_len) {
value = 0x0000;
}
return bt_gatt_attr_read(conn, attr, buf, len, offset, &value,
sizeof(value));
}
struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer,
const bt_conn_state_t state)
{
int i;
for (i = 0; i < ARRAY_SIZE(conns); i++) {
if (!atomic_get(&conns[i].ref)) {
continue;
}
if (conns[i].type != BT_CONN_TYPE_LE) {
continue;
}
if (peer && bt_addr_le_cmp(peer, &conns[i].le.dst)) {
continue;
}
if (conns[i].state == state) {
return bt_conn_ref(&conns[i]);
}
}
return NULL;
}
int bt_gatt_attr_write_ccc(struct bt_conn *conn,
const struct bt_gatt_attr *attr, const void *buf,
uint16_t len, uint16_t offset)
{
struct _bt_gatt_ccc *ccc = attr->user_data;
const uint16_t *data = buf;
bool bonded;
size_t i;
if (offset > sizeof(*data)) {
return -EINVAL;
}
if (offset + len > sizeof(*data)) {
return -EFBIG;
}
if (bt_keys_find_addr(&conn->le.dst))
bonded = true;
else
bonded = false;
for (i = 0; i < ccc->cfg_len; i++) {
/* Check for existing configuration */
if (!bt_addr_le_cmp(&ccc->cfg[i].peer, &conn->le.dst)) {
break;
}
}
if (i == ccc->cfg_len) {
for (i = 0; i < ccc->cfg_len; i++) {
/* Check for unused configuration */
if (!ccc->cfg[i].valid) {
bt_addr_le_copy(&ccc->cfg[i].peer, &conn->le.dst);
/* Only set valid if bonded */
ccc->cfg[i].valid = bonded;
break;
}
}
if (i == ccc->cfg_len) {
BT_WARN("No space to store CCC cfg");
return -ENOMEM;
}
}
ccc->cfg[i].value = sys_le16_to_cpu(*data);
BT_DBG("handle 0x%04x value %u", attr->handle, ccc->cfg[i].value);
/* Update cfg if don't match */
if (ccc->cfg[i].value != ccc->value) {
gatt_ccc_changed(ccc);
}
return len;
}
struct bt_keys *bt_keys_find_addr(const bt_addr_le_t *addr)
{
int i;
BT_DBG("%s", bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if (!bt_addr_le_cmp(&key_pool[i].addr, addr)) {
return &key_pool[i];
}
}
return NULL;
}
int bt_gatt_unsubscribe(struct bt_conn *conn,
struct bt_gatt_subscribe_params *params)
{
struct bt_gatt_subscribe_params *tmp;
bool has_subscription = false, found = false;
if (!conn || conn->state != BT_CONN_CONNECTED) {
return -ENOTCONN;
}
if (!params) {
return -EINVAL;
}
/* Check head */
if (subscriptions == params) {
subscriptions = params->_next;
found = true;
}
/* Lookup existing subscriptions */
for (tmp = subscriptions; tmp; tmp = tmp->_next) {
/* Remove subscription */
if (tmp->_next == params) {
tmp->_next = params->_next;
found = true;
}
/* Check if there still remains any other subscription */
if (!bt_addr_le_cmp(&tmp->_peer, &conn->le.dst) &&
tmp->value_handle == params->value_handle) {
has_subscription = true;
}
}
if (!found) {
return -EINVAL;
}
if (params->destroy) {
params->destroy(params);
}
if (has_subscription) {
return 0;
}
return gatt_write_ccc(conn, params->ccc_handle, 0x0000, NULL, NULL);
}
struct bt_keys *bt_keys_find(int type, const bt_addr_le_t *addr)
{
int i;
BT_DBG("type %d %s", type, bt_addr_le_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
if ((key_pool[i].keys & type) &&
!bt_addr_le_cmp(&key_pool[i].addr, addr)) {
return &key_pool[i];
}
}
return NULL;
}
static void remove_subscribtions(struct bt_conn *conn)
{
struct bt_gatt_subscribe_params *params, *prev;
/* Lookup existing subscriptions */
for (params = subscriptions, prev = NULL; params;
prev = params, params = params->_next) {
if (bt_addr_le_cmp(¶ms->_peer, &conn->le.dst)) {
continue;
}
/* Remove subscription */
gatt_subscription_remove(prev, params);
}
}
static ssize_t storage_read(const bt_addr_le_t *addr, uint16_t key, void *data,
size_t length)
{
if (addr) {
return -ENOENT;
}
if (key == BT_STORAGE_ID_ADDR && length == sizeof(id_addr) &&
bt_addr_le_cmp(&id_addr, BT_ADDR_LE_ANY)) {
bt_addr_le_copy(data, &id_addr);
return sizeof(id_addr);
}
return -EIO;
}
struct bt_conn *bt_conn_lookup_addr_le(const bt_addr_le_t *peer)
{
int i;
for (i = 0; i < ARRAY_SIZE(conns); i++) {
if (!atomic_get(&conns[i].ref)) {
continue;
}
if (!bt_addr_le_cmp(peer, &conns[i].dst)) {
return bt_conn_ref(&conns[i]);
}
}
return NULL;
}
static uint8_t disconnected_cb(const struct bt_gatt_attr *attr, void *user_data)
{
struct bt_conn *conn = user_data;
struct _bt_gatt_ccc *ccc;
size_t i;
/* Check attribute user_data must be of type struct _bt_gatt_ccc */
if (attr->write != bt_gatt_attr_write_ccc) {
return BT_GATT_ITER_CONTINUE;
}
ccc = attr->user_data;
/* If already disabled skip */
if (!ccc->value) {
return BT_GATT_ITER_CONTINUE;
}
for (i = 0; i < ccc->cfg_len; i++) {
/* Ignore configurations with disabled value */
if (!ccc->cfg[i].value) {
continue;
}
if (bt_addr_le_cmp(&conn->le.dst, &ccc->cfg[i].peer)) {
struct bt_conn *tmp;
/* Skip if there is another peer connected */
tmp = bt_conn_lookup_addr_le(&ccc->cfg[i].peer);
if (tmp && tmp->state == BT_CONN_CONNECTED) {
bt_conn_unref(tmp);
return BT_GATT_ITER_CONTINUE;
}
}
}
/* Reset value while disconnected */
memset(&ccc->value, 0, sizeof(ccc->value));
ccc->cfg_changed(ccc->value);
BT_DBG("ccc %p reseted", ccc);
return BT_GATT_ITER_CONTINUE;
}
static void on_disconnected(struct bt_conn *conn, uint8_t reason)
{
int i;
const bt_addr_le_t *dst;
dst = bt_conn_get_dst(conn);
if (!dst) {
return;
}
/* This is a specific behaviour: we are discarding CCCD value for the
* measurement characteristic -> forces the peer to resubscribe
*/
for (i = 0; i < ARRAY_SIZE(rsc_measurement_ccc_cfg); i++) {
if (!bt_addr_le_cmp(dst, &rsc_measurement_ccc_cfg[i].peer)) {
rsc_measurement_ccc_cfg[i].value = 0;
}
}
}
int bt_gatt_subscribe(struct bt_conn *conn,
struct bt_gatt_subscribe_params *params)
{
struct bt_gatt_subscribe_params *tmp;
bool has_subscription = false;
if (!conn || conn->state != BT_CONN_CONNECTED) {
return -ENOTCONN;
}
if (!params || !params->func || !params->value || !params->ccc_handle) {
return -EINVAL;
}
/* Lookup existing subscriptions */
for (tmp = subscriptions; tmp; tmp = tmp->_next) {
/* Fail if entry already exists */
if (tmp == params) {
return -EALREADY;
}
/* Check if another subscription exists */
if (!bt_addr_le_cmp(&tmp->_peer, &conn->le.dst) &&
tmp->value_handle == params->value_handle &&
tmp->value >= params->value) {
has_subscription = true;
}
}
/* Skip write if already subcribed */
if (has_subscription) {
gatt_subscription_add(conn, params);
return 0;
}
return gatt_write_ccc(conn, params->ccc_handle, params->value,
att_write_ccc_rsp, params);
}
static struct bt_keys *bt_keys_get_addr_br(const bt_addr_t *addr)
{
struct bt_keys *keys;
int i;
BT_DBG("%s", bt_addr_str(addr));
for (i = 0; i < ARRAY_SIZE(key_pool); i++) {
keys = &key_pool[i];
/*
* When both LE and BR/EDR keys are for the same device,
* the bt_addr_le_t is the public address, i.e. the same
* as the BR/EDR address.
*/
if (keys->addr.type == BT_ADDR_LE_PUBLIC &&
!bt_addr_cmp((const bt_addr_t *)keys->addr.val, addr)) {
return keys;
}
/*
* BT_ADDR_LE_ANY has the same type of as BT_ADDR_LE_PUBLIC
* value. No need to make redudant comparision against
* BT_ADDR_LE_PUBLIC.
*/
if (!bt_addr_le_cmp(&keys->addr, BT_ADDR_LE_ANY)) {
bt_addr_copy((bt_addr_t *)keys->addr.val, addr);
BT_DBG("created %p for %s", keys, bt_addr_str(addr));
return keys;
}
}
BT_DBG("unable to create keys for %s", bt_addr_str(addr));
return NULL;
}