diff --git a/apps/bttester/src/gatt.c b/apps/bttester/src/gatt.c new file mode 100644 index 000000000..412227d8c --- /dev/null +++ b/apps/bttester/src/gatt.c @@ -0,0 +1,2098 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you 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. + */ + +/* gatt.c - Bluetooth GATT Server Tester */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#include "host/ble_gap.h" +#include "host/ble_gatt.h" +#include "console/console.h" +#include "services/gatt/ble_svc_gatt.h" +#include "../../../nimble/host/src/ble_att_priv.h" +#include "../../../nimble/host/src/ble_gatt_priv.h" + +#include "bttester.h" + +#define CONTROLLER_INDEX 0 +#define MAX_BUFFER_SIZE 2048 + +/* 0000xxxx-8c26-476f-89a7-a108033a69c7 */ +#define PTS_UUID_DECLARE(uuid16) \ + ((const ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT( \ + 0xc7, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89, \ + 0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00 \ + ))) + +/* 0000xxxx-8c26-476f-89a7-a108033a69c6 */ +#define PTS_UUID_DECLARE_ALT(uuid16) \ + ((const ble_uuid_t *) (&(ble_uuid128_t) BLE_UUID128_INIT( \ + 0xc6, 0x69, 0x3a, 0x03, 0x08, 0xa1, 0xa7, 0x89, \ + 0x6f, 0x47, 0x26, 0x8c, uuid16, uuid16 >> 8, 0x00, 0x00 \ + ))) + +#define PTS_SVC 0x0001 +#define PTS_CHR_READ 0x0002 +#define PTS_CHR_WRITE 0x0003 +#define PTS_CHR_RELIABLE_WRITE 0x0004 +#define PTS_CHR_WRITE_NO_RSP 0x0005 +#define PTS_CHR_READ_WRITE 0x0006 +#define PTS_CHR_READ_WRITE_ENC 0x0007 +#define PTS_CHR_READ_WRITE_AUTHEN 0x0008 +#define PTS_DSC_READ 0x0009 +#define PTS_DSC_WRITE 0x000a +#define PTS_DSC_READ_WRITE 0x000b +#define PTS_CHR_NOTIFY 0x0025 +#define PTS_LONG_CHR_READ_WRITE 0x0015 +#define PTS_LONG_CHR_READ_WRITE_ALT 0x0016 +#define PTS_LONG_DSC_READ_WRITE 0x001b +#define PTS_INC_SVC 0x001e +#define PTS_CHR_READ_WRITE_ALT 0x001f + +static uint8_t gatt_svr_pts_static_long_val[300]; +static uint8_t gatt_svr_pts_static_val[30]; +static uint8_t gatt_svr_pts_static_short_val; +static uint8_t notify_state; +static uint8_t indicate_state; +static uint16_t myconn_handle; +static struct os_callout notify_tx_timer; +uint16_t notify_handle; +uint8_t notify_value = 90; + +struct find_attr_data { + ble_uuid_any_t *uuid; + int attr_type; + void *ptr; + uint16_t handle; +}; + +static int +gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static int +gatt_svr_dsc_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static const struct ble_gatt_svc_def gatt_svr_inc_svcs[] = { + { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = PTS_UUID_DECLARE(PTS_INC_SVC), + .characteristics = (struct ble_gatt_chr_def[]) {{ + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ALT), + .access_cb = gatt_svr_read_write_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + 0, + } }, + }, + + { + 0, /* No more services. */ + }, +}; + +static const struct ble_gatt_svc_def *inc_svcs[] = { + &gatt_svr_inc_svcs[0], + NULL, +}; + +static const struct ble_gatt_svc_def gatt_svr_svcs[] = { + { + /*** Service: PTS test. */ + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = PTS_UUID_DECLARE(PTS_SVC), + .includes = inc_svcs, + .characteristics = (struct ble_gatt_chr_def[]) { { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE), + .access_cb = gatt_svr_read_write_test, + .flags = BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE, + .descriptors = (struct ble_gatt_dsc_def[]) { { + .uuid = PTS_UUID_DECLARE(PTS_DSC_READ_WRITE), + .access_cb = gatt_svr_dsc_read_write_test, + .att_flags = BLE_ATT_F_READ | + BLE_ATT_F_WRITE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_LONG_DSC_READ_WRITE), + .access_cb = gatt_svr_dsc_read_write_long_test, + .att_flags = BLE_ATT_F_READ | + BLE_ATT_F_WRITE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_DSC_READ), + .access_cb = gatt_svr_dsc_read_test, + .att_flags = BLE_ATT_F_READ, + }, { + 0, /* No more descriptors in this characteristic */ + } } + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_WRITE_NO_RSP), + .access_cb = gatt_svr_write_no_rsp_test, + .flags = BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_WRITE_NO_RSP, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_AUTHEN), + .access_cb = gatt_svr_read_write_auth_test, + .flags = BLE_GATT_CHR_F_READ_AUTHEN | + BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE_AUTHEN | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_WRITE_AUTHEN, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_RELIABLE_WRITE), + .access_cb = gatt_svr_rel_write_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_RELIABLE_WRITE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_ENC), + .access_cb = gatt_svr_read_write_enc_test, + .flags = BLE_GATT_CHR_F_READ_ENC | + BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_WRITE_ENC, + .min_key_size = 16, + }, { + .uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE), + .access_cb = gatt_svr_read_write_long_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + .uuid = PTS_UUID_DECLARE(PTS_LONG_CHR_READ_WRITE_ALT), + .access_cb = gatt_svr_read_write_long_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_NOTIFY), + .access_cb = gatt_svr_read_write_test, + .val_handle = ¬ify_handle, + .flags = BLE_GATT_CHR_F_NOTIFY | + BLE_GATT_CHR_F_INDICATE, + }, { + 0, /* No more characteristics in this service. */ + } }, + }, + + { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = PTS_UUID_DECLARE_ALT(PTS_SVC), + .characteristics = (struct ble_gatt_chr_def[]) { { + .uuid = PTS_UUID_DECLARE_ALT(PTS_CHR_READ_WRITE), + .access_cb = gatt_svr_read_write_test, + .flags = BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_READ, + }, { + 0, /* No more characteristics in this service */ + } }, + }, + + { + 0, /* No more services. */ + }, +}; + +static void attr_value_changed_ev(uint16_t handle, struct os_mbuf *data) +{ + struct gatt_attr_value_changed_ev *ev; + struct os_mbuf *buf = os_msys_get(0, 0); + + SYS_LOG_DBG(""); + + net_buf_simple_init(buf, 0); + ev = net_buf_simple_add(buf, sizeof(*ev)); + + ev->handle = sys_cpu_to_le16(handle); + ev->data_length = sys_cpu_to_le16(os_mbuf_len(data)); + os_mbuf_appendfrom(buf, data, 0, os_mbuf_len(data)); + + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_ATTR_VALUE_CHANGED, + CONTROLLER_INDEX, buf); +} + +static int +gatt_svr_chr_write(uint16_t conn_handle, uint16_t attr_handle, + struct os_mbuf *om, uint16_t min_len, uint16_t max_len, + void *dst, uint16_t *len) +{ + uint16_t om_len; + int rc; + + om_len = OS_MBUF_PKTLEN(om); + if (om_len < min_len || om_len > max_len) { + return BLE_ATT_ERR_INVALID_ATTR_VALUE_LEN; + } + + rc = ble_hs_mbuf_to_flat(om, dst, max_len, len); + if (rc != 0) { + return BLE_ATT_ERR_UNLIKELY; + } + + attr_value_changed_ev(attr_handle, om); + + return 0; +} + +static uint16_t +extract_uuid16_from_pts_uuid128(const ble_uuid_t *uuid) +{ + const uint8_t *u8ptr; + uint16_t uuid16; + + u8ptr = BLE_UUID128(uuid)->value; + uuid16 = u8ptr[12]; + uuid16 |= (uint16_t)u8ptr[13] << 8; + return uuid16; +} + +static int +gatt_svr_read_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_CHR_READ_WRITE: + case PTS_CHR_READ_WRITE_ALT: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_short_val, + &gatt_svr_pts_static_short_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, + sizeof gatt_svr_pts_static_short_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_LONG_CHR_READ_WRITE: + case PTS_LONG_CHR_READ_WRITE_ALT: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_long_val, + &gatt_svr_pts_static_long_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, + sizeof gatt_svr_pts_static_long_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_read_write_auth_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_CHR_READ_WRITE_AUTHEN: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_val, + &gatt_svr_pts_static_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val, + sizeof gatt_svr_pts_static_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_read_write_enc_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_CHR_READ_WRITE_ENC: + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_val, + sizeof gatt_svr_pts_static_val); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_val, + &gatt_svr_pts_static_val, NULL); + return rc; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_dsc_read_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_DSC_READ_WRITE: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_short_val, + &gatt_svr_pts_static_short_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, + sizeof gatt_svr_pts_static_short_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_dsc_read_write_long_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_LONG_DSC_READ_WRITE: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_DSC) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_long_val, + &gatt_svr_pts_static_long_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, + sizeof gatt_svr_pts_static_long_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_dsc_read_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_DSC_READ: + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_long_val, + sizeof gatt_svr_pts_static_long_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_write_no_rsp_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_CHR_WRITE_NO_RSP: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_short_val, + &gatt_svr_pts_static_short_val, NULL); + return rc; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + rc = os_mbuf_append(ctxt->om, &gatt_svr_pts_static_short_val, + sizeof gatt_svr_pts_static_short_val); + return rc == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static int +gatt_svr_rel_write_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + int rc; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_CHR_RELIABLE_WRITE: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + rc = gatt_svr_chr_write(conn_handle, attr_handle, + ctxt->om, 0, + sizeof gatt_svr_pts_static_val, + &gatt_svr_pts_static_val, NULL); + return rc; + } + + default: + assert(0); + return BLE_ATT_ERR_UNLIKELY; + } +} + +static void start_server(uint8_t *data, uint16_t len) +{ + struct gatt_start_server_rp rp; + + SYS_LOG_DBG(""); + + ble_gatts_show_local(); + + ble_svc_gatt_changed(0x0001, 0xffff); + + rp.db_attr_off = 0; + rp.db_attr_cnt = 0; + + tester_send(BTP_SERVICE_ID_GATT, GATT_START_SERVER, CONTROLLER_INDEX, + (uint8_t *) &rp, sizeof(rp)); +} + +/* Convert UUID from BTP command to bt_uuid */ +static uint8_t btp2bt_uuid(const uint8_t *uuid, uint8_t len, + ble_uuid_any_t *bt_uuid) +{ + uint16_t le16; + + switch (len) { + case 0x02: /* UUID 16 */ + bt_uuid->u.type = BLE_UUID_TYPE_16; + memcpy(&le16, uuid, sizeof(le16)); + BLE_UUID16(bt_uuid)->value = sys_le16_to_cpu(le16); + break; + case 0x10: /* UUID 128*/ + bt_uuid->u.type = BLE_UUID_TYPE_128; + memcpy(BLE_UUID128(bt_uuid)->value, uuid, 16); + break; + default: + return BTP_STATUS_FAILED; + } + return BTP_STATUS_SUCCESS; +} + +/* + * 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_add(const void *data, size_t len) +{ + void *ptr = gatt_buf.buf + gatt_buf.len; + + if ((len + gatt_buf.len) > MAX_BUFFER_SIZE) { + return NULL; + } + + if (data) { + memcpy(ptr, data, len); + } else { + (void)memset(ptr, 0, len); + } + + gatt_buf.len += len; + + SYS_LOG_DBG("%d/%d used", gatt_buf.len, MAX_BUFFER_SIZE); + + return ptr; +} + +static void *gatt_buf_reserve(size_t len) +{ + return gatt_buf_add(NULL, len); +} + +static void gatt_buf_clear(void) +{ + (void)memset(&gatt_buf, 0, sizeof(gatt_buf)); +} + +static void discover_destroy(void) +{ + gatt_buf_clear(); +} + +static void read_destroy(void) +{ + gatt_buf_clear(); +} + +static int read_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + struct gatt_read_rp *rp = (void *) gatt_buf.buf; + uint8_t btp_opcode = (uint8_t) (int) arg; + + SYS_LOG_DBG("status=%d", error->status); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status); + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + return 0; + } + + if (!gatt_buf_add(attr->om->om_data, attr->om->om_len)) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + read_destroy(); + return 0; + } + + rp->data_length += attr->om->om_len; + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + + return 0; +} + +static void read(uint8_t *data, uint16_t len) +{ + const struct gatt_read_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } + + if (ble_gattc_read(conn.conn_handle, sys_le16_to_cpu(cmd->handle), + read_cb, (void *)GATT_READ)) { + read_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static int read_long_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + struct gatt_read_rp *rp = (void *) gatt_buf.buf; + uint8_t btp_opcode = (uint8_t) (int) arg; + + SYS_LOG_DBG("status=%d", error->status); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + rp->att_response = (uint8_t) BLE_HS_ATT_ERR(error->status); + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + return 0; + } + + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + read_destroy(); + return 0; + } + + if (gatt_buf_add(attr->om->om_data, attr->om->om_len) == NULL) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + read_destroy(); + return BLE_HS_ENOMEM; + } + + rp->data_length += attr->om->om_len; + + return 0; +} + +static void read_long(uint8_t *data, uint16_t len) +{ + const struct gatt_read_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } + + if (ble_gattc_read_long(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), + sys_le16_to_cpu(cmd->offset), + read_long_cb, (void *)GATT_READ_LONG)) { + read_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +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 ble_gap_conn_desc conn; + int rc, i; + + SYS_LOG_DBG(""); + + for (i = 0; i < ARRAY_SIZE(handles); i++) { + handles[i] = sys_le16_to_cpu(cmd->handles[i]); + } + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } + + if (ble_gattc_read_mult(conn.conn_handle, handles, + cmd->handles_count, read_cb, + (void *)GATT_READ_MULTIPLE)) { + read_destroy(); + goto fail; + } + + return; + +fail: + 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, uint8_t op, bool sign) +{ + const struct gatt_write_without_rsp_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint8_t status = BTP_STATUS_SUCCESS; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + status = BTP_STATUS_FAILED; + goto rsp; + } + + if (ble_gattc_write_no_rsp_flat(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), cmd->data, + sys_le16_to_cpu(cmd->data_length))) { + status = BTP_STATUS_FAILED; + } + +rsp: + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); +} + +static int write_rsp(uint16_t conn_handle, const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + uint8_t err = (uint8_t) error->status; + uint8_t btp_opcode = (uint8_t) (int) arg; + + SYS_LOG_DBG(""); + + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, &err, sizeof(err)); + return 0; +} + +static void write(uint8_t *data, uint16_t len) +{ + const struct gatt_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (ble_gattc_write_flat(conn.conn_handle, sys_le16_to_cpu(cmd->handle), + cmd->data, sys_le16_to_cpu(cmd->data_length), + write_rsp, (void *) GATT_WRITE)) { + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void write_long(uint8_t *data, uint16_t len) +{ + const struct gatt_write_long_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + struct os_mbuf *om = NULL; + int rc = 0; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); + if (!om) { + SYS_LOG_ERR("Insufficient resources"); + goto fail; + } + + rc = ble_gattc_write_long(conn.conn_handle, + sys_le16_to_cpu(cmd->handle), + sys_le16_to_cpu(cmd->offset), + om, write_rsp, + (void *) GATT_WRITE_LONG); + if (!rc) { + return; + } + +fail: + SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc); + os_mbuf_free_chain(om); + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static int reliable_write_rsp(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attrs, + uint8_t num_attrs, + void *arg) +{ + uint8_t err = (uint8_t) error->status; + + SYS_LOG_DBG("Reliable write status %d", err); + + tester_send(BTP_SERVICE_ID_GATT, GATT_RELIABLE_WRITE, + CONTROLLER_INDEX, &err, sizeof(err)); + return 0; +} + +static void reliable_write(uint8_t *data, uint16_t len) +{ + const struct gatt_reliable_write_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + struct ble_gatt_attr attr; + struct os_mbuf *om = NULL; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + om = ble_hs_mbuf_from_flat(cmd->data, sys_le16_to_cpu(cmd->data_length)); + /* This is required, because Nimble checks if + * the data is longer than offset + */ + if (os_mbuf_extend(om, sys_le16_to_cpu(cmd->offset) + 1) == NULL) { + goto fail; + } + + attr.handle = sys_le16_to_cpu(cmd->handle); + attr.offset = sys_le16_to_cpu(cmd->offset); + attr.om = om; + + if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, + reliable_write_rsp, NULL)) { + goto fail; + } + + return; + +fail: + os_mbuf_free_chain(om); + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_WRITE_LONG, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static struct bt_gatt_subscribe_params { + uint16_t ccc_handle; + uint16_t value; + uint16_t value_handle; +} subscribe_params; + +static void read_uuid(uint8_t *data, uint16_t len) +{ + const struct gatt_read_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + ble_uuid_any_t uuid; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + goto fail; + } + + /* Clear buffer */ + read_destroy(); + + if (!gatt_buf_reserve(sizeof(struct gatt_read_rp))) { + goto fail; + } + + if (ble_gattc_read_by_uuid(conn.conn_handle, + sys_le16_to_cpu(cmd->start_handle), + sys_le16_to_cpu(cmd->end_handle), &uuid.u, + read_long_cb, (void *)GATT_READ_UUID)) { + read_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_READ, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static int disc_prim_uuid_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_svc *gatt_svc, void *arg) +{ + struct gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf; + struct gatt_service *service; + const ble_uuid_any_t *uuid; + uint8_t uuid_length; + uint8_t opcode = (uint8_t) (int) arg; + + SYS_LOG_DBG(""); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } + + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } + + uuid = &gatt_svc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + + service = gatt_buf_reserve(sizeof(*service) + uuid_length); + if (!service) { + tester_rsp(BTP_SERVICE_ID_GATT, opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return BLE_HS_ENOMEM; + } + + service->start_handle = sys_cpu_to_le16(gatt_svc->start_handle); + service->end_handle = sys_cpu_to_le16(gatt_svc->end_handle); + service->uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(service->uuid, &u16, uuid_length); + } else { + memcpy(service->uuid, BLE_UUID128(uuid)->value, + uuid_length); + } + + rp->services_count++; + + return 0; +} + +static int disc_all_desc_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + uint16_t chr_val_handle, + const struct ble_gatt_dsc *gatt_dsc, + void *arg) +{ + struct gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf; + struct gatt_descriptor *dsc; + const ble_uuid_any_t *uuid; + uint8_t uuid_length; + + SYS_LOG_DBG(""); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } + + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } + + uuid = &gatt_dsc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_UUID_TYPE_16 ? 2 : 16); + + dsc = gatt_buf_reserve(sizeof(*dsc) + uuid_length); + if (!dsc) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return BLE_HS_ENOMEM; + } + + dsc->descriptor_handle = sys_cpu_to_le16(gatt_dsc->handle); + dsc->uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(dsc->uuid, &u16, uuid_length); + } else { + memcpy(dsc->uuid, BLE_UUID128(uuid)->value, uuid_length); + } + + rp->descriptors_count++; + + return 0; +} + +static void disc_all_prim_svcs(uint8_t *data, uint16_t len) +{ + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_prim_svcs_rp))) { + goto fail; + } + + if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_uuid_cb, + (void *) GATT_DISC_ALL_PRIM_SVCS)) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_PRIM_SVCS, + CONTROLLER_INDEX, BTP_STATUS_FAILED); +} + +static void disc_all_desc(uint8_t *data, uint16_t len) +{ + const struct gatt_disc_all_desc_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (!gatt_buf_reserve(sizeof(struct gatt_disc_all_desc_rp))) { + goto fail; + } + + start_handle = sys_le16_to_cpu(cmd->start_handle) - 1; + end_handle = sys_le16_to_cpu(cmd->end_handle); + + rc = ble_gattc_disc_all_dscs(conn.conn_handle, start_handle, end_handle, + disc_all_desc_cb, NULL); + + SYS_LOG_DBG("rc=%d", rc); + + if (rc) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_DESC, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static int find_included_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_svc *gatt_svc, void *arg) +{ + struct gatt_find_included_rp *rp = (void *) gatt_buf.buf; + struct gatt_included *included; + const ble_uuid_any_t *uuid; + int service_handle = (int) arg; + uint8_t uuid_length; + + SYS_LOG_DBG(""); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } + + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } + + uuid = &gatt_svc->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_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(); + return BLE_HS_ENOMEM; + } + + /* FIXME */ + included->included_handle = sys_cpu_to_le16(service_handle + 1 + + rp->services_count); + included->service.start_handle = sys_cpu_to_le16(gatt_svc->start_handle); + included->service.end_handle = sys_cpu_to_le16(gatt_svc->end_handle); + included->service.uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(included->service.uuid, &u16, uuid_length); + } else { + memcpy(included->service.uuid, BLE_UUID128(uuid)->value, + uuid_length); + } + + rp->services_count++; + + return 0; +} + +static int disc_chrc_cb(uint16_t conn_handle, + const struct ble_gatt_error *error, + const struct ble_gatt_chr *gatt_chr, void *arg) +{ + struct gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf; + struct gatt_characteristic *chrc; + const ble_uuid_any_t *uuid; + uint8_t btp_opcode = (uint8_t) (int) arg; + uint8_t uuid_length; + + SYS_LOG_DBG(""); + + if (error->status != 0 && error->status != BLE_HS_EDONE) { + tester_rsp(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + discover_destroy(); + return 0; + } + + if (error->status == BLE_HS_EDONE) { + tester_send(BTP_SERVICE_ID_GATT, btp_opcode, + CONTROLLER_INDEX, gatt_buf.buf, gatt_buf.len); + discover_destroy(); + return 0; + } + + uuid = &gatt_chr->uuid; + uuid_length = (uint8_t) (uuid->u.type == BLE_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(); + return BLE_HS_ENOMEM; + } + + chrc->characteristic_handle = sys_cpu_to_le16(gatt_chr->def_handle); + chrc->properties = gatt_chr->properties; + chrc->value_handle = sys_cpu_to_le16(gatt_chr->val_handle); + chrc->uuid_length = uuid_length; + + if (uuid->u.type == BLE_UUID_TYPE_16) { + uint16_t u16 = sys_cpu_to_le16(BLE_UUID16(uuid)->value); + memcpy(chrc->uuid, &u16, uuid_length); + } else { + memcpy(chrc->uuid, BLE_UUID128(uuid)->value, + uuid_length); + } + + rp->characteristics_count++; + + return 0; +} + +static void disc_chrc_uuid(uint8_t *data, uint16_t len) +{ + const struct gatt_disc_chrc_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + ble_uuid_any_t uuid; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + goto fail; + } + + if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { + goto fail; + } + + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + + if (ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle, + end_handle, &uuid.u, disc_chrc_cb, + (void *)GATT_DISC_CHRC_UUID)) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_CHRC_UUID, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void disc_prim_uuid(uint8_t *data, uint16_t len) +{ + const struct gatt_disc_prim_uuid_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + ble_uuid_any_t uuid; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (btp2bt_uuid(cmd->uuid, cmd->uuid_length, &uuid)) { + goto fail; + } + + if (!gatt_buf_reserve(sizeof(struct gatt_disc_prim_uuid_rp))) { + goto fail; + } + + if (ble_gattc_disc_svc_by_uuid(conn.conn_handle, + &uuid.u, disc_prim_uuid_cb, + (void *) GATT_DISC_PRIM_UUID)) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_PRIM_UUID, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void disc_all_chrc(uint8_t *data, uint16_t len) +{ + const struct gatt_disc_all_chrc_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + SYS_LOG_DBG("Conn find failed"); + goto fail; + } + + if (!gatt_buf_reserve(sizeof(struct gatt_disc_chrc_rp))) { + SYS_LOG_DBG("Buf reserve failed"); + goto fail; + } + + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + + rc = ble_gattc_disc_all_chrs(conn.conn_handle, start_handle, end_handle, + disc_chrc_cb, (void *)GATT_DISC_ALL_CHRC); + if (rc) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_DISC_ALL_CHRC, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static void find_included(uint8_t *data, uint16_t len) +{ + const struct gatt_find_included_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t start_handle, end_handle; + int service_handle_arg; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (!gatt_buf_reserve(sizeof(struct gatt_find_included_rp))) { + goto fail; + } + + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + service_handle_arg = start_handle; + + if (ble_gattc_find_inc_svcs(conn.conn_handle, start_handle, end_handle, + find_included_cb, + (void *)service_handle_arg)) { + discover_destroy(); + goto fail; + } + + return; + +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_FIND_INCLUDED, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +} + +static int exchange_func(uint16_t conn_handle, + const struct ble_gatt_error *error, + uint16_t mtu, void *arg) +{ + SYS_LOG_DBG(""); + + if (error->status) { + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, + CONTROLLER_INDEX, BTP_STATUS_FAILED); + + return 0; +} + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + + return 0; +} + +static void exchange_mtu(uint8_t *data, uint16_t len) +{ + struct ble_gap_conn_desc conn; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + goto fail; + } + + if (ble_gattc_exchange_mtu(conn.conn_handle, exchange_func, NULL)) { + goto fail; + } + + return; +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_EXCHANGE_MTU, + CONTROLLER_INDEX, BTP_STATUS_FAILED); +} + +static int enable_subscription(uint16_t conn_handle, uint16_t ccc_handle, + uint16_t value) +{ + uint8_t op; + + SYS_LOG_DBG(""); + + op = (uint8_t) (value == 0x0001 ? GATT_CFG_NOTIFY : GATT_CFG_INDICATE); + + if (ble_gattc_write_flat(conn_handle, ccc_handle, + &value, sizeof(value), NULL, NULL)) { + return -EINVAL; + } + + subscribe_params.ccc_handle = value; + + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + return 0; +} + +static int disable_subscription(uint16_t conn_handle, uint16_t ccc_handle) +{ + uint16_t value = 0x00; + + SYS_LOG_DBG(""); + + /* Fail if CCC handle doesn't match */ + if (ccc_handle != subscribe_params.ccc_handle) { + SYS_LOG_ERR("CCC handle doesn't match"); + return -EINVAL; + } + + if (ble_gattc_write_no_rsp_flat(conn_handle, ccc_handle, + &value, sizeof(value))) { + return -EINVAL; + } + + subscribe_params.ccc_handle = 0; + return 0; +} + +static void config_subscription(uint8_t *data, uint16_t len, uint8_t op) +{ + const struct gatt_cfg_notify_cmd *cmd = (void *) data; + struct ble_gap_conn_desc conn; + uint16_t ccc_handle = sys_le16_to_cpu(cmd->ccc_handle); + uint8_t status; + int rc; + + SYS_LOG_DBG(""); + + rc = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + if (rc) { + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, + BTP_STATUS_FAILED); + return; + } + + if (cmd->enable) { + uint16_t value; + + if (op == GATT_CFG_NOTIFY) { + value = 0x0001; + } else { + value = 0x0002; + } + + /* on success response will be sent from callback */ + if (enable_subscription(conn.conn_handle, + ccc_handle, value) == 0) { + return; + } + + status = BTP_STATUS_FAILED; + } else { + if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { + status = BTP_STATUS_FAILED; + } else { + status = BTP_STATUS_SUCCESS; + } + } + + SYS_LOG_DBG("Config subscription (op %u) status %u", op, status); + + tester_rsp(BTP_SERVICE_ID_GATT, op, CONTROLLER_INDEX, status); +} + +#define BTP_PERM_F_READ 0x01 +#define BTP_PERM_F_WRITE 0x02 +#define BTP_PERM_F_READ_ENC 0x04 +#define BTP_PERM_F_WRITE_ENC 0x08 +#define BTP_PERM_F_READ_AUTHEN 0x10 +#define BTP_PERM_F_WRITE_AUTHEN 0x20 +#define BTP_PERM_F_READ_AUTHOR 0x40 +#define BTP_PERM_F_WRITE_AUTHOR 0x80 + +static int flags_hs2btp_map[] = { + BTP_PERM_F_READ, + BTP_PERM_F_WRITE, + BTP_PERM_F_READ_ENC, + BTP_PERM_F_READ_AUTHEN, + BTP_PERM_F_READ_AUTHOR, + BTP_PERM_F_WRITE_ENC, + BTP_PERM_F_WRITE_AUTHEN, + BTP_PERM_F_WRITE_AUTHOR, +}; + +static uint8_t flags_hs2btp(uint8_t flags) +{ + int i; + uint8_t ret = 0; + + for (i = 0; i < 8; ++i) { + if (flags & BIT(i)) { + ret |= flags_hs2btp_map[i]; + } + } + + return ret; +} + +static void get_attrs(uint8_t *data, uint16_t len) +{ + const struct gatt_get_attributes_cmd *cmd = (void *) data; + struct gatt_get_attributes_rp *rp; + struct gatt_attr *gatt_attr; + struct os_mbuf *buf = os_msys_get(0, 0); + uint16_t start_handle, end_handle; + struct ble_att_svr_entry *entry = NULL; + ble_uuid_any_t uuid; + ble_uuid_t *uuid_ptr = NULL; + uint8_t count = 0; + char str[BLE_UUID_STR_LEN]; + + SYS_LOG_DBG(""); + + memset(str, 0, sizeof(str)); + memset(&uuid, 0, sizeof(uuid)); + start_handle = sys_le16_to_cpu(cmd->start_handle); + end_handle = sys_le16_to_cpu(cmd->end_handle); + + if (cmd->type_length) { + if (btp2bt_uuid(cmd->type, cmd->type_length, &uuid)) { + goto fail; + } + + ble_uuid_to_str(&uuid.u, str); + SYS_LOG_DBG("start 0x%04x end 0x%04x, uuid %s", start_handle, + end_handle, str); + + uuid_ptr = &uuid.u; + } else { + SYS_LOG_DBG("start 0x%04x end 0x%04x", start_handle, end_handle); + } + + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); + while (entry) { + + if (entry->ha_handle_id < start_handle) { + entry = ble_att_svr_find_by_uuid(entry, + uuid_ptr, end_handle); + continue; + } + + gatt_attr = net_buf_simple_add(buf, sizeof(*gatt_attr)); + gatt_attr->handle = sys_cpu_to_le16(entry->ha_handle_id); + gatt_attr->permission = flags_hs2btp(entry->ha_flags); + + if (entry->ha_uuid->type == BLE_UUID_TYPE_16) { + gatt_attr->type_length = 2; + net_buf_simple_add_le16(buf, + BLE_UUID16(entry->ha_uuid)->value); + } else { + gatt_attr->type_length = 16; + net_buf_simple_add_mem(buf, + BLE_UUID128(entry->ha_uuid)->value, + gatt_attr->type_length); + } + + count++; + + entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); + } + + rp->attrs_count = count; + + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, + CONTROLLER_INDEX, buf); + + goto free; +fail: + tester_rsp(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTES, CONTROLLER_INDEX, + BTP_STATUS_FAILED); +free: + os_mbuf_free_chain(buf); +} + +static void get_attr_val(uint8_t *data, uint16_t len) +{ + const struct gatt_get_attribute_value_cmd *cmd = (void *) data; + struct gatt_get_attribute_value_rp *rp; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + uint16_t handle = sys_cpu_to_le16(cmd->handle); + uint8_t out_att_err; + int conn_status; + + conn_status = ble_gap_conn_find_by_addr((ble_addr_t *)data, &conn); + + if (conn_status) { + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + ble_att_svr_read_handle(BLE_HS_CONN_HANDLE_NONE, + handle, 0, buf, + &out_att_err); + + rp->att_response = out_att_err; + rp->value_length = os_mbuf_len(buf) - sizeof(*rp); + + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE, + CONTROLLER_INDEX, buf); + + goto free; + } else { + net_buf_simple_init(buf, 0); + rp = net_buf_simple_add(buf, sizeof(*rp)); + + ble_att_svr_read_handle(conn.conn_handle, + handle, 0, buf, + &out_att_err); + + rp->att_response = out_att_err; + rp->value_length = os_mbuf_len(buf) - sizeof(*rp); + + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_GET_ATTRIBUTE_VALUE, + CONTROLLER_INDEX, buf); + + goto free; + } + +free: + os_mbuf_free_chain(buf); +} + +static void change_database(uint8_t *data, uint16_t len) +{ + const struct gatt_change_database *cmd = (void *) data; + + SYS_LOG_DBG("") + + ble_gatts_show_local(); + + ble_svc_gatt_changed(cmd->start_handle, cmd->end_handle); + + tester_rsp(BTP_SERVICE_ID_GATT, GATT_CHANGE_DATABASE, CONTROLLER_INDEX, + BTP_STATUS_SUCCESS); + + return; +} + +static void supported_commands(uint8_t *data, uint16_t len) +{ + uint8_t cmds[4]; + struct gatt_read_supported_commands_rp *rp = (void *) cmds; + + SYS_LOG_DBG(""); + + memset(cmds, 0, sizeof(cmds)); + + tester_set_bit(cmds, GATT_READ_SUPPORTED_COMMANDS); + tester_set_bit(cmds, GATT_START_SERVER); + tester_set_bit(cmds, GATT_EXCHANGE_MTU); + tester_set_bit(cmds, GATT_DISC_ALL_PRIM_SVCS); + tester_set_bit(cmds, GATT_DISC_PRIM_UUID); + tester_set_bit(cmds, GATT_FIND_INCLUDED); + tester_set_bit(cmds, GATT_DISC_ALL_CHRC); + tester_set_bit(cmds, GATT_DISC_CHRC_UUID); + tester_set_bit(cmds, GATT_DISC_ALL_DESC); + tester_set_bit(cmds, GATT_READ); + tester_set_bit(cmds, GATT_READ_LONG); + tester_set_bit(cmds, GATT_READ_MULTIPLE); + tester_set_bit(cmds, GATT_WRITE_WITHOUT_RSP); +#if 0 + tester_set_bit(cmds, GATT_SIGNED_WRITE_WITHOUT_RSP); +#endif + tester_set_bit(cmds, GATT_WRITE); + tester_set_bit(cmds, GATT_WRITE_LONG); + tester_set_bit(cmds, GATT_CFG_NOTIFY); + tester_set_bit(cmds, GATT_CFG_INDICATE); + tester_set_bit(cmds, GATT_GET_ATTRIBUTES); + tester_set_bit(cmds, GATT_GET_ATTRIBUTE_VALUE); + tester_set_bit(cmds, GATT_CHANGE_DATABASE); + + tester_send(BTP_SERVICE_ID_GATT, GATT_READ_SUPPORTED_COMMANDS, + CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds)); +} + +enum attr_type { + BLE_GATT_ATTR_SVC = 0, + BLE_GATT_ATTR_CHR, + BLE_GATT_ATTR_DSC, +}; + +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_START_SERVER: + start_server(data, len); + return; + case GATT_EXCHANGE_MTU: + exchange_mtu(data, len); + return; + case GATT_DISC_ALL_PRIM_SVCS: + disc_all_prim_svcs(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_CHANGE_DATABASE: + change_database(data, len); + return; + case GATT_READ: + read(data, len); + return; + case GATT_READ_UUID: + read_uuid(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, opcode, false); + return; +#if 0 + case GATT_SIGNED_WRITE_WITHOUT_RSP: + write_without_rsp(data, len, opcode, true); + return; +#endif + case GATT_WRITE: + write(data, len); + return; + case GATT_WRITE_LONG: + write_long(data, len); + return; + case GATT_RELIABLE_WRITE: + reliable_write(data, len); + return; + case GATT_CFG_NOTIFY: + case GATT_CFG_INDICATE: + config_subscription(data, len, opcode); + return; + case GATT_GET_ATTRIBUTES: + get_attrs(data, len); + return; + case GATT_GET_ATTRIBUTE_VALUE: + get_attr_val(data, len); + return; + default: + tester_rsp(BTP_SERVICE_ID_GATT, opcode, index, + BTP_STATUS_UNKNOWN_CMD); + return; + } +} + +int tester_gatt_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, + uint8_t indication, struct os_mbuf *om) +{ + struct gatt_notification_ev *ev; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + const ble_addr_t *addr; + + SYS_LOG_DBG(""); + + if (!subscribe_params.ccc_handle) { + goto fail; + } + + if (ble_gap_conn_find(conn_handle, &conn)) { + goto fail; + } + + net_buf_simple_init(buf, 0); + ev = net_buf_simple_add(buf, sizeof(*ev)); + + addr = &conn.peer_ota_addr; + + ev->address_type = addr->type; + memcpy(ev->address, addr->val, sizeof(ev->address)); + ev->type = (uint8_t) (indication ? 0x02 : 0x01); + ev->handle = sys_cpu_to_le16(attr_handle); + ev->data_length = sys_cpu_to_le16(os_mbuf_len(om)); + os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om)); + + tester_send_buf(BTP_SERVICE_ID_GATT, GATT_EV_NOTIFICATION, + CONTROLLER_INDEX, buf); + +fail: + os_mbuf_free_chain(buf); + return 0; +} + +void notify_test_stop(void) +{ + os_callout_stop(¬ify_tx_timer); +} + +void notify_test_reset(void) +{ + int rc; + + rc = os_callout_reset(¬ify_tx_timer, OS_TICKS_PER_SEC); + assert(rc == 0); +} + +void notify_test(struct os_event *ev) +{ + static uint8_t ntf[1]; + struct os_mbuf *om; + int rc; + + if (!notify_state && !indicate_state) { + notify_test_stop(); + notify_value = 90; + return; + } + + ntf[0] = notify_value; + + notify_value++; + if (notify_value == 160) { + notify_value = 90; + } + + om = ble_hs_mbuf_from_flat(ntf, sizeof(ntf)); + + if (notify_state) { + rc = ble_gatts_notify_custom(myconn_handle, notify_handle, om); + assert(rc == 0); + } + + if (indicate_state) { + rc = ble_gatts_indicate_custom(myconn_handle, notify_handle, om); + assert(rc == 0); + } +} + +int tester_gatt_subscribe_ev(uint16_t conn_handle, uint16_t attr_handle, uint8_t reason, + uint8_t prev_notify, uint8_t cur_notify, + uint8_t prev_indicate, uint8_t cur_indicate) +{ + SYS_LOG_DBG(""); + myconn_handle = conn_handle; + + if (cur_notify == 0 && cur_indicate == 0) { + SYS_LOG_INF("Unsubscribed"); + memset(&subscribe_params, 0, sizeof(subscribe_params)); + return 0; + } + + if (cur_notify) { + SYS_LOG_INF("Subscribed to notifications"); + if (attr_handle == notify_handle) { + notify_state = cur_notify; + } + } + + if (cur_indicate) { + SYS_LOG_INF("Subscribed to indications"); + if (attr_handle == notify_handle) { + indicate_state = cur_indicate; + } + } + + + if (notify_state || indicate_state) { + notify_test_reset(); + } else { + notify_test_stop(); + } + + return 0; +} + +void +gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) +{ + char buf[BLE_UUID_STR_LEN]; + + switch (ctxt->op) { + case BLE_GATT_REGISTER_OP_SVC: + MODLOG_DFLT(DEBUG, "registered service %s with handle=%d\n", + ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf), + ctxt->svc.handle); + break; + + case BLE_GATT_REGISTER_OP_CHR: + MODLOG_DFLT(DEBUG, "registering characteristic %s with " + "def_handle=%d val_handle=%d\n", + ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf), + ctxt->chr.def_handle, + ctxt->chr.val_handle); + break; + + case BLE_GATT_REGISTER_OP_DSC: + MODLOG_DFLT(DEBUG, "registering descriptor %s with handle=%d\n", + ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf), + ctxt->dsc.handle); + break; + + default: + assert(0); + break; + } +} + +int gatt_svr_init(void) +{ + int rc; + + rc = ble_gatts_count_cfg(gatt_svr_inc_svcs); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_add_svcs(gatt_svr_inc_svcs); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_count_cfg(gatt_svr_svcs); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_add_svcs(gatt_svr_svcs); + if (rc != 0) { + return rc; + } + + return 0; +} + +uint8_t tester_init_gatt(void) +{ + os_callout_init(¬ify_tx_timer, os_eventq_dflt_get(), + notify_test, NULL); + + return BTP_STATUS_SUCCESS; +} + +uint8_t tester_unregister_gatt(void) +{ + return BTP_STATUS_SUCCESS; +} diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index c19c0be89..98a78ff85 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -329,7 +329,7 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num) * the keys in database may get lost. */ static int -ble_nvs_restore_sec_keys() +ble_nvs_restore_sec_keys(void) { esp_err_t err; @@ -362,7 +362,7 @@ ble_nvs_restore_sec_keys() return 0; } -int ble_store_config_persist_cccds() +int ble_store_config_persist_cccds(void) { int nvs_count, nvs_idx; union ble_store_value val; @@ -388,7 +388,7 @@ int ble_store_config_persist_cccds() return 0; } -int ble_store_config_persist_peer_secs() +int ble_store_config_persist_peer_secs(void) { int nvs_count, nvs_idx; union ble_store_value val; @@ -414,7 +414,7 @@ int ble_store_config_persist_peer_secs() return 0; } -int ble_store_config_persist_our_secs() +int ble_store_config_persist_our_secs(void) { int nvs_count, nvs_idx; union ble_store_value val; @@ -440,7 +440,7 @@ int ble_store_config_persist_our_secs() return 0; } -void ble_store_config_conf_init() +void ble_store_config_conf_init(void) { esp_err_t err;