| /* gatt.c - Bluetooth GATT Server Tester */ |
| |
| /* |
| * Copyright (c) 2015 Intel Corporation |
| * |
| * 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 <stdint.h> |
| #include <string.h> |
| #include <errno.h> |
| |
| #include <toolchain.h> |
| #include <bluetooth/bluetooth.h> |
| #include <bluetooth/conn.h> |
| #include <bluetooth/gatt.h> |
| #include <bluetooth/uuid.h> |
| #include <misc/byteorder.h> |
| #include <misc/printk.h> |
| |
| #include "bttester.h" |
| |
| #define CONTROLLER_INDEX 0 |
| #define MAX_ATTRIBUTES 50 |
| #define MAX_BUFFER_SIZE 2048 |
| |
| #define GATT_PERM_ENC_READ_MASK (BT_GATT_PERM_READ_ENCRYPT | \ |
| BT_GATT_PERM_READ_AUTHEN) |
| #define GATT_PERM_ENC_WRITE_MASK (BT_GATT_PERM_WRITE_ENCRYPT | \ |
| BT_GATT_PERM_WRITE_AUTHEN) |
| |
| static struct bt_gatt_attr gatt_db[MAX_ATTRIBUTES]; |
| |
| /* |
| * gatt_buf - cache used by a gatt client (to cache data read/discovered) |
| * and gatt server (to store attribute user_data). |
| * It is not intended to be used by client and server at the same time. |
| */ |
| static struct { |
| uint16_t len; |
| uint8_t buf[MAX_BUFFER_SIZE]; |
| } gatt_buf; |
| |
| static void *gatt_buf_reserve(size_t len) |
| { |
| void *ptr; |
| |
| if ((len + gatt_buf.len) > ARRAY_SIZE(gatt_buf.buf)) { |
| return NULL; |
| } |
| |
| ptr = memset(gatt_buf.buf + gatt_buf.len, 0, len); |
| gatt_buf.len += len; |
| |
| BTTESTER_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); |
| |
| return ptr; |
| } |
| |
| static void *gatt_buf_add(const void *data, size_t len) |
| { |
| void *ptr; |
| |
| if ((len + gatt_buf.len) > ARRAY_SIZE(gatt_buf.buf)) { |
| return NULL; |
| } |
| |
| ptr = memcpy(gatt_buf.buf + gatt_buf.len, data, len); |
| gatt_buf.len += len; |
| |
| BTTESTER_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); |
| |
| return ptr; |
| } |
| |
| static void gatt_buf_clear(void) |
| { |
| memset(&gatt_buf, 0, sizeof(gatt_buf)); |
| } |
| |
| static bool gatt_buf_isempty(void) |
| { |
| if (gatt_buf.len) { |
| return false; |
| } |
| |
| return true; |
| } |
| |
| static struct bt_gatt_attr *gatt_db_add(const struct bt_gatt_attr *pattern) |
| { |
| static struct bt_gatt_attr *attr = gatt_db; |
| |
| /* Return NULL if gatt_db is full */ |
| if (attr == &gatt_db[ARRAY_SIZE(gatt_db)]) { |
| return NULL; |
| } |
| |
| memcpy(attr, pattern, sizeof(*attr)); |
| |
| /* Register attribute in GATT database, this will assign it a handle */ |
| if (bt_gatt_register(attr, 1)) { |
| return NULL; |
| } |
| |
| BTTESTER_DBG("handle 0x%04x", attr->handle); |
| |
| return attr++; |
| } |
| |
| /* Convert UUID from BTP command to bt_uuid */ |
| static uint8_t btp2bt_uuid(const uint8_t *uuid, uint8_t len, |
| struct bt_uuid *bt_uuid) |
| { |
| uint16_t le16; |
| |
| switch (len) { |
| case 0x02: /* UUID 16 */ |
| bt_uuid->type = BT_UUID_TYPE_16; |
| memcpy(&le16, uuid, sizeof(le16)); |
| BT_UUID_16(bt_uuid)->val = sys_le16_to_cpu(le16); |
| break; |
| case 0x10: /* UUID 128*/ |
| bt_uuid->type = BT_UUID_TYPE_128; |
| memcpy(BT_UUID_128(bt_uuid)->val, uuid, 16); |
| break; |
| default: |
| return BTP_STATUS_FAILED; |
| } |
| |
| return BTP_STATUS_SUCCESS; |
| } |
| |
| static void supported_commands(uint8_t *data, uint16_t len) |
| { |
| uint64_t cmds[2]; |
| struct gatt_read_supported_commands_rp *rp = (void *) cmds; |
| |
| cmds[0] = 1 << GATT_READ_SUPPORTED_COMMANDS; |
| cmds[0] |= 1 << GATT_ADD_SERVICE; |
| cmds[0] |= 1 << GATT_ADD_CHARACTERISTIC; |
| cmds[0] |= 1 << GATT_ADD_DESCRIPTOR; |
| cmds[0] |= 1 << GATT_ADD_INCLUDED_SERVICE; |
| cmds[0] |= 1 << GATT_SET_VALUE; |
| cmds[0] |= 1 << GATT_START_SERVER; |
| cmds[0] |= 1 << GATT_SET_ENC_KEY_SIZE; |
| cmds[1] = 1 << (GATT_EXCHANGE_MTU - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_DISC_PRIM_UUID - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_FIND_INCLUDED - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_DISC_ALL_CHRC - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_DISC_CHRC_UUID - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_DISC_ALL_DESC - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_READ - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_READ_LONG - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_READ_MULTIPLE - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_WRITE_WITHOUT_RSP - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_SIGNED_WRITE_WITHOUT_RSP - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_WRITE - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_WRITE_LONG - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_CFG_NOTIFY - GATT_CLIENT_OP_OFFSET); |
| cmds[1] |= 1 << (GATT_CFG_INDICATE - GATT_CLIENT_OP_OFFSET); |
| |
| tester_send(BTP_SERVICE_ID_GATT, GATT_READ_SUPPORTED_COMMANDS, |
| CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); |
| } |
| |
| static struct bt_gatt_attr svc_pri = BT_GATT_PRIMARY_SERVICE(NULL); |
| static struct bt_gatt_attr svc_sec = BT_GATT_SECONDARY_SERVICE(NULL); |
| |
| union uuid { |
| struct bt_uuid uuid; |
| struct bt_uuid_16 u16; |
| struct bt_uuid_128 u128; |
| }; |
| |
| static void add_service(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_add_service_cmd *cmd = (void *) data; |
| struct gatt_add_service_rp rp; |
| struct bt_gatt_attr *attr_svc; |
| union uuid uuid; |
| |
| if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid.uuid)) { |
| goto fail; |
| } |
| |
| switch (cmd->type) { |
| case GATT_SERVICE_PRIMARY: |
| attr_svc = gatt_db_add(&svc_pri); |
| break; |
| case GATT_SERVICE_SECONDARY: |
| attr_svc = gatt_db_add(&svc_sec); |
| break; |
| default: |
| goto fail; |
| } |
| |
| if (!attr_svc) { |
| goto fail; |
| } |
| |
| attr_svc->user_data = gatt_buf_add(&uuid, sizeof(uuid)); |
| if (!attr_svc->user_data) { |
| goto fail; |
| } |
| |
| rp.svc_id = sys_cpu_to_le16(attr_svc->handle); |
| |
| tester_send(BTP_SERVICE_ID_GATT, GATT_ADD_SERVICE, CONTROLLER_INDEX, |
| (uint8_t *) &rp, sizeof(rp)); |
| |
| return; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_ADD_SERVICE, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| struct gatt_value { |
| uint16_t len; |
| uint8_t *data; |
| uint8_t *prep_data; |
| uint8_t enc_key_size; |
| bool has_ccc; |
| }; |
| |
| static int read_value(struct bt_conn *conn, const struct bt_gatt_attr *attr, |
| void *buf, uint16_t len, uint16_t offset) |
| { |
| const struct gatt_value *value = attr->user_data; |
| |
| if ((attr->perm & GATT_PERM_ENC_READ_MASK) && |
| (value->enc_key_size > bt_conn_enc_key_size(conn))) { |
| return -EACCES; |
| } |
| |
| return bt_gatt_attr_read(conn, attr, buf, len, offset, value->data, |
| value->len); |
| } |
| |
| static int write_value(struct bt_conn *conn, const struct bt_gatt_attr *attr, |
| const void *buf, uint16_t len, uint16_t offset) |
| { |
| struct gatt_value *value = attr->user_data; |
| |
| if ((attr->perm & GATT_PERM_ENC_WRITE_MASK) && |
| (value->enc_key_size > bt_conn_enc_key_size(conn))) { |
| return -EACCES; |
| } |
| |
| /* |
| * If the prepare Value Offset is greater than the current length of |
| * the attribute value Error Response shall be sent with the |
| * «Invalid Offset». |
| */ |
| if (offset > value->len) { |
| return -EINVAL; |
| } |
| |
| if (offset + len > value->len) { |
| return -EFBIG; |
| } |
| |
| memcpy(value->prep_data + offset, buf, len); |
| |
| return len; |
| } |
| |
| static int flush_value(struct bt_conn *conn, |
| const struct bt_gatt_attr *attr, uint8_t flags) |
| { |
| struct gatt_value *value = attr->user_data; |
| |
| switch (flags) { |
| case BT_GATT_FLUSH_SYNC: |
| /* Sync buffer to data */ |
| memcpy(value->data, value->prep_data, value->len); |
| /* Fallthrough */ |
| case BT_GATT_FLUSH_DISCARD: |
| memset(value->prep_data, 0, value->len); |
| return 0; |
| } |
| |
| return -EINVAL; |
| } |
| |
| static struct bt_gatt_attr chr = BT_GATT_CHARACTERISTIC(NULL, 0); |
| static struct bt_gatt_attr chr_val = BT_GATT_LONG_DESCRIPTOR(NULL, 0, |
| read_value, |
| write_value, |
| flush_value, NULL); |
| |
| static uint8_t add_characteristic_cb(const struct bt_gatt_attr *attr, |
| void *user_data) |
| { |
| const struct gatt_add_characteristic_cmd *cmd = user_data; |
| struct gatt_add_characteristic_rp rp; |
| struct bt_gatt_attr *attr_chrc, *attr_value; |
| struct bt_gatt_chrc chrc; |
| union uuid uuid; |
| |
| if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid.uuid)) { |
| goto fail; |
| } |
| |
| attr_chrc = gatt_db_add(&chr); |
| if (!attr_chrc) { |
| goto fail; |
| } |
| |
| attr_value = gatt_db_add(&chr_val); |
| if (!attr_value) { |
| goto fail; |
| } |
| |
| chrc.properties = cmd->properties; |
| chrc.uuid = gatt_buf_add(&uuid, sizeof(uuid)); |
| if (!chrc.uuid) { |
| goto fail; |
| } |
| |
| attr_chrc->user_data = gatt_buf_add(&chrc, sizeof(chrc)); |
| if (!attr_chrc->user_data) { |
| goto fail; |
| } |
| |
| attr_value->uuid = chrc.uuid; |
| attr_value->perm = cmd->permissions; |
| |
| rp.char_id = sys_cpu_to_le16(attr_chrc->handle); |
| tester_send(BTP_SERVICE_ID_GATT, GATT_ADD_CHARACTERISTIC, |
| CONTROLLER_INDEX, (uint8_t *) &rp, sizeof(rp)); |
| |
| return BT_GATT_ITER_STOP; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_ADD_CHARACTERISTIC, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| static void add_characteristic(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_add_characteristic_cmd *cmd = (void *) data; |
| uint16_t handle = sys_le16_to_cpu(cmd->svc_id); |
| |
| /* TODO Return error if no attribute found */ |
| bt_gatt_foreach_attr(handle, handle, add_characteristic_cb, data); |
| } |
| |
| static bool ccc_added; |
| |
| static struct bt_gatt_ccc_cfg ccc_cfg[CONFIG_BLUETOOTH_MAX_PAIRED] = {}; |
| |
| static void ccc_cfg_changed(uint16_t value) |
| { |
| /* NOP */ |
| } |
| |
| static struct bt_gatt_attr ccc = BT_GATT_CCC(ccc_cfg, ccc_cfg_changed); |
| |
| static struct bt_gatt_attr *add_ccc(const struct bt_gatt_attr *attr_chrc) |
| { |
| struct bt_gatt_attr *attr_desc, *attr_value; |
| struct bt_gatt_chrc *chrc = attr_chrc->user_data; |
| struct gatt_value *value; |
| |
| /* Fail if another CCC already exist on server */ |
| if (ccc_added) { |
| return NULL; |
| } |
| |
| /* Check characteristic properties */ |
| if (!(chrc->properties & |
| (BT_GATT_CHRC_NOTIFY | BT_GATT_CHRC_INDICATE))) { |
| return NULL; |
| } |
| |
| /* |
| * Look for characteristic value (stored under next handle) to set |
| * 'has_ccc' flag |
| */ |
| attr_value = bt_gatt_attr_next(attr_chrc); |
| if (!attr_value) { |
| return NULL; |
| } |
| |
| value = attr_value->user_data; |
| if (!value) { |
| return NULL; |
| } |
| |
| /* Add CCC descriptor to GATT database */ |
| attr_desc = gatt_db_add(&ccc); |
| if (!attr_desc) { |
| return NULL; |
| } |
| |
| value->has_ccc = true; |
| ccc_added = true; |
| |
| return attr_desc; |
| } |
| |
| static struct bt_gatt_attr cep = BT_GATT_CEP(NULL); |
| |
| static struct bt_gatt_attr *add_cep(const struct bt_gatt_attr *attr_chrc) |
| { |
| struct bt_gatt_attr *attr_desc; |
| struct bt_gatt_chrc *chrc = attr_chrc->user_data; |
| struct bt_gatt_cep cep_value; |
| |
| /* Extended Properties bit shall be set */ |
| if (!(chrc->properties & BT_GATT_CHRC_EXT_PROP)) { |
| return NULL; |
| } |
| |
| /* Add CEP descriptor to GATT database */ |
| attr_desc = gatt_db_add(&cep); |
| if (!attr_desc) { |
| return NULL; |
| } |
| |
| attr_desc->user_data = gatt_buf_add(&cep_value, sizeof(cep_value)); |
| if (!attr_desc->user_data) { |
| return NULL; |
| } |
| |
| return attr_desc; |
| } |
| |
| static struct bt_gatt_attr *dsc = &chr_val; |
| |
| static uint8_t add_descriptor_cb(const struct bt_gatt_attr *attr, |
| void *user_data) |
| { |
| const struct gatt_add_descriptor_cmd *cmd = user_data; |
| struct gatt_add_descriptor_rp rp; |
| struct bt_gatt_attr *attr_desc; |
| union uuid uuid; |
| |
| if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid.uuid)) { |
| goto fail; |
| } |
| |
| if (!bt_uuid_cmp(&uuid.uuid, cep.uuid)) { |
| attr_desc = add_cep(attr); |
| } else if (!bt_uuid_cmp(&uuid.uuid, ccc.uuid)) { |
| attr_desc = add_ccc(attr); |
| } else { |
| attr_desc = gatt_db_add(dsc); |
| } |
| |
| if (!attr_desc) { |
| goto fail; |
| } |
| |
| /* CCC and CEP have permissions already set */ |
| if (!attr_desc->perm) { |
| attr_desc->perm = cmd->permissions; |
| } |
| |
| /* CCC and CEP have UUID already set */ |
| if (!attr_desc->uuid) { |
| attr_desc->uuid = gatt_buf_add(&uuid, sizeof(uuid)); |
| if (!attr_desc->uuid) { |
| goto fail; |
| } |
| } |
| |
| rp.desc_id = sys_cpu_to_le16(attr_desc->handle); |
| |
| tester_send(BTP_SERVICE_ID_GATT, GATT_ADD_DESCRIPTOR, CONTROLLER_INDEX, |
| (uint8_t *) &rp, sizeof(rp)); |
| |
| return BT_GATT_ITER_STOP; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_ADD_DESCRIPTOR, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| static void add_descriptor(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_add_descriptor_cmd *cmd = (void *) data; |
| uint16_t handle = sys_le16_to_cpu(cmd->char_id); |
| |
| /* TODO Return error if no attribute found */ |
| bt_gatt_foreach_attr(handle, handle, add_descriptor_cb, data); |
| } |
| |
| static uint8_t get_service_handles(const struct bt_gatt_attr *attr, |
| void *user_data) |
| { |
| struct bt_gatt_include *include = user_data; |
| |
| /* |
| * The first attribute found is service declaration. |
| * Preset end handle - next attribute can be a service. |
| */ |
| if (!include->start_handle) { |
| include->start_handle = attr->handle; |
| include->end_handle = attr->handle; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| /* Stop if attribute is a service */ |
| if (!bt_uuid_cmp(attr->uuid, svc_pri.uuid) || |
| !bt_uuid_cmp(attr->uuid, svc_sec.uuid)) { |
| return BT_GATT_ITER_STOP; |
| } |
| |
| include->end_handle = attr->handle; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static struct bt_gatt_attr svc_inc = BT_GATT_INCLUDE_SERVICE(NULL); |
| |
| static uint8_t add_included_cb(const struct bt_gatt_attr *attr, void *user_data) |
| { |
| struct gatt_add_included_service_rp rp; |
| struct bt_gatt_attr *attr_incl; |
| struct bt_gatt_include include; |
| |
| /* Fail if attribute stored under requested handle is not a service */ |
| if (bt_uuid_cmp(attr->uuid, svc_pri.uuid) && |
| bt_uuid_cmp(attr->uuid, svc_sec.uuid)) { |
| goto fail; |
| } |
| |
| attr_incl = gatt_db_add(&svc_inc); |
| if (!attr_incl) { |
| goto fail; |
| } |
| |
| include.uuid = attr->user_data; |
| include.start_handle = 0; |
| |
| attr_incl->user_data = gatt_buf_add(&include, sizeof(include)); |
| if (!attr_incl->user_data) { |
| goto fail; |
| } |
| |
| /* Lookup for service end handle */ |
| bt_gatt_foreach_attr(attr->handle, 0xffff, get_service_handles, |
| attr_incl->user_data); |
| |
| rp.included_service_id = sys_cpu_to_le16(attr_incl->handle); |
| |
| tester_send(BTP_SERVICE_ID_GATT, GATT_ADD_CHARACTERISTIC, |
| CONTROLLER_INDEX, (uint8_t *) &rp, sizeof(rp)); |
| |
| return BT_GATT_ITER_STOP; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_ADD_CHARACTERISTIC, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| static void add_included(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_add_included_service_cmd *cmd = (void *) data; |
| uint16_t handle = sys_le16_to_cpu(cmd->svc_id); |
| |
| /* TODO Return error if no attribute found */ |
| bt_gatt_foreach_attr(handle, handle, add_included_cb, data); |
| } |
| |
| static uint8_t set_ccc_value(struct bt_gatt_attr *attr, const void *value, |
| const uint16_t len) |
| { |
| uint16_t ccc_val; |
| |
| if (len != sizeof(ccc_val)) { |
| return BTP_STATUS_FAILED; |
| } |
| |
| memcpy(&ccc_val, value, sizeof(ccc_val)); |
| |
| /* |
| * CCC Data has been already set, so we can only verify if the |
| * requested data is correct |
| */ |
| if (sys_le16_to_cpu(ccc_val) != 0) { |
| return BTP_STATUS_FAILED; |
| } |
| |
| return BTP_STATUS_SUCCESS; |
| } |
| |
| static uint8_t set_cep_value(struct bt_gatt_attr *attr, const void *value, |
| const uint16_t len) |
| { |
| struct bt_gatt_cep *cep_value = attr->user_data; |
| |
| if (len != sizeof(cep_value->properties)) { |
| return BTP_STATUS_FAILED; |
| } |
| |
| memcpy(&cep_value->properties, value, len); |
| |
| return BTP_STATUS_SUCCESS; |
| } |
| |
| static uint8_t set_value_cb(struct bt_gatt_attr *attr, void *user_data) |
| { |
| const struct gatt_set_value_cmd *cmd = user_data; |
| struct gatt_value value; |
| uint8_t status; |
| |
| /* Handle CCC value */ |
| if (!bt_uuid_cmp(attr->uuid, ccc.uuid)) { |
| status = set_ccc_value(attr, cmd->value, |
| sys_le16_to_cpu(cmd->len)); |
| goto rsp; |
| } |
| |
| /* Set CEP value */ |
| if (!bt_uuid_cmp(attr->uuid, cep.uuid)) { |
| status = set_cep_value(attr, cmd->value, |
| sys_le16_to_cpu(cmd->len)); |
| goto rsp; |
| } |
| |
| if (!bt_uuid_cmp(attr->uuid, chr.uuid)) { |
| attr = bt_gatt_attr_next(attr); |
| if (!attr) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| } |
| |
| value.len = sys_le16_to_cpu(cmd->len); |
| |
| /* Check if attribute value has been already set */ |
| if (attr->user_data) { |
| struct gatt_value *gatt_value = attr->user_data; |
| |
| /* Fail if value length doesn't match */ |
| if (value.len != gatt_value->len) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| memcpy(gatt_value->data, cmd->value, gatt_value->len); |
| |
| if (gatt_value->has_ccc) { |
| bt_gatt_notify(NULL, attr, gatt_value->data, |
| gatt_value->len); |
| } |
| |
| status = BTP_STATUS_SUCCESS; |
| goto rsp; |
| } |
| |
| value.data = gatt_buf_add(cmd->value, value.len); |
| if (!value.data) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| value.prep_data = gatt_buf_reserve(value.len); |
| if (!value.prep_data) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| value.has_ccc = false; |
| value.enc_key_size = 0x00; |
| |
| attr->user_data = gatt_buf_add(&value, sizeof(value)); |
| if (!attr->user_data) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| status = BTP_STATUS_SUCCESS; |
| rsp: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_SET_VALUE, CONTROLLER_INDEX, |
| status); |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| static void set_value(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_set_value_cmd *cmd = (void *) data; |
| uint16_t handle = sys_le16_to_cpu(cmd->attr_id); |
| |
| /* TODO Return error if no attribute found */ |
| bt_gatt_foreach_attr(handle, handle, (bt_gatt_attr_func_t) set_value_cb, |
| data); |
| } |
| |
| static void start_server(uint8_t *data, uint16_t len) |
| { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_START_SERVER, |
| CONTROLLER_INDEX, BTP_STATUS_SUCCESS); |
| } |
| |
| static uint8_t set_enc_key_size_cb(const struct bt_gatt_attr *attr, |
| void *user_data) |
| { |
| const struct gatt_set_enc_key_size_cmd *cmd = user_data; |
| struct gatt_value *value; |
| uint8_t status; |
| |
| /* Fail if requested key size is invalid */ |
| if (cmd->key_size < 0x07 || cmd->key_size > 0x0f) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| /* Fail if requested attribute is a service */ |
| if (!bt_uuid_cmp(attr->uuid, svc_pri.uuid) || |
| !bt_uuid_cmp(attr->uuid, svc_sec.uuid) || |
| !bt_uuid_cmp(attr->uuid, svc_inc.uuid)) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| /* Lookup for characteristic value attribute */ |
| if (!bt_uuid_cmp(attr->uuid, chr.uuid)) { |
| attr = bt_gatt_attr_next(attr); |
| if (!attr) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| } |
| |
| /* Fail if permissions are not set */ |
| if (!(attr->perm & (GATT_PERM_ENC_READ_MASK | |
| GATT_PERM_ENC_WRITE_MASK))) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| /* Fail if there is no attribute value */ |
| if (!attr->user_data) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| value = attr->user_data; |
| value->enc_key_size = cmd->key_size; |
| |
| status = BTP_STATUS_SUCCESS; |
| rsp: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_SET_ENC_KEY_SIZE, CONTROLLER_INDEX, |
| status); |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| static void set_enc_key_size(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_set_enc_key_size_cmd *cmd = (void *) data; |
| uint16_t handle = sys_le16_to_cpu(cmd->attr_id); |
| |
| /* TODO Return error if no attribute found */ |
| bt_gatt_foreach_attr(handle, handle, set_enc_key_size_cb, data); |
| } |
| |
| static void exchange_mtu_rsp(struct bt_conn *conn, uint8_t err) |
| { |
| if (err) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| |
| return; |
| } |
| |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, CONTROLLER_INDEX, |
| BTP_STATUS_SUCCESS); |
| } |
| |
| static void exchange_mtu(uint8_t *data, uint16_t len) |
| { |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail; |
| } |
| |
| if (bt_gatt_exchange_mtu(conn, exchange_mtu_rsp) < 0) { |
| bt_conn_unref(conn); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| } |
| |
| static struct bt_gatt_discover_params discover_params; |
| static union uuid uuid; |
| static uint8_t btp_opcode; |
| |
| static void discover_destroy(struct bt_gatt_discover_params *params) |
| { |
| memset(params, 0, sizeof(*params)); |
| gatt_buf_clear(); |
| } |
| |
| static uint8_t disc_prim_uuid_cb(struct bt_conn *conn, |
| const struct bt_gatt_attr *attr, |
| struct bt_gatt_discover_params *params) |
| { |
| struct bt_gatt_service *data; |
| struct gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf; |
| struct gatt_service *service; |
| uint8_t uuid_length; |
| |
| if (!attr) { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, |
| CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| data = attr->user_data; |
| |
| uuid_length = data->uuid->type == BT_UUID_TYPE_16 ? 2 : 16; |
| |
| service = gatt_buf_reserve(sizeof(*service) + uuid_length); |
| if (!service) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| service->start_handle = sys_cpu_to_le16(attr->handle); |
| service->end_handle = sys_cpu_to_le16(data->end_handle); |
| service->uuid_length = uuid_length; |
| |
| if (data->uuid->type == BT_UUID_TYPE_16) { |
| uint16_t u16 = sys_cpu_to_le16(BT_UUID_16(data->uuid)->val); |
| |
| memcpy(service->uuid, &u16, uuid_length); |
| } else { |
| memcpy(service->uuid, BT_UUID_128(data->uuid)->val, |
| uuid_length); |
| } |
| |
| rp->services_count++; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static void disc_prim_uuid(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_disc_prim_uuid_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid.uuid)) { |
| goto fail; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_disc_prim_uuid_rp))) { |
| goto fail; |
| } |
| |
| discover_params.uuid = &uuid.uuid; |
| discover_params.start_handle = 0x0001; |
| discover_params.end_handle = 0xffff; |
| discover_params.type = BT_GATT_DISCOVER_PRIMARY; |
| discover_params.func = disc_prim_uuid_cb; |
| |
| if (bt_gatt_discover(conn, &discover_params) < 0) { |
| discover_destroy(&discover_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static uint8_t find_included_cb(struct bt_conn *conn, |
| const struct bt_gatt_attr *attr, |
| struct bt_gatt_discover_params *params) |
| { |
| struct bt_gatt_include *data; |
| struct gatt_find_included_rp *rp = (void *) gatt_buf.buf; |
| struct gatt_included *included; |
| uint8_t uuid_length; |
| |
| if (!attr) { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, |
| CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| data = attr->user_data; |
| |
| uuid_length = data->uuid->type == BT_UUID_TYPE_16 ? 2 : 16; |
| |
| included = gatt_buf_reserve(sizeof(*included) + uuid_length); |
| if (!included) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| included->included_handle = attr->handle; |
| included->service.start_handle = sys_cpu_to_le16(data->start_handle); |
| included->service.end_handle = sys_cpu_to_le16(data->end_handle); |
| included->service.uuid_length = uuid_length; |
| |
| if (data->uuid->type == BT_UUID_TYPE_16) { |
| uint16_t u16 = sys_cpu_to_le16(BT_UUID_16(data->uuid)->val); |
| |
| memcpy(included->service.uuid, &u16, uuid_length); |
| } else { |
| /* TODO Read this 128bit UUID */ |
| memset(included->service.uuid, 0, uuid_length); |
| } |
| |
| rp->services_count++; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static void find_included(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_find_included_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_find_included_rp))) { |
| goto fail; |
| } |
| |
| discover_params.start_handle = sys_le16_to_cpu(cmd->start_handle); |
| discover_params.end_handle = sys_le16_to_cpu(cmd->end_handle); |
| discover_params.type = BT_GATT_DISCOVER_INCLUDE; |
| discover_params.func = find_included_cb; |
| |
| if (bt_gatt_discover(conn, &discover_params) < 0) { |
| discover_destroy(&discover_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static uint8_t disc_chrc_cb(struct bt_conn *conn, |
| const struct bt_gatt_attr *attr, |
| struct bt_gatt_discover_params *params) |
| { |
| struct bt_gatt_chrc *data; |
| struct gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf; |
| struct gatt_characteristic *chrc; |
| uint8_t uuid_length; |
| |
| if (!attr) { |
| tester_send(BTP_SERVICE_ID_GATT, btp_opcode, |
| CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| data = attr->user_data; |
| |
| uuid_length = data->uuid->type == BT_UUID_TYPE_16 ? 2 : 16; |
| |
| chrc = gatt_buf_reserve(sizeof(*chrc) + uuid_length); |
| if (!chrc) { |
| tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| chrc->characteristic_handle = sys_cpu_to_le16(attr->handle); |
| chrc->properties = data->properties; |
| chrc->value_handle = sys_cpu_to_le16(attr->handle + 1); |
| chrc->uuid_length = uuid_length; |
| |
| if (data->uuid->type == BT_UUID_TYPE_16) { |
| uint16_t u16 = sys_cpu_to_le16(BT_UUID_16(data->uuid)->val); |
| |
| memcpy(chrc->uuid, &u16, uuid_length); |
| } else { |
| memcpy(chrc->uuid, BT_UUID_128(data->uuid)->val, uuid_length); |
| } |
| |
| rp->characteristics_count++; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static void disc_all_chrc(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_disc_all_chrc_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { |
| goto fail; |
| } |
| |
| discover_params.start_handle = sys_le16_to_cpu(cmd->start_handle); |
| discover_params.end_handle = sys_le16_to_cpu(cmd->end_handle); |
| discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; |
| discover_params.func = disc_chrc_cb; |
| |
| /* TODO should be handled as user_data via CONTAINER_OF macro */ |
| btp_opcode = GATT_DISC_ALL_CHRC; |
| |
| if (bt_gatt_discover(conn, &discover_params) < 0) { |
| discover_destroy(&discover_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_CHRC, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static void disc_chrc_uuid(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_disc_chrc_uuid_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid.uuid)) { |
| goto fail; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { |
| goto fail; |
| } |
| |
| discover_params.uuid = &uuid.uuid; |
| discover_params.start_handle = sys_le16_to_cpu(cmd->start_handle); |
| discover_params.end_handle = sys_le16_to_cpu(cmd->end_handle); |
| discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; |
| discover_params.func = disc_chrc_cb; |
| |
| /* TODO should be handled as user_data via CONTAINER_OF macro */ |
| btp_opcode = GATT_DISC_CHRC_UUID; |
| |
| if (bt_gatt_discover(conn, &discover_params) < 0) { |
| discover_destroy(&discover_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_CHRC_UUID, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static uint8_t disc_all_desc_cb(struct bt_conn *conn, |
| const struct bt_gatt_attr *attr, |
| struct bt_gatt_discover_params *params) |
| { |
| struct gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf; |
| struct gatt_descriptor *descriptor; |
| uint8_t uuid_length; |
| |
| if (!attr) { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, |
| CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| uuid_length = attr->uuid->type == BT_UUID_TYPE_16 ? 2 : 16; |
| |
| descriptor = gatt_buf_reserve(sizeof(*descriptor) + uuid_length); |
| if (!descriptor) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| discover_destroy(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| descriptor->descriptor_handle = sys_cpu_to_le16(attr->handle); |
| descriptor->uuid_length = uuid_length; |
| |
| if (attr->uuid->type == BT_UUID_TYPE_16) { |
| uint16_t u16 = sys_cpu_to_le16(BT_UUID_16(attr->uuid)->val); |
| |
| memcpy(descriptor->uuid, &u16, uuid_length); |
| } else { |
| memcpy(descriptor->uuid, BT_UUID_128(attr->uuid)->val, |
| uuid_length); |
| } |
| |
| rp->descriptors_count++; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static void disc_all_desc(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_disc_all_desc_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_desc_rp))) { |
| goto fail; |
| } |
| |
| discover_params.start_handle = sys_le16_to_cpu(cmd->start_handle); |
| discover_params.end_handle = sys_le16_to_cpu(cmd->end_handle); |
| discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR; |
| discover_params.func = disc_all_desc_cb; |
| |
| if (bt_gatt_discover(conn, &discover_params) < 0) { |
| discover_destroy(&discover_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static struct bt_gatt_read_params read_params; |
| |
| static void read_destroy(void *user_data) |
| { |
| struct bt_gatt_read_params *params = user_data; |
| |
| memset(params, 0, sizeof(*params)); |
| |
| if (!gatt_buf_isempty()) { |
| gatt_buf_clear(); |
| } |
| } |
| |
| static void read_result(void *user_data) |
| { |
| /* Respond with an error if the buffer was cleared. */ |
| if (gatt_buf_isempty()) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| } else { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, |
| gatt_buf.buf, gatt_buf.len); |
| } |
| |
| read_destroy(user_data); |
| } |
| |
| static uint8_t read_cb(struct bt_conn *conn, int err, const void *data, |
| uint16_t length) |
| { |
| struct gatt_read_rp *rp = (void *) gatt_buf.buf; |
| |
| /* Respond to the Lower Tester with ATT Error received */ |
| if (err) { |
| rp->att_response = err; |
| return BT_GATT_ITER_STOP; |
| } |
| |
| /* |
| * Clear gatt_buf if there is no more space available to cache |
| * read result. This will cause read_result function to send |
| * BTP error status to the Lower Tester. |
| */ |
| if (!gatt_buf_add(data, length)) { |
| gatt_buf_clear(); |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| rp->data_length += length; |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static void read(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_read_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { |
| goto fail; |
| } |
| |
| read_params.handle = sys_le16_to_cpu(cmd->handle); |
| read_params.offset = 0x0000; |
| read_params.func = read_cb; |
| read_params.destroy = read_result; |
| |
| if (bt_gatt_read(conn, &read_params) < 0) { |
| read_destroy(&read_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static void read_long_result(void *user_data) |
| { |
| /* Respond with an error if the buffer was cleared. */ |
| if (gatt_buf_isempty()) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| } else { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_READ_LONG, |
| CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); |
| } |
| |
| read_destroy(user_data); |
| } |
| |
| static void read_long(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_read_long_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { |
| goto fail; |
| } |
| |
| read_params.handle = sys_le16_to_cpu(cmd->handle); |
| read_params.offset = sys_le16_to_cpu(cmd->offset); |
| read_params.func = read_cb; |
| read_params.destroy = read_long_result; |
| |
| if (bt_gatt_read(conn, &read_params) < 0) { |
| read_destroy(&read_params); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static uint8_t read_multiple_result(struct bt_conn *conn, int err, |
| const void *data, uint16_t length) |
| { |
| read_cb(conn, err, data, length); |
| |
| if (gatt_buf_isempty()) { |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, |
| CONTROLLER_INDEX, BTP_STATUS_FAILED); |
| } else { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, |
| CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); |
| |
| gatt_buf_clear(); |
| } |
| |
| return BT_GATT_ITER_STOP; |
| } |
| |
| static void read_multiple(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_read_multiple_cmd *cmd = (void *) data; |
| uint16_t handles[cmd->handles_count]; |
| struct bt_conn *conn; |
| int i; |
| |
| for (i = 0; i < ARRAY_SIZE(handles); i++) { |
| handles[i] = sys_le16_to_cpu(cmd->handles[i]); |
| } |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail_conn; |
| } |
| |
| if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { |
| goto fail; |
| } |
| |
| if (bt_gatt_read_multiple(conn, handles, cmd->handles_count, |
| read_multiple_result) < 0) { |
| gatt_buf_clear(); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| bt_conn_unref(conn); |
| |
| fail_conn: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_MULTIPLE, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static void write_without_rsp(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_write_without_rsp_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| uint8_t status = BTP_STATUS_SUCCESS; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| if (bt_gatt_write_without_response(conn, sys_le16_to_cpu(cmd->handle), |
| cmd->data, |
| sys_le16_to_cpu(cmd->data_length), |
| false) < 0) { |
| status = BTP_STATUS_FAILED; |
| } |
| |
| bt_conn_unref(conn); |
| rsp: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_WITHOUT_RSP, |
| CONTROLLER_INDEX, status); |
| } |
| |
| static void signed_write_without_rsp(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_write_without_rsp_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| uint8_t status = BTP_STATUS_SUCCESS; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| status = BTP_STATUS_FAILED; |
| goto rsp; |
| } |
| |
| if (bt_gatt_write_without_response(conn, sys_le16_to_cpu(cmd->handle), |
| cmd->data, |
| sys_le16_to_cpu(cmd->data_length), |
| true) < 0) { |
| status = BTP_STATUS_FAILED; |
| } |
| |
| bt_conn_unref(conn); |
| rsp: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_SIGNED_WRITE_WITHOUT_RSP, |
| CONTROLLER_INDEX, status); |
| } |
| |
| static void write_rsp(struct bt_conn *conn, uint8_t err) |
| { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX, &err, |
| sizeof(err)); |
| } |
| |
| static void write(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_write_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail; |
| } |
| |
| if (bt_gatt_write(conn, sys_le16_to_cpu(cmd->handle), 0, cmd->data, |
| sys_le16_to_cpu(cmd->data_length), write_rsp) < 0) { |
| bt_conn_unref(conn); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static void write_long_rsp(struct bt_conn *conn, uint8_t err) |
| { |
| tester_send(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, |
| &err, sizeof(err)); |
| } |
| |
| static void write_long(uint8_t *data, uint16_t len) |
| { |
| const struct gatt_write_long_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail; |
| } |
| |
| if (bt_gatt_write(conn, sys_le16_to_cpu(cmd->handle), |
| sys_le16_to_cpu(cmd->offset), cmd->data, |
| sys_le16_to_cpu(cmd->data_length), |
| write_long_rsp) < 0) { |
| bt_conn_unref(conn); |
| |
| goto fail; |
| } |
| |
| bt_conn_unref(conn); |
| |
| return; |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static struct bt_gatt_subscribe_params subscribe_params; |
| static struct bt_conn *default_conn; |
| |
| static void subscribe_destroy(void *user_data) |
| { |
| struct bt_gatt_subscribe_params *params = user_data; |
| |
| memset(params, 0, sizeof(*params)); |
| } |
| |
| /* ev header + default MTU_ATT-3 */ |
| static uint8_t ev_buf[33]; |
| |
| static uint8_t subscribe_func(struct bt_conn *conn, int err, |
| const void *data, uint16_t length) |
| { |
| struct gatt_notification_ev *ev = (void *) ev_buf; |
| const bt_addr_le_t *addr = bt_conn_get_dst(conn); |
| uint8_t op; |
| |
| op = subscribe_params.value == BT_GATT_CCC_NOTIFY ? GATT_CFG_NOTIFY : |
| GATT_CFG_INDICATE; |
| |
| if (!length) { |
| /* Subscribe procedure is complete, send response */ |
| tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, |
| err ? BTP_STATUS_FAILED : BTP_STATUS_SUCCESS); |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| if (length > ARRAY_SIZE(ev_buf)) { |
| BTTESTER_DBG("Out of memory"); |
| tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| ev->type = (uint8_t) subscribe_params.value; |
| ev->handle = sys_cpu_to_le16(subscribe_params.value_handle); |
| ev->data_length = sys_cpu_to_le16(length); |
| memcpy(ev->data, data, length); |
| memcpy(ev->address, addr->val, sizeof(ev->address)); |
| ev->address_type = addr->type; |
| |
| tester_send(BTP_SERVICE_ID_GATT, GATT_EV_NOTIFICATION, |
| CONTROLLER_INDEX, ev_buf, sizeof(*ev) + length); |
| |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static void discover_complete(struct bt_gatt_discover_params *params) |
| { |
| int err; |
| uint8_t op; |
| |
| /* If default_conn == NULL, it means that chrc has not been found */ |
| if (!default_conn) { |
| goto fail; |
| } |
| |
| err = bt_gatt_subscribe(default_conn, &subscribe_params); |
| |
| /* Drop conn reference taken by discover_func */ |
| bt_conn_unref(default_conn); |
| default_conn = NULL; |
| |
| /* Return if bt_gatt_subscribe succeeded */ |
| if (!err) { |
| return; |
| } |
| fail: |
| op = subscribe_params.value == BT_GATT_CCC_NOTIFY ? GATT_CFG_NOTIFY : |
| GATT_CFG_INDICATE; |
| subscribe_destroy(&subscribe_params); |
| tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| static uint8_t discover_func(struct bt_conn *conn, |
| const struct bt_gatt_attr *attr, |
| struct bt_gatt_discover_params *params) |
| { |
| if (!attr) { |
| discover_complete(params); |
| return BT_GATT_ITER_STOP; |
| } |
| |
| /* Characteristic Value Handle is the next handle beyond declaration */ |
| subscribe_params.value_handle = attr->handle + 1; |
| |
| /* Save this conn to be used in discover_complete*/ |
| if (!default_conn) { |
| default_conn = bt_conn_ref(conn); |
| } |
| |
| /* |
| * Continue characteristic discovery to get last characteristic |
| * preceding this CCC descriptor |
| */ |
| return BT_GATT_ITER_CONTINUE; |
| } |
| |
| static int enable_subscription(struct bt_conn *conn, uint16_t ccc_handle, |
| uint16_t value) |
| { |
| /* Fail if there is another subscription enabled */ |
| if (subscribe_params.value_handle) { |
| BTTESTER_DBG("Another subscription already enabled"); |
| return -EEXIST; |
| } |
| |
| /* Discover Characteristic Value this CCC Descriptor refers to */ |
| discover_params.start_handle = 0x0001; |
| discover_params.end_handle = ccc_handle; |
| discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; |
| discover_params.func = discover_func; |
| |
| subscribe_params.ccc_handle = ccc_handle; |
| subscribe_params.value = value; |
| subscribe_params.func = subscribe_func; |
| subscribe_params.destroy = subscribe_destroy; |
| |
| return bt_gatt_discover(conn, &discover_params); |
| } |
| |
| static int disable_subscription(struct bt_conn *conn, uint16_t ccc_handle) |
| { |
| /* Fail if CCC handle doesn't match */ |
| if (ccc_handle != subscribe_params.ccc_handle) { |
| BTTESTER_DBG("CCC handle doesn't match"); |
| return -EINVAL; |
| } |
| |
| if (bt_gatt_unsubscribe(conn, &subscribe_params) < 0) { |
| return -EBUSY; |
| } |
| |
| return 0; |
| } |
| |
| static void config_subscription(uint8_t *data, uint16_t len, uint16_t op) |
| { |
| const struct gatt_cfg_notify_cmd *cmd = (void *) data; |
| struct bt_conn *conn; |
| uint16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); |
| uint16_t value = op == GATT_CFG_NOTIFY ? BT_GATT_CCC_NOTIFY : |
| BT_GATT_CCC_INDICATE; |
| |
| conn = bt_conn_lookup_addr_le((bt_addr_le_t *) data); |
| if (!conn) { |
| goto fail; |
| } |
| |
| if (cmd->enable) { |
| if (enable_subscription(conn, ccc_handle, value) == 0) { |
| bt_conn_unref(conn); |
| /* Response will be sent by subscribe_func */ |
| return; |
| } |
| BTTESTER_DBG("Failed to enable subscription"); |
| } else { |
| if (disable_subscription(conn, ccc_handle) == 0) { |
| bt_conn_unref(conn); |
| tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, |
| BTP_STATUS_SUCCESS); |
| return; |
| } |
| BTTESTER_DBG("Failed to disable subscription"); |
| } |
| |
| bt_conn_unref(conn); |
| fail: |
| tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, |
| BTP_STATUS_FAILED); |
| } |
| |
| void tester_handle_gatt(uint8_t opcode, uint8_t index, uint8_t *data, |
| uint16_t len) |
| { |
| switch (opcode) { |
| case GATT_READ_SUPPORTED_COMMANDS: |
| supported_commands(data, len); |
| return; |
| case GATT_ADD_SERVICE: |
| add_service(data, len); |
| return; |
| case GATT_ADD_CHARACTERISTIC: |
| add_characteristic(data, len); |
| return; |
| case GATT_ADD_DESCRIPTOR: |
| add_descriptor(data, len); |
| return; |
| case GATT_ADD_INCLUDED_SERVICE: |
| add_included(data, len); |
| return; |
| case GATT_SET_VALUE: |
| set_value(data, len); |
| return; |
| case GATT_START_SERVER: |
| start_server(data, len); |
| return; |
| case GATT_SET_ENC_KEY_SIZE: |
| set_enc_key_size(data, len); |
| return; |
| case GATT_EXCHANGE_MTU: |
| exchange_mtu(data, len); |
| return; |
| case GATT_DISC_PRIM_UUID: |
| disc_prim_uuid(data, len); |
| return; |
| case GATT_FIND_INCLUDED: |
| find_included(data, len); |
| return; |
| case GATT_DISC_ALL_CHRC: |
| disc_all_chrc(data, len); |
| return; |
| case GATT_DISC_CHRC_UUID: |
| disc_chrc_uuid(data, len); |
| return; |
| case GATT_DISC_ALL_DESC: |
| disc_all_desc(data, len); |
| return; |
| case GATT_READ: |
| read(data, len); |
| return; |
| case GATT_READ_LONG: |
| read_long(data, len); |
| return; |
| case GATT_READ_MULTIPLE: |
| read_multiple(data, len); |
| return; |
| case GATT_WRITE_WITHOUT_RSP: |
| write_without_rsp(data, len); |
| return; |
| case GATT_SIGNED_WRITE_WITHOUT_RSP: |
| signed_write_without_rsp(data, len); |
| return; |
| case GATT_WRITE: |
| write(data, len); |
| return; |
| case GATT_WRITE_LONG: |
| write_long(data, len); |
| return; |
| case GATT_CFG_NOTIFY: |
| case GATT_CFG_INDICATE: |
| config_subscription(data, len, opcode); |
| return; |
| default: |
| tester_rsp(BTP_SERVICE_ID_GATT, opcode, index, |
| BTP_STATUS_UNKNOWN_CMD); |
| return; |
| } |
| } |
| |
| uint8_t tester_init_gatt(void) |
| { |
| ccc_added = false; |
| gatt_buf_clear(); |
| memset(&gatt_db, 0, sizeof(gatt_db)); |
| |
| return BTP_STATUS_SUCCESS; |
| } |