From 81c2323bfb7acfed5b61fdc22c19a92e6a8743f7 Mon Sep 17 00:00:00 2001 From: Piotr Narajowski Date: Tue, 4 Feb 2025 16:07:36 +0100 Subject: [PATCH 1/3] nimble/apps/bttester: Implement GATT Server BTP Service (ID 0x07) New BTP service is introduced. Most of the changes are just migration of code from btp_gatt.c and btp_gatt.h. Changes also include refactor of notifications sent by server and database initialization: - os_callout_init and functions/variables realted to it are removed. It was responsible for starting a timer when peer has subscribed for notifications/indications and sending notifications right after the timer has expired. An updated version of gatts_set_value command can be used to set values of one or more characteristics and handles sending notifications, indications and multiple handle value notifications - All predefined ble_gatt_svc_def structs will be registred on GATTS service registration with visibility set to invisible. BTP_GATTS_INITIALIZE_DATABASE command can be used to make chosen database visible. --- apps/bttester/src/btp/btp.h | 2 + apps/bttester/src/btp/btp_gatts.h | 142 ++++ apps/bttester/src/btp/bttester.h | 25 +- apps/bttester/src/btp_core.c | 13 + apps/bttester/src/btp_gap.c | 14 +- apps/bttester/src/btp_gatts.c | 1207 +++++++++++++++++++++++++++++ apps/bttester/src/main.c | 4 +- 7 files changed, 1389 insertions(+), 18 deletions(-) create mode 100644 apps/bttester/src/btp/btp_gatts.h create mode 100644 apps/bttester/src/btp_gatts.c diff --git a/apps/bttester/src/btp/btp.h b/apps/bttester/src/btp/btp.h index 17fe388767..49a8167437 100644 --- a/apps/bttester/src/btp/btp.h +++ b/apps/bttester/src/btp/btp.h @@ -31,6 +31,7 @@ #include "btp_gap.h" #include "btp_gatt.h" #include "btp_gattc.h" +#include "btp_gatts.h" #include "btp_l2cap.h" #include "btp_mesh.h" #include "btp_pacs.h" @@ -48,6 +49,7 @@ #define BTP_SERVICE_ID_L2CAP 3 #define BTP_SERVICE_ID_MESH 4 #define BTP_SERVICE_ID_GATTC 6 +#define BTP_SERVICE_ID_GATTS 7 #define BTP_SERVICE_ID_PACS 12 #define BTP_SERVICE_ID_BAP 14 diff --git a/apps/bttester/src/btp/btp_gatts.h b/apps/bttester/src/btp/btp_gatts.h new file mode 100644 index 0000000000..a73e87b51d --- /dev/null +++ b/apps/bttester/src/btp/btp_gatts.h @@ -0,0 +1,142 @@ +/* + * 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. + */ + +/* btp_gatts.h - Bluetooth tester GATT Server service headers */ + +/* + * Copyright (c) 2015-2016 Intel Corporation + * Copyright (C) 2025 Codecoup + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/* GATT Server Service */ +#define BTP_MAX_PTS_SVCS 1 +#define BTP_GATT_HL_MAX_CNT 16 +struct btp_notify_hlv { + uint16_t handle; + uint16_t len; +}; + +struct btp_gatt_service { + uint16_t start_handle; + uint16_t end_handle; + uint8_t uuid_length; + uint8_t uuid[0]; +} __packed; + +struct btp_gatt_included { + uint16_t included_handle; + struct btp_gatt_service service; +} __packed; + +struct btp_gatt_read_uuid_chr { + uint16_t handle; + uint8_t data[0]; +} __packed; + +struct btp_gatt_characteristic { + uint16_t characteristic_handle; + uint16_t value_handle; + uint8_t properties; + uint8_t uuid_length; + uint8_t uuid[0]; +} __packed; + +struct btp_gatt_descriptor { + uint16_t descriptor_handle; + uint8_t uuid_length; + uint8_t uuid[0]; +} __packed; + +struct btp_gatt_read_rp { + uint8_t att_response; + uint16_t data_length; + uint8_t data[0]; +} __packed; + +/* commands */ +#define BTP_GATTS_READ_SUPPORTED_COMMANDS 0x01 +struct btp_gatts_read_supported_commands_rp { + uint8_t data[0]; +} __packed; + +#define BTP_GATTS_INITIALIZE_DATABASE 0x02 +struct btp_gatts_initialize_database_cmd { + uint8_t id; + uint32_t flags; +} __packed; + +#define BTP_GATTS_GET_ATTRIBUTES 0x03 +struct btp_gatts_get_attributes_cmd { + uint16_t start_handle; + uint16_t end_handle; + uint8_t type_length; + uint8_t type[0]; +} __packed; +struct btp_gatts_get_attributes_rp { + uint8_t attrs_count; + uint8_t attrs[0]; +} __packed; +struct btp_gatts_attr { + uint16_t handle; + uint8_t permission; + uint8_t type_length; + uint8_t type[0]; +} __packed; + +#define BTP_GATTS_GET_ATTRIBUTE_VALUE 0x04 +struct btp_gatts_get_attribute_value_cmd { + ble_addr_t address; + uint16_t handle; +} __packed; +struct btp_gatts_get_attribute_value_rp { + uint8_t att_response; + uint16_t value_length; + uint8_t value[0]; +} __packed; + +#define BTP_GATTS_SET_CHRC_VALUE 0x05 +struct btp_gatts_set_chrc_value_cmd { + ble_addr_t address; + uint16_t count; + struct btp_notify_hlv hl[BTP_GATT_HL_MAX_CNT]; + uint8_t value[0]; +} __packed; + +#define BTP_GATTS_CHANGE_DATABASE 0x06 +struct btp_gatts_change_database_cmd { + uint16_t start_handle; + uint16_t end_handle; + uint8_t visibility; +} __packed; + +#define BTP_GATTS_START_SERVER 0x07 +struct btp_gatts_start_server_rp { + uint16_t db_attr_off; + uint8_t db_attr_cnt; +} __packed; + +/* GATTS events */ +#define BTP_GATTS_EV_ATTR_VALUE_CHANGED 0x81 +struct btp_gatts_attr_value_changed_ev { + uint16_t handle; + uint16_t data_length; + uint8_t data[0]; +} __packed; diff --git a/apps/bttester/src/btp/bttester.h b/apps/bttester/src/btp/bttester.h index ac56d8846d..a4e2a3f789 100644 --- a/apps/bttester/src/btp/bttester.h +++ b/apps/bttester/src/btp/bttester.h @@ -113,14 +113,15 @@ tester_unregister_gatt(void); int tester_gattc_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, uint8_t indication, struct os_mbuf *om); + 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); +tester_gatts_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); #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) uint8_t @@ -139,11 +140,17 @@ uint8_t tester_init_gatt_cl(void); uint8_t tester_unregister_gatt_cl(void); +uint8_t +tester_init_gatts(void); +uint8_t +tester_unregister_gatts(void); +uint8_t +register_database(void); void -gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg); +gatts_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg); int -gatt_svr_init(void); +gatts_svr_init(void); #if MYNEWT_VAL(BLE_AUDIO) uint8_t diff --git a/apps/bttester/src/btp_core.c b/apps/bttester/src/btp_core.c index 65f3491a19..20c562ac3d 100644 --- a/apps/bttester/src/btp_core.c +++ b/apps/bttester/src/btp_core.c @@ -58,6 +58,7 @@ supported_services(const void *cmd, uint16_t cmd_len, tester_set_bit(rp->data, BTP_SERVICE_ID_MESH); #endif /* MYNEWT_VAL(BLE_MESH) */ tester_set_bit(rp->data, BTP_SERVICE_ID_GATTC); + tester_set_bit(rp->data, BTP_SERVICE_ID_GATTS); *rsp_len = sizeof(*rp) + 2; @@ -70,6 +71,7 @@ register_service(const void *cmd, uint16_t cmd_len, { const struct btp_core_register_service_cmd *cp = cmd; uint8_t status; + int rc; /* invalid service */ if ((cp->id == BTP_SERVICE_ID_CORE) || (cp->id > BTP_SERVICE_ID_MAX)) { @@ -111,6 +113,14 @@ register_service(const void *cmd, uint16_t cmd_len, case BTP_SERVICE_ID_GATTC: status = tester_init_gatt_cl(); break; + case BTP_SERVICE_ID_GATTS: + rc = register_database(); + if (rc != 0) { + status = rc; + break; + } + status = tester_init_gatts(); + break; default: status = BTP_STATUS_FAILED; break; @@ -160,6 +170,9 @@ unregister_service(const void *cmd, uint16_t cmd_len, case BTP_SERVICE_ID_GATTC: status = tester_unregister_gatt_cl(); break; + case BTP_SERVICE_ID_GATTS: + status = tester_unregister_gatts(); + break; #if MYNEWT_VAL(BLE_ISO_BROADCAST_SOURCE) case BTP_SERVICE_ID_BAP: status = tester_unregister_bap(); diff --git a/apps/bttester/src/btp_gap.c b/apps/bttester/src/btp_gap.c index ef87b0154b..28b6f2a496 100644 --- a/apps/bttester/src/btp_gap.c +++ b/apps/bttester/src/btp_gap.c @@ -1436,13 +1436,13 @@ gap_event_cb(struct ble_gap_event *event, void *arg) event->subscribe.cur_notify, event->subscribe.prev_indicate, event->subscribe.cur_indicate); - tester_gatt_subscribe_ev(event->subscribe.conn_handle, - event->subscribe.attr_handle, - event->subscribe.reason, - event->subscribe.prev_notify, - event->subscribe.cur_notify, - event->subscribe.prev_indicate, - event->subscribe.cur_indicate); + tester_gatts_subscribe_ev(event->subscribe.conn_handle, + event->subscribe.attr_handle, + event->subscribe.reason, + event->subscribe.prev_notify, + event->subscribe.cur_notify, + event->subscribe.prev_indicate, + event->subscribe.cur_indicate); break; case BLE_GAP_EVENT_REPEAT_PAIRING: console_printf("repeat pairing event; conn_handle=%d " diff --git a/apps/bttester/src/btp_gatts.c b/apps/bttester/src/btp_gatts.c new file mode 100644 index 0000000000..0d44f514d0 --- /dev/null +++ b/apps/bttester/src/btp_gatts.c @@ -0,0 +1,1207 @@ +/* + * 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. + */ + +/* btp_gatts.c - Bluetooth GATT Server Service 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 "btp/btp.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_CHR_NOTIFY_ALT 0x0026 +#define PTS_CHR_READ_WRITE_AUTHOR 0x0027 +#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 uint16_t myconn_handle; +static uint16_t notify_handle; +static uint16_t notify_handle_alt; + +struct find_attr_data { + ble_uuid_any_t *uuid; + int attr_type; + void *ptr; + uint16_t handle; +}; + +struct notify_mult_cb_data { + size_t tuple_cnt; + uint16_t handles[BTP_GATT_HL_MAX_CNT]; +}; + +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_author_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 = BLE_UUID16_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_READ | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_NOTIFY | + BLE_GATT_CHR_F_INDICATE, + }, { + .uuid = PTS_UUID_DECLARE(PTS_CHR_NOTIFY_ALT), + .access_cb = gatt_svr_read_write_test, + .val_handle = ¬ify_handle_alt, + .flags = BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE | + BLE_GATT_CHR_F_NOTIFY | + BLE_GATT_CHR_F_INDICATE, + }, + { + .uuid = PTS_UUID_DECLARE(PTS_CHR_READ_WRITE_AUTHOR), + .access_cb = gatt_svr_read_write_author_test, + .flags = BLE_GATT_CHR_F_READ_AUTHOR | + BLE_GATT_CHR_F_READ | + BLE_GATT_CHR_F_WRITE_AUTHOR | + BLE_GATT_CHR_F_WRITE, + }, { + 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 const struct ble_gatt_svc_def *pts_db[] = { + gatt_svr_svcs, NULL}; + +static void +attr_value_changed_ev(uint16_t handle, struct os_mbuf *data) +{ + struct btp_gatts_attr_value_changed_ev *ev; + struct os_mbuf *buf = os_msys_get(0, 0); + + SYS_LOG_DBG(""); + + ev = os_mbuf_extend(buf, sizeof(*ev)); + if (!ev) { + return; + } + + ev->handle = htole16(handle); + ev->data_length = htole16(os_mbuf_len(data)); + os_mbuf_appendfrom(buf, data, 0, os_mbuf_len(data)); + + tester_event(BTP_SERVICE_ID_GATTS, BTP_GATTS_EV_ATTR_VALUE_CHANGED, + buf->om_data, buf->om_len); +} + +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; + } + + if (conn_handle == BLE_HS_CONN_HANDLE_NONE) { + return 0; + } + + 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: + case PTS_CHR_NOTIFY: + case PTS_CHR_NOTIFY_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_author_test(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg) +{ + uint16_t uuid16; + + uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); + assert(uuid16 != 0); + + switch (uuid16) { + case PTS_CHR_READ_WRITE_AUTHOR: + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + return BLE_ATT_ERR_INSUFFICIENT_AUTHOR; + } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + return BLE_ATT_ERR_INSUFFICIENT_AUTHOR; + } + 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 uint8_t +start_server(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + struct btp_gatts_start_server_rp *rp = rsp; + + SYS_LOG_DBG(""); + + ble_gatts_show_local(); + + ble_svc_gatt_changed(0x0001, 0xffff); + + rp->db_attr_off = 0; + rp->db_attr_cnt = 0; + + *rsp_len = sizeof(*rp); + + return BTP_STATUS_SUCCESS; +} + +/* 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 = le16toh(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; +} + +#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 uint8_t +get_attrs(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + const struct btp_gatts_get_attributes_cmd *cp = cmd; + struct btp_gatts_get_attributes_rp *rp = rsp; + struct btp_gatts_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]; + uint8_t status = BTP_STATUS_SUCCESS; + + SYS_LOG_DBG(""); + + if (!buf) { + return BTP_STATUS_FAILED; + } + + memset(str, 0, sizeof(str)); + memset(&uuid, 0, sizeof(uuid)); + start_handle = le16toh(cp->start_handle); + end_handle = le16toh(cp->end_handle); + + if (cp->type_length) { + if (btp2bt_uuid(cp->type, cp->type_length, &uuid)) { + status = BTP_STATUS_FAILED; + goto done; + } + + 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); + } + + 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 = os_mbuf_extend(buf, sizeof(*gatt_attr)); + if (!gatt_attr) { + status = BTP_STATUS_FAILED; + goto done; + } + gatt_attr->handle = htole16(entry->ha_handle_id); + gatt_attr->permission = flags_hs2btp(entry->ha_flags); + + if (entry->ha_uuid->type == BLE_UUID_TYPE_16) { + uint16_t uuid_val; + + gatt_attr->type_length = 2; + uuid_val = htole16(BLE_UUID16(entry->ha_uuid)->value); + if (os_mbuf_append(buf, &uuid_val, sizeof(uuid_val))) { + status = BTP_STATUS_FAILED; + goto done; + } + } else { + gatt_attr->type_length = 16; + if (os_mbuf_append(buf, BLE_UUID128(entry->ha_uuid)->value, + gatt_attr->type_length)) { + status = BTP_STATUS_FAILED; + goto done; + } + } + + count++; + + entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); + } + + rp->attrs_count = count; + os_mbuf_copydata(buf, 0, os_mbuf_len(buf), rp->attrs); + + *rsp_len = sizeof(*rp) + os_mbuf_len(buf); + +done: + os_mbuf_free_chain(buf); + return status; +} + +static uint8_t +get_attr_val(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + const struct btp_gatts_get_attribute_value_cmd *cp = cmd; + struct btp_gatts_get_attribute_value_rp *rp; + struct ble_gap_conn_desc conn; + struct os_mbuf *buf = os_msys_get(0, 0); + uint16_t handle = le16toh(cp->handle); + uint8_t out_att_err = 0; + int conn_status; + uint8_t status = BTP_STATUS_SUCCESS; + + conn_status = ble_gap_conn_find_by_addr(&cp->address, &conn); + + if (conn_status) { + rp = os_mbuf_extend(buf, sizeof(*rp)); + if (!rp) { + status = BTP_STATUS_FAILED; + goto free; + } + + 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); + + os_mbuf_copydata(buf, 0, os_mbuf_len(buf), rsp); + *rsp_len = os_mbuf_len(buf); + + goto free; + } else { + rp = os_mbuf_extend(buf, sizeof(*rp)); + if (!rp) { + status = BTP_STATUS_FAILED; + goto free; + } + + 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); + + os_mbuf_copydata(buf, 0, os_mbuf_len(buf), rsp); + *rsp_len = os_mbuf_len(buf); + + goto free; + } + +free: + os_mbuf_free_chain(buf); + return status; +} + +static int +notify_multiple(uint16_t conn_handle, void *arg) +{ + struct notify_mult_cb_data *notify_data = + (struct notify_mult_cb_data *) arg; + int rc; + + SYS_LOG_DBG("") + + rc = ble_gatts_notify_multiple(conn_handle, + notify_data->tuple_cnt, + notify_data->handles); + + return rc; +} + +static uint8_t +set_values(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + const struct btp_gatts_set_chrc_value_cmd *cp = cmd; + struct os_mbuf *buf; + struct notify_mult_cb_data cb_data; + uint16_t conn_handle; + uint16_t val_idx = 0; + uint16_t handle; + uint16_t len; + uint8_t cccd_value; + int i; + int rc = 0; + + for (i = 0; i < cp->count; i++) { + buf = ble_hs_mbuf_att_pkt(); + handle = cp->hl[i].handle; + len = cp->hl[i].len; + os_mbuf_append(buf, cp->value + val_idx, len); + ble_att_svr_write_local(handle, buf); + val_idx += len; + cb_data.handles[i] = handle; + } + + if (cp->count == 1) { + rc = ble_gatts_read_cccd(myconn_handle, handle, &cccd_value); + + if (rc != 0) { + return BTP_STATUS_FAILED; + } + + if (cccd_value == BLE_GATT_CCCD_NOTIFY) { + rc = ble_gatts_notify_custom(myconn_handle, handle, buf); + } + if (cccd_value == BLE_GATT_CCCD_INDICATE) { + rc = ble_gatts_indicate_custom(myconn_handle, handle, buf); + } + } else { + cb_data.tuple_cnt = cp->count; + if (ble_addr_cmp(&cp->address, BLE_ADDR_ANY)) { + ble_gap_conn_foreach_handle(notify_multiple, &cb_data); + } else { + ble_gap_conn_find_handle_by_addr(&cp->address, &conn_handle); + rc = notify_multiple(conn_handle, &cb_data); + } + } + + if (rc != 0) { + return BTP_STATUS_FAILED; + } + + return BTP_STATUS_SUCCESS; +} + +static uint8_t +change_database(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + const struct btp_gatts_change_database_cmd *cp = cmd; + + SYS_LOG_DBG("") + + ble_gatts_show_local(); + + ble_svc_gatt_changed(cp->start_handle, cp->end_handle); + + return BTP_STATUS_SUCCESS; +} + +uint8_t +set_visibility(uint8_t db_id, int visible) +{ + int i, j, rc; + uint16_t handle; + + for (i = 0; pts_db[db_id][i].type != 0 ; i++) { + rc = ble_gatts_find_svc(pts_db[db_id][i].uuid, &handle); + if (rc != 0) { + return rc; + } + rc = ble_gatts_svc_set_visibility(handle, visible); + if (rc != 0) { + return rc; + } + + if (!pts_db[db_id][i].includes) { + continue; + } + + if (pts_db[db_id][i].includes) { + for (j = 0; pts_db[db_id][i].includes[j] != NULL; j++) { + rc = ble_gatts_find_svc(pts_db[db_id][i].includes[j]->uuid, + &handle); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_svc_set_visibility(handle, visible); + if (rc != 0) { + return rc; + } + } + } + } + + return 0; +} + +static uint8_t +initialize_database(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + const struct btp_gatts_initialize_database_cmd *cp = cmd; + int rc; + + rc = set_visibility(cp->id, 1); + if (rc != 0) { + return BTP_STATUS_FAILED; + } + + return BTP_STATUS_SUCCESS; +} + +static uint8_t +supported_commands(const void *cmd, uint16_t cmd_len, + void *rsp, uint16_t *rsp_len) +{ + struct btp_gatts_read_supported_commands_rp *rp = rsp; + + /* octet 0 */ + tester_set_bit(rp->data, BTP_GATTS_READ_SUPPORTED_COMMANDS); + tester_set_bit(rp->data, BTP_GATTS_INITIALIZE_DATABASE); + tester_set_bit(rp->data, BTP_GATTS_GET_ATTRIBUTES); + tester_set_bit(rp->data, BTP_GATTS_GET_ATTRIBUTE_VALUE); + tester_set_bit(rp->data, BTP_GATTS_SET_CHRC_VALUE); + tester_set_bit(rp->data, BTP_GATTS_CHANGE_DATABASE); + tester_set_bit(rp->data, BTP_GATTS_START_SERVER); + *rsp_len = sizeof(*rp) + 4; + + return BTP_STATUS_SUCCESS; +} + +enum attr_type { + BLE_GATT_ATTR_SVC = 0, + BLE_GATT_ATTR_CHR, + BLE_GATT_ATTR_DSC, +}; + +static const struct btp_handler handlers[] = { + { + .opcode = BTP_GATTS_READ_SUPPORTED_COMMANDS, + .index = BTP_INDEX_NONE, + .expect_len = 0, + .func = supported_commands, + }, + { + .opcode = BTP_GATTS_INITIALIZE_DATABASE, + .expect_len = sizeof(struct btp_gatts_initialize_database_cmd), + .func = initialize_database, + }, + { + .opcode = BTP_GATTS_GET_ATTRIBUTES, + .expect_len = BTP_HANDLER_LENGTH_VARIABLE, + .func = get_attrs, + }, + { + .opcode = BTP_GATTS_GET_ATTRIBUTE_VALUE, + .expect_len = sizeof(struct btp_gatts_get_attribute_value_cmd), + .func = get_attr_val, + }, + { + .opcode = BTP_GATTS_CHANGE_DATABASE, + .expect_len = sizeof(struct btp_gatts_change_database_cmd), + .func = change_database, + }, + { + .opcode = BTP_GATTS_SET_CHRC_VALUE, + .expect_len = BTP_HANDLER_LENGTH_VARIABLE, + .func = set_values, + }, + { + .opcode = BTP_GATTS_START_SERVER, + .expect_len = 0, + .func = start_server, + }, +}; + +static struct bt_gatt_subscribe_params { + uint16_t ccc_handle; + uint16_t value; + uint16_t value_handle; +} subscribe_params; + +uint8_t +register_database(void) +{ + int rc; + int i; + + for (i = 0; i < BTP_MAX_PTS_SVCS; i++) { + rc = set_visibility(i, 0); + + if (rc != 0) { + return BTP_STATUS_FAILED; + } + } + + return BTP_STATUS_SUCCESS; +} + +int +tester_gatts_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; conn_handle=%d\n", conn_handle); + memset(&subscribe_params, 0, sizeof(subscribe_params)); + return 0; + } + + if (cur_notify) { + SYS_LOG_INF("Subscribed to notifications; conn_handle=%d, " + "attr_handle=%d\n", conn_handle, attr_handle); + } + + if (cur_indicate) { + SYS_LOG_INF("Subscribed to indications; conn_handle=%d, " + "attr_handle=%d\n", conn_handle, attr_handle); + } + + return 0; +} + +void +gatts_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 +gatts_svr_init(void) +{ + int rc; + int i; + + for (i = 0; i < BTP_MAX_PTS_SVCS; i++) { + rc = ble_gatts_count_cfg(pts_db[i][0].includes[0]); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_add_svcs(pts_db[i][0].includes[0]); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_count_cfg(pts_db[i]); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_add_svcs(pts_db[i]); + if (rc != 0) { + return rc; + } + } + + return 0; +} + + +uint8_t +tester_init_gatts(void) +{ + tester_register_command_handlers(BTP_SERVICE_ID_GATTS, handlers, + ARRAY_SIZE(handlers)); + + return BTP_STATUS_SUCCESS; +} + +uint8_t +tester_unregister_gatts(void) +{ + return BTP_STATUS_SUCCESS; +} diff --git a/apps/bttester/src/main.c b/apps/bttester/src/main.c index 242133056e..6665774957 100644 --- a/apps/bttester/src/main.c +++ b/apps/bttester/src/main.c @@ -58,10 +58,10 @@ mynewt_main(int argc, char **argv) /* Initialize the NimBLE host configuration. */ ble_hs_cfg.reset_cb = on_reset; ble_hs_cfg.sync_cb = on_sync; - ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb, + ble_hs_cfg.gatts_register_cb = gatts_svr_register_cb, ble_hs_cfg.store_status_cb = ble_store_util_status_rr; - rc = gatt_svr_init(); + rc = gatts_svr_init(); assert(rc == 0); while (1) { From 852986e980cae2fc305d4d652653bbc21b29ad12 Mon Sep 17 00:00:00 2001 From: Piotr Narajowski Date: Mon, 16 Jun 2025 11:37:57 +0200 Subject: [PATCH 2/3] nimble/apps/bttester: Remove deprecated BTP GATT service - Removes files and content related to BTP GATT Service (0x02). - All necessary content has been migrated to btp_gatts.c and btp_gatts.h files - Ensured no loss of functionality; all essential GATT components preserved --- apps/bttester/src/btp/btp.h | 4 +- apps/bttester/src/btp/btp_gatt.h | 339 ----- apps/bttester/src/btp/bttester.h | 4 - apps/bttester/src/btp_core.c | 7 - apps/bttester/src/btp_gatt.c | 2307 ------------------------------ 5 files changed, 1 insertion(+), 2660 deletions(-) delete mode 100644 apps/bttester/src/btp/btp_gatt.h delete mode 100644 apps/bttester/src/btp_gatt.c diff --git a/apps/bttester/src/btp/btp.h b/apps/bttester/src/btp/btp.h index 49a8167437..1d02648a69 100644 --- a/apps/bttester/src/btp/btp.h +++ b/apps/bttester/src/btp/btp.h @@ -29,9 +29,8 @@ #include "bttester.h" #include "btp_core.h" #include "btp_gap.h" -#include "btp_gatt.h" -#include "btp_gattc.h" #include "btp_gatts.h" +#include "btp_gattc.h" #include "btp_l2cap.h" #include "btp_mesh.h" #include "btp_pacs.h" @@ -45,7 +44,6 @@ #define BTP_SERVICE_ID_CORE 0 #define BTP_SERVICE_ID_GAP 1 -#define BTP_SERVICE_ID_GATT 2 #define BTP_SERVICE_ID_L2CAP 3 #define BTP_SERVICE_ID_MESH 4 #define BTP_SERVICE_ID_GATTC 6 diff --git a/apps/bttester/src/btp/btp_gatt.h b/apps/bttester/src/btp/btp_gatt.h deleted file mode 100644 index 6c57ade092..0000000000 --- a/apps/bttester/src/btp/btp_gatt.h +++ /dev/null @@ -1,339 +0,0 @@ -/* - * 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. - */ - -/* btp_gatt.h - Bluetooth tester GATT service headers */ - -/* - * Copyright (c) 2015-2016 Intel Corporation - * Copyright (C) 2023 Codecoup - * - * SPDX-License-Identifier: Apache-2.0 - */ - -/* GATT Service */ -/* commands */ -#define BTP_GATT_READ_SUPPORTED_COMMANDS 0x01 -struct btp_gatt_read_supported_commands_rp { - uint8_t data[0]; -} __packed; - -#define BTP_GATT_SERVICE_PRIMARY 0x00 -#define BTP_GATT_SERVICE_SECONDARY 0x01 - -#define BTP_GATT_ADD_SERVICE 0x02 -struct btp_gatt_add_service_cmd { - uint8_t type; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; -struct btp_gatt_add_service_rp { - uint16_t svc_id; -} __packed; - -#define BTP_GATT_ADD_CHARACTERISTIC 0x03 -struct btp_gatt_add_characteristic_cmd { - uint16_t svc_id; - uint8_t properties; - uint8_t permissions; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; -struct btp_gatt_add_characteristic_rp { - uint16_t char_id; -} __packed; - -#define BTP_GATT_ADD_DESCRIPTOR 0x04 -struct btp_gatt_add_descriptor_cmd { - uint16_t char_id; - uint8_t permissions; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; -struct btp_gatt_add_descriptor_rp { - uint16_t desc_id; -} __packed; - -#define BTP_GATT_ADD_INCLUDED_SERVICE 0x05 -struct btp_gatt_add_included_service_cmd { - uint16_t svc_id; -} __packed; -struct btp_gatt_add_included_service_rp { - uint16_t included_service_id; -} __packed; - -#define BTP_GATT_SET_VALUE 0x06 -struct btp_gatt_set_value_cmd { - uint16_t attr_id; - uint16_t len; - uint8_t value[0]; -} __packed; - -#define BTP_GATT_START_SERVER 0x07 -struct btp_gatt_start_server_rp { - uint16_t db_attr_off; - uint8_t db_attr_cnt; -} __packed; - -#define BTP_GATT_SET_ENC_KEY_SIZE 0x09 -struct btp_gatt_set_enc_key_size_cmd { - uint16_t attr_id; - uint8_t key_size; -} __packed; - -struct btp_gatt_service { - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; - -struct btp_gatt_included { - uint16_t included_handle; - struct btp_gatt_service service; -} __packed; - -struct btp_gatt_read_uuid_chr { - uint16_t handle; - uint8_t data[0]; -} __packed; - -struct btp_gatt_characteristic { - uint16_t characteristic_handle; - uint16_t value_handle; - uint8_t properties; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; - -struct btp_gatt_descriptor { - uint16_t descriptor_handle; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; - -#define BTP_GATT_EXCHANGE_MTU 0x0a -struct btp_gatt_exchange_mtu_cmd { - ble_addr_t address; -} __packed; - -#define BTP_GATT_DISC_ALL_PRIM_SVCS 0x0b -struct btp_gatt_disc_all_prim_svcs_cmd { - ble_addr_t address; -} __packed; -struct btp_gatt_disc_all_prim_svcs_rp { - uint8_t services_count; - struct btp_gatt_service services[0]; -} __packed; - -#define BTP_GATT_DISC_PRIM_UUID 0x0c -struct btp_gatt_disc_prim_uuid_cmd { - ble_addr_t address; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; -struct btp_gatt_disc_prim_uuid_rp { - uint8_t services_count; - struct btp_gatt_service services[0]; -} __packed; - -#define BTP_GATT_FIND_INCLUDED 0x0d -struct btp_gatt_find_included_cmd { - ble_addr_t address; - uint16_t start_handle; - uint16_t end_handle; -} __packed; -struct btp_gatt_find_included_rp { - uint8_t services_count; - struct btp_gatt_included included[0]; -} __packed; - -#define BTP_GATT_DISC_ALL_CHRC 0x0e -struct btp_gatt_disc_all_chrc_cmd { - ble_addr_t address; - uint16_t start_handle; - uint16_t end_handle; -} __packed; -struct btp_gatt_disc_chrc_rp { - uint8_t characteristics_count; - struct btp_gatt_characteristic characteristics[0]; -} __packed; - -#define BTP_GATT_DISC_CHRC_UUID 0x0f -struct btp_gatt_disc_chrc_uuid_cmd { - ble_addr_t address; - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; - -#define BTP_GATT_DISC_ALL_DESC 0x10 -struct btp_gatt_disc_all_desc_cmd { - ble_addr_t address; - uint16_t start_handle; - uint16_t end_handle; -} __packed; -struct btp_gatt_disc_all_desc_rp { - uint8_t descriptors_count; - struct btp_gatt_descriptor descriptors[0]; -} __packed; - -#define BTP_GATT_READ 0x11 -struct btp_gatt_read_cmd { - ble_addr_t address; - uint16_t handle; -} __packed; -struct btp_gatt_read_rp { - uint8_t att_response; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_READ_UUID 0x12 -struct btp_gatt_read_uuid_cmd { - ble_addr_t address; - uint16_t start_handle; - uint16_t end_handle; - uint8_t uuid_length; - uint8_t uuid[0]; -} __packed; - -#define BTP_GATT_READ_LONG 0x13 -struct btp_gatt_read_long_cmd { - ble_addr_t address; - uint16_t handle; - uint16_t offset; -} __packed; - -#define BTP_GATT_READ_MULTIPLE 0x14 -struct btp_gatt_read_multiple_cmd { - ble_addr_t address; - uint8_t handles_count; - uint16_t handles[0]; -} __packed; - -#define BTP_GATT_WRITE_WITHOUT_RSP 0x15 -struct btp_gatt_write_without_rsp_cmd { - ble_addr_t address; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_SIGNED_WRITE_WITHOUT_RSP 0x16 -struct btp_gatt_signed_write_without_rsp_cmd { - ble_addr_t address; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_WRITE 0x17 -struct btp_gatt_write_cmd { - ble_addr_t address; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_WRITE_LONG 0x18 -struct btp_gatt_write_long_cmd { - ble_addr_t address; - uint16_t handle; - uint16_t offset; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_RELIABLE_WRITE 0x19 -struct btp_gatt_reliable_write_cmd { - ble_addr_t address; - uint16_t handle; - uint16_t offset; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_CFG_NOTIFY 0x1a -#define BTP_GATT_CFG_INDICATE 0x1b -struct btp_gatt_cfg_notify_cmd { - ble_addr_t address; - uint8_t enable; - uint16_t ccc_handle; -} __packed; - -#define BTP_GATT_GET_ATTRIBUTES 0x1c -struct btp_gatt_get_attributes_cmd { - uint16_t start_handle; - uint16_t end_handle; - uint8_t type_length; - uint8_t type[0]; -} __packed; -struct btp_gatt_get_attributes_rp { - uint8_t attrs_count; - uint8_t attrs[0]; -} __packed; -struct btp_gatt_attr { - uint16_t handle; - uint8_t permission; - uint8_t type_length; - uint8_t type[0]; -} __packed; - -#define BTP_GATT_GET_ATTRIBUTE_VALUE 0x1d -struct btp_gatt_get_attribute_value_cmd { - ble_addr_t address; - uint16_t handle; -} __packed; -struct btp_gatt_get_attribute_value_rp { - uint8_t att_response; - uint16_t value_length; - uint8_t value[0]; -} __packed; - -#define BTP_GATT_CHANGE_DATABASE 0x1e -struct btp_gatt_change_database_cmd { - uint16_t start_handle; - uint16_t end_handle; - uint8_t visibility; -} __packed; - -#define BTP_GATT_NOTIFY_MULTIPLE 0x21 -struct btp_gatt_notify_mult_val_cmd { - ble_addr_t addr; - uint16_t count; - uint16_t handles[0]; -} __packed; - -/* GATT events */ -#define BTP_GATT_EV_NOTIFICATION 0x80 -struct btp_gatt_notification_ev { - ble_addr_t address; - uint8_t type; - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; -} __packed; - -#define BTP_GATT_EV_ATTR_VALUE_CHANGED 0x81 -struct btp_gatt_attr_value_changed_ev { - uint16_t handle; - uint16_t data_length; - uint8_t data[0]; -} __packed; diff --git a/apps/bttester/src/btp/bttester.h b/apps/bttester/src/btp/bttester.h index a4e2a3f789..59bb055b54 100644 --- a/apps/bttester/src/btp/bttester.h +++ b/apps/bttester/src/btp/bttester.h @@ -106,10 +106,6 @@ uint8_t tester_unregister_gap(void); void tester_init_core(void); -uint8_t -tester_init_gatt(void); -uint8_t -tester_unregister_gatt(void); int tester_gattc_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, uint8_t indication, struct os_mbuf *om); diff --git a/apps/bttester/src/btp_core.c b/apps/bttester/src/btp_core.c index 20c562ac3d..5f4dc1a3cc 100644 --- a/apps/bttester/src/btp_core.c +++ b/apps/bttester/src/btp_core.c @@ -50,7 +50,6 @@ supported_services(const void *cmd, uint16_t cmd_len, /* octet 0 */ tester_set_bit(rp->data, BTP_SERVICE_ID_CORE); tester_set_bit(rp->data, BTP_SERVICE_ID_GAP); - tester_set_bit(rp->data, BTP_SERVICE_ID_GATT); #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) tester_set_bit(rp->data, BTP_SERVICE_ID_L2CAP); #endif /* MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) */ @@ -87,9 +86,6 @@ register_service(const void *cmd, uint16_t cmd_len, case BTP_SERVICE_ID_GAP: status = tester_init_gap(); break; - case BTP_SERVICE_ID_GATT: - status = tester_init_gatt(); - break; #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) case BTP_SERVICE_ID_L2CAP: status = tester_init_l2cap(); @@ -154,9 +150,6 @@ unregister_service(const void *cmd, uint16_t cmd_len, case BTP_SERVICE_ID_GAP: status = tester_unregister_gap(); break; - case BTP_SERVICE_ID_GATT: - status = tester_unregister_gatt(); - break; #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) case BTP_SERVICE_ID_L2CAP: status = tester_unregister_l2cap(); diff --git a/apps/bttester/src/btp_gatt.c b/apps/bttester/src/btp_gatt.c deleted file mode 100644 index 17c083bf3a..0000000000 --- a/apps/bttester/src/btp_gatt.c +++ /dev/null @@ -1,2307 +0,0 @@ -/* - * 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 "btp/btp.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_CHR_NOTIFY_ALT 0x0026 -#define PTS_CHR_READ_WRITE_AUTHOR 0x0027 -#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; -uint16_t notify_handle_alt; -uint8_t notify_value = 90; - -struct find_attr_data { - ble_uuid_any_t *uuid; - int attr_type; - void *ptr; - uint16_t handle; -}; - -struct notify_mult_cb_data { - size_t tuple_cnt; - uint16_t handles[8]; -}; - -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_author_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 = BLE_UUID16_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_READ_WRITE_AUTHOR), - .access_cb = gatt_svr_read_write_author_test, - .flags = BLE_GATT_CHR_F_READ_AUTHOR | - BLE_GATT_CHR_F_READ | - BLE_GATT_CHR_F_WRITE_AUTHOR | - BLE_GATT_CHR_F_WRITE - }, { - .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_READ | - BLE_GATT_CHR_F_WRITE | - BLE_GATT_CHR_F_NOTIFY | - BLE_GATT_CHR_F_INDICATE, - }, { - .uuid = PTS_UUID_DECLARE(PTS_CHR_NOTIFY_ALT), - .access_cb = gatt_svr_read_write_test, - .val_handle = ¬ify_handle_alt, - .flags = BLE_GATT_CHR_F_READ | - BLE_GATT_CHR_F_WRITE | - 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 btp_gatt_attr_value_changed_ev *ev; - struct os_mbuf *buf = os_msys_get(0, 0); - - SYS_LOG_DBG(""); - - ev = os_mbuf_extend(buf, sizeof(*ev)); - if (!ev) { - return; - } - - ev->handle = htole16(handle); - ev->data_length = htole16(os_mbuf_len(data)); - os_mbuf_appendfrom(buf, data, 0, os_mbuf_len(data)); - - tester_event(BTP_SERVICE_ID_GATT, BTP_GATT_EV_ATTR_VALUE_CHANGED, - buf->om_data, buf->om_len); -} - -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: - case PTS_CHR_NOTIFY: - case PTS_CHR_NOTIFY_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_author_test(uint16_t conn_handle, uint16_t attr_handle, - struct ble_gatt_access_ctxt *ctxt, - void *arg) -{ - uint16_t uuid16; - - uuid16 = extract_uuid16_from_pts_uuid128(ctxt->chr->uuid); - assert(uuid16 != 0); - - switch (uuid16) { - case PTS_CHR_READ_WRITE_AUTHOR: - if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { - return BLE_ATT_ERR_INSUFFICIENT_AUTHOR; - } else if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { - return BLE_ATT_ERR_INSUFFICIENT_AUTHOR; - } - 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 uint8_t -start_server(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - struct btp_gatt_start_server_rp *rp = rsp; - - SYS_LOG_DBG(""); - - ble_gatts_show_local(); - - ble_svc_gatt_changed(0x0001, 0xffff); - - rp->db_attr_off = 0; - rp->db_attr_cnt = 0; - - *rsp_len = sizeof(*rp); - - return BTP_STATUS_SUCCESS; -} - -/* 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 = le16toh(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() -{ - 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 btp_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_rsp_full(BTP_SERVICE_ID_GATT, btp_opcode, - 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, - BTP_STATUS_FAILED); - read_destroy(); - return 0; - } - - rp->data_length += attr->om->om_len; - tester_rsp_full(BTP_SERVICE_ID_GATT, btp_opcode, - gatt_buf.buf, gatt_buf.len); - read_destroy(); - - return 0; -} - -static uint8_t -read_data(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_read_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - /* Clear buffer */ - read_destroy(); - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_read_rp))) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_read(conn.conn_handle, le16toh(cp->handle), - read_cb, (void *) BTP_GATT_READ)) { - read_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_SUCCESS; -} - -static int -read_long_cb(uint16_t conn_handle, - const struct ble_gatt_error *error, - struct ble_gatt_attr *attr, - void *arg) -{ - struct btp_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_rsp_full(BTP_SERVICE_ID_GATT, btp_opcode, - gatt_buf.buf, gatt_buf.len); - read_destroy(); - return 0; - } - - if (error->status == BLE_HS_EDONE) { - tester_rsp_full(BTP_SERVICE_ID_GATT, btp_opcode, - 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, - BTP_STATUS_FAILED); - read_destroy(); - return BLE_HS_ENOMEM; - } - - rp->data_length += attr->om->om_len; - - return 0; -} - -static uint8_t -read_long(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_read_long_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - /* Clear buffer */ - read_destroy(); - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_read_rp))) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_read_long(conn.conn_handle, - le16toh(cp->handle), - le16toh(cp->offset), - read_long_cb, (void *) BTP_GATT_READ_LONG)) { - read_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_SUCCESS; -} - -static uint8_t -read_multiple(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_read_multiple_cmd *cp = cmd; - uint16_t handles[cp->handles_count]; - struct ble_gap_conn_desc conn; - int rc, i; - - SYS_LOG_DBG(""); - - for (i = 0; i < ARRAY_SIZE(handles); i++) { - handles[i] = le16toh(cp->handles[i]); - } - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - /* Clear buffer */ - read_destroy(); - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_read_rp))) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_read_mult(conn.conn_handle, handles, - cp->handles_count, read_cb, - (void *) BTP_GATT_READ_MULTIPLE)) { - read_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_SUCCESS; -} - -static uint8_t -write_without_rsp(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_write_without_rsp_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - int rc; - - SYS_LOG_DBG(""); - - if (cmd_len < sizeof(*cp) || - cmd_len != sizeof(*cp) + le16toh(cp->data_length)) { - return BTP_STATUS_FAILED; - } - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_write_no_rsp_flat(conn.conn_handle, - le16toh(cp->handle), cp->data, - le16toh(cp->data_length))) { - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_SUCCESS; -} - -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_rsp_full(BTP_SERVICE_ID_GATT, btp_opcode, - &err, sizeof(err)); - return 0; -} - -static uint8_t -write_data(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_write_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - int rc; - - SYS_LOG_DBG(""); - - if (cmd_len < sizeof(*cp) || - cmd_len != sizeof(*cp) + le16toh(cp->data_length)) { - return BTP_STATUS_FAILED; - } - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_write_flat(conn.conn_handle, le16toh(cp->handle), - cp->data, le16toh(cp->data_length), - write_rsp, (void *) BTP_GATT_WRITE)) { - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -static uint8_t -write_long(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_write_long_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - struct os_mbuf *om = NULL; - int rc = 0; - - SYS_LOG_DBG(""); - - if (cmd_len < sizeof(*cp) || - cmd_len != sizeof(*cp) + le16toh(cp->data_length)) { - goto fail; - } - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - goto fail; - } - - om = ble_hs_mbuf_from_flat(cp->data, le16toh(cp->data_length)); - if (!om) { - SYS_LOG_ERR("Insufficient resources"); - goto fail; - } - - rc = ble_gattc_write_long(conn.conn_handle, - le16toh(cp->handle), - le16toh(cp->offset), - om, write_rsp, - (void *) BTP_GATT_WRITE_LONG); - if (!rc) { - return BTP_STATUS_DELAY_REPLY; - } - -fail: - SYS_LOG_ERR("Failed to send Write Long request, rc=%d", rc); - os_mbuf_free_chain(om); - return 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_rsp_full(BTP_SERVICE_ID_GATT, BTP_GATT_RELIABLE_WRITE, - &err, sizeof(err)); - return 0; -} - -static uint8_t -reliable_write(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_reliable_write_cmd *cp = cmd; - 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(&cp->address, &conn); - if (rc) { - goto fail; - } - - om = ble_hs_mbuf_from_flat(cp->data, le16toh(cp->data_length)); - /* This is required, because Nimble checks if - * the data is longer than offset - */ - if (os_mbuf_extend(om, le16toh(cp->offset) + 1) == NULL) { - goto fail; - } - - attr.handle = le16toh(cp->handle); - attr.offset = le16toh(cp->offset); - attr.om = om; - - if (ble_gattc_write_reliable(conn.conn_handle, &attr, 1, - reliable_write_rsp, NULL)) { - goto fail; - } - - return BTP_STATUS_SUCCESS; - -fail: - os_mbuf_free_chain(om); - - return BTP_STATUS_FAILED; -} - -static struct bt_gatt_subscribe_params { - uint16_t ccc_handle; - uint16_t value; - uint16_t value_handle; -} subscribe_params; - -static uint8_t -read_uuid(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_read_uuid_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - ble_uuid_any_t uuid; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (btp2bt_uuid(cp->uuid, cp->uuid_length, &uuid)) { - return BTP_STATUS_FAILED; - } - - /* Clear buffer */ - read_destroy(); - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_read_rp))) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_read_by_uuid(conn.conn_handle, - le16toh(cp->start_handle), - le16toh(cp->end_handle), &uuid.u, - read_long_cb, (void *) BTP_GATT_READ_UUID)) { - read_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_SUCCESS; -} - -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 btp_gatt_disc_prim_uuid_rp *rp = (void *) gatt_buf.buf; - struct btp_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, - BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } - - if (error->status == BLE_HS_EDONE) { - tester_rsp_full(BTP_SERVICE_ID_GATT, opcode, - 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, - BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } - - service->start_handle = htole16(gatt_svc->start_handle); - service->end_handle = htole16(gatt_svc->end_handle); - service->uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = htole16(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 btp_gatt_disc_all_desc_rp *rp = (void *) gatt_buf.buf; - struct btp_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, BTP_GATT_DISC_ALL_DESC, - BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } - - if (error->status == BLE_HS_EDONE) { - tester_rsp_full(BTP_SERVICE_ID_GATT, BTP_GATT_DISC_ALL_DESC, - 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, BTP_GATT_DISC_ALL_DESC, - BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } - - dsc->descriptor_handle = htole16(gatt_dsc->handle); - dsc->uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = htole16(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 uint8_t -disc_all_prim_svcs(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_disc_all_prim_svcs_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_disc_all_prim_svcs_rp))) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_disc_all_svcs(conn.conn_handle, disc_prim_uuid_cb, - (void *) BTP_GATT_DISC_ALL_PRIM_SVCS)) { - discover_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -static uint8_t -disc_all_desc(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_disc_all_desc_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_disc_all_desc_rp))) { - return BTP_STATUS_FAILED; - } - - start_handle = le16toh(cp->start_handle) - 1; - end_handle = le16toh(cp->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(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -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 btp_gatt_find_included_rp *rp = (void *) gatt_buf.buf; - struct btp_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, BTP_GATT_FIND_INCLUDED, - BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } - - if (error->status == BLE_HS_EDONE) { - tester_rsp_full(BTP_SERVICE_ID_GATT, BTP_GATT_FIND_INCLUDED, - 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, BTP_GATT_FIND_INCLUDED, - BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } - - included->included_handle = htole16(service_handle + 1 + - rp->services_count); - included->service.start_handle = htole16(gatt_svc->start_handle); - included->service.end_handle = htole16(gatt_svc->end_handle); - included->service.uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = htole16(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 btp_gatt_disc_chrc_rp *rp = (void *) gatt_buf.buf; - struct btp_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, - BTP_STATUS_FAILED); - discover_destroy(); - return 0; - } - - if (error->status == BLE_HS_EDONE) { - tester_rsp_full(BTP_SERVICE_ID_GATT, btp_opcode, - 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, - BTP_STATUS_FAILED); - discover_destroy(); - return BLE_HS_ENOMEM; - } - - chrc->characteristic_handle = htole16(gatt_chr->def_handle); - chrc->properties = gatt_chr->properties; - chrc->value_handle = htole16(gatt_chr->val_handle); - chrc->uuid_length = uuid_length; - - if (uuid->u.type == BLE_UUID_TYPE_16) { - uint16_t u16 = htole16(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 uint8_t -disc_chrc_uuid(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_disc_chrc_uuid_cmd *cp = cmd; - 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(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (btp2bt_uuid(cp->uuid, cp->uuid_length, &uuid)) { - return BTP_STATUS_FAILED; - } - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_disc_chrc_rp))) { - return BTP_STATUS_FAILED; - } - - start_handle = le16toh(cp->start_handle); - end_handle = le16toh(cp->end_handle); - - if (ble_gattc_disc_chrs_by_uuid(conn.conn_handle, start_handle, - end_handle, &uuid.u, disc_chrc_cb, - (void *) BTP_GATT_DISC_CHRC_UUID)) { - discover_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -static uint8_t -disc_prim_uuid(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_disc_prim_uuid_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - ble_uuid_any_t uuid; - int rc; - - SYS_LOG_DBG(""); - - if ((cmd_len < sizeof(*cp)) || - (cmd_len != sizeof(*cp) + cp->uuid_length)) { - return BTP_STATUS_FAILED; - } - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (btp2bt_uuid(cp->uuid, cp->uuid_length, &uuid)) { - return BTP_STATUS_FAILED; - } - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_disc_prim_uuid_rp))) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_disc_svc_by_uuid(conn.conn_handle, - &uuid.u, disc_prim_uuid_cb, - (void *) BTP_GATT_DISC_PRIM_UUID)) { - discover_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -static uint8_t -disc_all_chrc(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_disc_all_chrc_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - uint16_t start_handle, end_handle; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - SYS_LOG_DBG("Conn find failed"); - return BTP_STATUS_FAILED; - } - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_disc_chrc_rp))) { - SYS_LOG_DBG("Buf reserve failed"); - return BTP_STATUS_FAILED; - } - - start_handle = le16toh(cp->start_handle); - end_handle = le16toh(cp->end_handle); - - rc = ble_gattc_disc_all_chrs(conn.conn_handle, start_handle, end_handle, - disc_chrc_cb, (void *) BTP_GATT_DISC_ALL_CHRC); - if (rc) { - discover_destroy(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -static uint8_t -find_included(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_find_included_cmd *cp = cmd; - 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(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (!gatt_buf_reserve(sizeof(struct btp_gatt_find_included_rp))) { - return BTP_STATUS_FAILED; - } - - start_handle = le16toh(cp->start_handle); - end_handle = le16toh(cp->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(); - return BTP_STATUS_FAILED; - } - - return BTP_STATUS_DELAY_REPLY; -} - -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) { - SYS_LOG_DBG("MTU exchange failed"); - - return 0; - } - - SYS_LOG_DBG("MTU exchange succeed"); - - return 0; -} - -static uint8_t -exchange_mtu(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_exchange_mtu_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (ble_gattc_exchange_mtu(conn.conn_handle, exchange_func, NULL)) { - return BTP_STATUS_FAILED; - } - - /* this BTP command is about initiating MTU exchange, no need to wait - * for procedure to complete. - */ - return BTP_STATUS_SUCCESS; -} - -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 ? BTP_GATT_CFG_NOTIFY : BTP_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, 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 uint8_t -config_subscription_notif(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_cfg_notify_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - uint16_t ccc_handle = le16toh(cp->ccc_handle); - uint8_t status; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (cp->enable) { - /* on success response will be sent from callback */ - if (enable_subscription(conn.conn_handle, - ccc_handle, 0x0001) == 0) { - return BTP_STATUS_DELAY_REPLY; - } - - status = BTP_STATUS_FAILED; - } else { - if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { - status = BTP_STATUS_FAILED; - } else { - status = BTP_STATUS_SUCCESS; - } - } - - return status; -} - -static uint8_t -config_subscription_ind(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_cfg_notify_cmd *cp = cmd; - struct ble_gap_conn_desc conn; - uint16_t ccc_handle = le16toh(cp->ccc_handle); - uint8_t status; - int rc; - - SYS_LOG_DBG(""); - - rc = ble_gap_conn_find_by_addr(&cp->address, &conn); - if (rc) { - return BTP_STATUS_FAILED; - } - - if (cp->enable) { - /* on success response will be sent from callback */ - if (enable_subscription(conn.conn_handle, - ccc_handle, 0x0002) == 0) { - return BTP_STATUS_DELAY_REPLY; - } - - status = BTP_STATUS_FAILED; - } else { - if (disable_subscription(conn.conn_handle, ccc_handle) < 0) { - status = BTP_STATUS_FAILED; - } else { - status = BTP_STATUS_SUCCESS; - } - } - - return 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 uint8_t -get_attrs(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_get_attributes_cmd *cp = cmd; - struct btp_gatt_get_attributes_rp *rp = rsp; - struct btp_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]; - uint8_t status = BTP_STATUS_SUCCESS; - - SYS_LOG_DBG(""); - - if (!buf) { - return BTP_STATUS_FAILED; - } - - memset(str, 0, sizeof(str)); - memset(&uuid, 0, sizeof(uuid)); - start_handle = le16toh(cp->start_handle); - end_handle = le16toh(cp->end_handle); - - if (cp->type_length) { - if (btp2bt_uuid(cp->type, cp->type_length, &uuid)) { - status = BTP_STATUS_FAILED; - goto done; - } - - 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); - } - - 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 = os_mbuf_extend(buf, sizeof(*gatt_attr)); - if (!gatt_attr) { - status = BTP_STATUS_FAILED; - goto done; - } - gatt_attr->handle = htole16(entry->ha_handle_id); - gatt_attr->permission = flags_hs2btp(entry->ha_flags); - - if (entry->ha_uuid->type == BLE_UUID_TYPE_16) { - uint16_t uuid_val; - - gatt_attr->type_length = 2; - uuid_val = htole16(BLE_UUID16(entry->ha_uuid)->value); - if (os_mbuf_append(buf, &uuid_val, sizeof(uuid_val))) { - status = BTP_STATUS_FAILED; - goto done; - } - } else { - gatt_attr->type_length = 16; - if (os_mbuf_append(buf, BLE_UUID128(entry->ha_uuid)->value, - gatt_attr->type_length)) { - status = BTP_STATUS_FAILED; - goto done; - } - } - - count++; - - entry = ble_att_svr_find_by_uuid(entry, uuid_ptr, end_handle); - } - - rp->attrs_count = count; - os_mbuf_copydata(buf, 0, os_mbuf_len(buf), rp->attrs); - - *rsp_len = sizeof(*rp) + os_mbuf_len(buf); - -done: - os_mbuf_free_chain(buf); - return status; -} - -static uint8_t -get_attr_val(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_get_attribute_value_cmd *cp = cmd; - struct btp_gatt_get_attribute_value_rp *rp; - struct ble_gap_conn_desc conn; - struct os_mbuf *buf = os_msys_get(0, 0); - uint16_t handle = le16toh(cp->handle); - uint8_t out_att_err = 0; - int conn_status; - uint8_t status = BTP_STATUS_SUCCESS; - - conn_status = ble_gap_conn_find_by_addr(&cp->address, &conn); - - if (conn_status) { - rp = os_mbuf_extend(buf, sizeof(*rp)); - if (!rp) { - status = BTP_STATUS_FAILED; - goto free; - } - - 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); - - os_mbuf_copydata(buf, 0, os_mbuf_len(buf), rsp); - *rsp_len = os_mbuf_len(buf); - - goto free; - } else { - rp = os_mbuf_extend(buf, sizeof(*rp)); - if (!rp) { - status = BTP_STATUS_FAILED; - goto free; - } - - 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); - - os_mbuf_copydata(buf, 0, os_mbuf_len(buf), rsp); - *rsp_len = os_mbuf_len(buf); - - goto free; - } - -free: - os_mbuf_free_chain(buf); - return status; -} - -int -notify_multiple(uint16_t conn_handle, void *arg) -{ - struct notify_mult_cb_data *notify_data = - (struct notify_mult_cb_data *) arg; - int rc; - - SYS_LOG_DBG("") - - rc = ble_gatts_notify_multiple(conn_handle, - notify_data->tuple_cnt, - notify_data->handles); - - return rc; -} - -static uint8_t -notify_mult(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_notify_mult_val_cmd *cp = cmd; - struct notify_mult_cb_data cb_data; - int i; - - - if (cmd_len < sizeof(*cp) || - (cmd_len != (sizeof(*cp) + - (le16toh(cp->count) * sizeof(cp->handles[0]))))) { - - return BTP_STATUS_FAILED; - } - - if (le16toh(cp->count) > sizeof(cb_data.handles)) { - SYS_LOG_ERR("Too many handles to notify"); - return BTP_STATUS_FAILED; - } - - for (i = 0; i < cp->count; i++) { - cb_data.handles[i] = le16toh(cp->handles[i]); - } - - cb_data.tuple_cnt = cp->count; - - ble_gap_conn_foreach_handle(notify_multiple, (void *)&cb_data); - - return BTP_STATUS_SUCCESS; -} - -static uint8_t -change_database(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - const struct btp_gatt_change_database_cmd *cp = cmd; - - SYS_LOG_DBG("") - - ble_gatts_show_local(); - - ble_svc_gatt_changed(cp->start_handle, cp->end_handle); - - return BTP_STATUS_SUCCESS; -} - -static uint8_t -supported_commands(const void *cmd, uint16_t cmd_len, - void *rsp, uint16_t *rsp_len) -{ - struct btp_gatt_read_supported_commands_rp *rp = rsp; - - *rsp_len = tester_supported_commands(BTP_SERVICE_ID_GATT, rp->data); - *rsp_len += sizeof(*rp); - - return BTP_STATUS_SUCCESS; -} - -enum attr_type { - BLE_GATT_ATTR_SVC = 0, - BLE_GATT_ATTR_CHR, - BLE_GATT_ATTR_DSC, -}; - -static const struct btp_handler handlers[] = { - { - .opcode = BTP_GATT_READ_SUPPORTED_COMMANDS, - .index = BTP_INDEX_NONE, - .expect_len = 0, - .func = supported_commands, - }, - { - .opcode = BTP_GATT_START_SERVER, - .expect_len = 0, - .func = start_server, - }, - { - .opcode = BTP_GATT_EXCHANGE_MTU, - .expect_len = sizeof(struct btp_gatt_exchange_mtu_cmd), - .func = exchange_mtu, - }, - { - .opcode = BTP_GATT_DISC_ALL_PRIM_SVCS, - .expect_len = sizeof(struct btp_gatt_disc_all_prim_svcs_cmd), - .func = disc_all_prim_svcs, - }, - { - .opcode = BTP_GATT_DISC_PRIM_UUID, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = disc_prim_uuid, - }, - { - .opcode = BTP_GATT_FIND_INCLUDED, - .expect_len = sizeof(struct btp_gatt_find_included_cmd), - .func = find_included, - }, - { - .opcode = BTP_GATT_DISC_ALL_CHRC, - .expect_len = sizeof(struct btp_gatt_disc_all_chrc_cmd), - .func = disc_all_chrc, - }, - { - .opcode = BTP_GATT_DISC_CHRC_UUID, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = disc_chrc_uuid, - }, - { - .opcode = BTP_GATT_DISC_ALL_DESC, - .expect_len = sizeof(struct btp_gatt_disc_all_desc_cmd), - .func = disc_all_desc, - }, - { - .opcode = BTP_GATT_CHANGE_DATABASE, - .expect_len = sizeof(struct btp_gatt_change_database_cmd), - .func = change_database, - }, - { - .opcode = BTP_GATT_READ, - .expect_len = sizeof(struct btp_gatt_read_cmd), - .func = read_data, - }, - { - .opcode = BTP_GATT_READ_UUID, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = read_uuid, - }, - { - .opcode = BTP_GATT_READ_LONG, - .expect_len = sizeof(struct btp_gatt_read_long_cmd), - .func = read_long, - }, - { - .opcode = BTP_GATT_READ_MULTIPLE, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = read_multiple, - }, - { - .opcode = BTP_GATT_WRITE_WITHOUT_RSP, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = write_without_rsp, - }, -#if 0 - { - .opcode = BTP_GATT_SIGNED_WRITE_WITHOUT_RSP, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = write_signed_without_rsp, - }, -#endif - { - .opcode = BTP_GATT_WRITE, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = write_data, - }, - { - .opcode = BTP_GATT_WRITE_LONG, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = write_long, - }, - { - .opcode = BTP_GATT_RELIABLE_WRITE, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = reliable_write, - }, - { - .opcode = BTP_GATT_CFG_NOTIFY, - .expect_len = sizeof(struct btp_gatt_cfg_notify_cmd), - .func = config_subscription_notif, - }, - { - .opcode = BTP_GATT_CFG_INDICATE, - .expect_len = sizeof(struct btp_gatt_cfg_notify_cmd), - .func = config_subscription_ind, - }, - { - .opcode = BTP_GATT_GET_ATTRIBUTES, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = get_attrs, - }, - { - .opcode = BTP_GATT_GET_ATTRIBUTE_VALUE, - .expect_len = sizeof(struct btp_gatt_get_attribute_value_cmd), - .func = get_attr_val, - }, - { - .opcode = BTP_GATT_NOTIFY_MULTIPLE, - .expect_len = BTP_HANDLER_LENGTH_VARIABLE, - .func = notify_mult, - }, -}; - -int -tester_gatt_notify_rx_ev(uint16_t conn_handle, uint16_t attr_handle, - uint8_t indication, struct os_mbuf *om) -{ - struct btp_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; - } - - ev = os_mbuf_extend(buf, sizeof(*ev)); - if (!ev) { - goto fail; - } - - addr = &conn.peer_ota_addr; - - memcpy(&ev->address, addr, sizeof(ev->address)); - ev->type = (uint8_t) (indication ? 0x02 : 0x01); - ev->handle = htole16(attr_handle); - ev->data_length = htole16(os_mbuf_len(om)); - os_mbuf_appendfrom(buf, om, 0, os_mbuf_len(om)); - - tester_event(BTP_SERVICE_ID_GATT, BTP_GATT_EV_NOTIFICATION, - buf->om_data, buf->om_len); - -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); - - tester_register_command_handlers(BTP_SERVICE_ID_GATT, handlers, - ARRAY_SIZE(handlers)); - - return BTP_STATUS_SUCCESS; -} - -uint8_t -tester_unregister_gatt(void) -{ - return BTP_STATUS_SUCCESS; -} From 645b0e9cc1f29375d6c657e28709221a3b636453 Mon Sep 17 00:00:00 2001 From: Piotr Narajowski Date: Fri, 27 Jun 2025 16:16:46 +0200 Subject: [PATCH 3/3] nimble/host: Add method for retrieving configuration values of CCCD This metod allows reading configuration flags of Client Characteristic Configuration Descriptor for specified characteristic. --- nimble/host/include/host/ble_gatt.h | 31 +++++++++++++++++++++++++++++ nimble/host/src/ble_gatts.c | 29 +++++++++++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/nimble/host/include/host/ble_gatt.h b/nimble/host/include/host/ble_gatt.h index 229d99dda3..edeb5ee8aa 100644 --- a/nimble/host/include/host/ble_gatt.h +++ b/nimble/host/include/host/ble_gatt.h @@ -185,6 +185,19 @@ struct ble_hs_cfg; /** @} */ +/** + * @defgroup ble_gatts_clt_cfg Client Characteristic Configuration Descriptor (CCCD) Flags Types + * @{ + */ + +/** GATT Client Charactaristic Configuration Flag: Notify. */ +#define BLE_GATT_CCCD_NOTIFY 0x01 + +/** GATT Client Charactaristic Configuration Flag: Indicate. */ +#define BLE_GATT_CCCD_INDICATE 0x02 + +/** @} */ + /*** @client. */ /** Represents a GATT error. */ struct ble_gatt_error { @@ -1214,6 +1227,24 @@ int ble_gatts_start(void); */ int ble_gatts_peer_cl_sup_feat_get(uint16_t conn_handle, uint8_t *out_supported_feat, uint8_t len); +/** + * Reads configuration values from Client Characteristic Configuration + * Descriptor for specified characteristic. + * + * @param conn_handle Connection handle identifying the connection + * to which CCC instance is related. + * @param chr_val_handle The value handle of characteristic. + * @param cccd_value Configuration value of CCC. + * + * @return 0 on success; + * BLE_HS_ENOTCONN if no matching connection + * was found + * BLE_HS_ENOENT if descriptor could not be found. + * + */ +int ble_gatts_read_cccd(uint16_t conn_handle, uint16_t chr_val_handle, + uint8_t *cccd_value); + #ifdef __cplusplus } #endif diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c index ee6a8a3c65..2f453f3e6b 100644 --- a/nimble/host/src/ble_gatts.c +++ b/nimble/host/src/ble_gatts.c @@ -737,6 +737,35 @@ ble_gatts_clt_cfg_access_locked(struct ble_hs_conn *conn, uint16_t attr_handle, return 0; } +int +ble_gatts_read_cccd(uint16_t conn_handle, uint16_t chr_val_handle, + uint8_t *cccd_value) +{ + struct ble_gatts_clt_cfg *clt_cfg; + struct ble_hs_conn *conn; + + ble_hs_lock(); + + conn = ble_hs_conn_find(conn_handle); + if (conn == NULL) { + ble_hs_lock(); + return BLE_HS_ENOTCONN; + } + + clt_cfg = ble_gatts_clt_cfg_find(conn->bhc_gatt_svr.clt_cfgs, + chr_val_handle); + if (clt_cfg == NULL) { + ble_hs_lock(); + return BLE_HS_ENOENT; + } + + *cccd_value = clt_cfg->flags & ~BLE_GATTS_CLT_CFG_F_RESERVED; + + ble_hs_unlock(); + + return 0; +} + int ble_gatts_clt_cfg_access(uint16_t conn_handle, uint16_t attr_handle, uint8_t op, uint16_t offset, struct os_mbuf **om,