From d0cdd9babd7a5904dc931f917e6af3920f083aab Mon Sep 17 00:00:00 2001 From: Sean Madigan Date: Tue, 5 Nov 2024 16:03:49 +0000 Subject: [PATCH] bluetooth: services: ras: Add RREQ implementation to RAS Add implementation for Ranging Requestor, a Ranging Service client which can be used to obtain channel sounding ranging data from a Ranging Responder which operates as a Ranging Service server. This current implementation can perform most mandatory features from the Ranging Profile/Ranging Service specifications to alert an application of available ranging data and receive the data from the server. Further work can be done to add support for reading the features from the server, receiving real time ranging data, aborting receive operation, adding filters for ranging data Signed-off-by: Sean Madigan --- include/bluetooth/services/ras.h | 165 ++++ subsys/bluetooth/services/ras/CMakeLists.txt | 1 + .../services/ras/rreq/CMakeLists.txt | 9 + .../services/ras/rreq/Kconfig.ras_rreq | 9 + subsys/bluetooth/services/ras/rreq/ras_rreq.c | 861 ++++++++++++++++++ 5 files changed, 1045 insertions(+) create mode 100644 subsys/bluetooth/services/ras/rreq/CMakeLists.txt create mode 100644 subsys/bluetooth/services/ras/rreq/ras_rreq.c diff --git a/include/bluetooth/services/ras.h b/include/bluetooth/services/ras.h index d35ff1b09d41..26a00a11f17f 100644 --- a/include/bluetooth/services/ras.h +++ b/include/bluetooth/services/ras.h @@ -12,6 +12,7 @@ #include #include #include +#include /** @file * @defgroup bt_ras Ranging Service API @@ -312,6 +313,170 @@ int bt_ras_rd_buffer_release(struct ras_rd_buffer *buf); int bt_ras_rd_buffer_bytes_pull(struct ras_rd_buffer *buf, uint8_t *out_buf, uint16_t max_data_len, uint16_t *read_cursor, bool *empty); +/** @brief Ranging data ready callback. Called when peer has ranging data available. + * + * @param[in] conn Connection Object. + * @param[in] ranging_counter Ranging counter ready to be requested. + */ +typedef void (*bt_ras_rreq_rd_ready_cb_t)(struct bt_conn *conn, uint16_t ranging_counter); + +/** @brief Ranging data overwritten callback. Called when peer has overwritten previously available + * ranging data. + * + * @param[in] conn Connection Object. + * @param[in] ranging_counter Ranging counter which has been overwritten. + */ +typedef void (*bt_ras_rreq_rd_overwritten_cb_t)(struct bt_conn *conn, uint16_t ranging_counter); + +/** @brief Ranging data get complete callback. Called when ranging data get procedure has completed. + * + * @param[in] conn Connection Object. + * @param[in] ranging_counter Ranging counter which has been completed. + * @param[in] err Error code, 0 if the ranging data get was successful. Otherwise a + * negative error code. + */ +typedef void (*bt_ras_rreq_ranging_data_get_complete_t)(struct bt_conn *conn, + uint16_t ranging_counter, int err); + +/** @brief Allocate a RREQ context and assign GATT handles. Takes a reference to the connection. + * + * @note RREQ context will be freed automatically on disconnect. + * + * @param[in] dm Discovery Object. + * @param[in] conn Connection Object. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. No RREQ context will be allocated if + * there is an error. + */ +int bt_ras_rreq_alloc_and_assign_handles(struct bt_gatt_dm *dm, struct bt_conn *conn); + +/** @brief Get ranging data for given ranging counter. + * + * @note This should only be called after receiving a ranging data ready callback and + * when subscribed to ondemand ranging data and RAS-CP. + * + * @param[in] conn Connection Object. + * @param[in] ranging_data_out Simple buffer to store received ranging data. + * @param[in] ranging_counter Ranging counter to get. + * @param[in] data_get_complete_cb Callback called when get ranging data completes. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_cp_get_ranging_data(struct bt_conn *conn, struct net_buf_simple *ranging_data_out, + uint16_t ranging_counter, + bt_ras_rreq_ranging_data_get_complete_t data_get_complete_cb); + +/** @brief Free RREQ context for connection. This will unsubscribe from any remaining subscriptions. + * + * @note RREQ context will be freed automatically on disconnect. + * + * @param[in] conn Connection Object. + */ +void bt_ras_rreq_free(struct bt_conn *conn); + +/** @brief Subscribe to RAS-CP. Required to be called before @ref bt_ras_rreq_cp_get_ranging_data. + * + * @note Calling from BT RX thread may return an error as bt_gatt_subscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_cp_subscribe(struct bt_conn *conn); + +/** @brief Unsubscribe from RAS-CP. + * + * @note Calling from BT RX thread may return an error as bt_gatt_unsubscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_cp_unsubscribe(struct bt_conn *conn); + +/** @brief Subscribe to on-demand ranging data notifications. Required to be called before @ref + * bt_ras_rreq_cp_get_ranging_data. + * + * @note Calling from BT RX thread may return an error as bt_gatt_subscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_on_demand_rd_subscribe(struct bt_conn *conn); + +/** @brief Unsubscribe from on-demand ranging data notifications. + * + * @note Calling from BT RX thread may return an error as bt_gatt_unsubscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_on_demand_rd_unsubscribe(struct bt_conn *conn); + +/** @brief Subscribe to ranging data ready notifications. These notify when on-demand ranging data + * is available for a given CS procedure counter. + * + * @note Calling from BT RX thread may return an error as bt_gatt_subscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * @param[in] cb Ranging data ready callback. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_rd_ready_subscribe(struct bt_conn *conn, bt_ras_rreq_rd_ready_cb_t cb); + +/** @brief Unsubscribe from ranging data ready notifications. + * + * @note Calling from BT RX thread may return an error as bt_gatt_unsubscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_rd_ready_unsubscribe(struct bt_conn *conn); + +/** @brief Subscribe to ranging data overwritten notifications. These notify when on-demand ranging + * data is no longer available for a given CS procedure counter. + * + * @note Calling from BT RX thread may return an error as bt_gatt_subscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * @param[in] cb Ranging data overwritten callback. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_rd_overwritten_subscribe(struct bt_conn *conn, bt_ras_rreq_rd_overwritten_cb_t cb); + +/** @brief Unsubscribe from ranging data overwritten notifications. + * + * @note Calling from BT RX thread may return an error as bt_gatt_unsubscribe will not block if + * there are no available TX buffers. + * + * @param[in] conn Connection Object, which already has associated RREQ context. + * + * @retval 0 If the operation was successful. + * Otherwise, a negative error code is returned. + */ +int bt_ras_rreq_rd_overwritten_unsubscribe(struct bt_conn *conn); + #ifdef __cplusplus } #endif diff --git a/subsys/bluetooth/services/ras/CMakeLists.txt b/subsys/bluetooth/services/ras/CMakeLists.txt index 7b1948e11776..cc101fbd4edb 100644 --- a/subsys/bluetooth/services/ras/CMakeLists.txt +++ b/subsys/bluetooth/services/ras/CMakeLists.txt @@ -5,3 +5,4 @@ # add_subdirectory_ifdef(CONFIG_BT_RAS_RRSP rrsp) +add_subdirectory_ifdef(CONFIG_BT_RAS_RREQ rreq) diff --git a/subsys/bluetooth/services/ras/rreq/CMakeLists.txt b/subsys/bluetooth/services/ras/rreq/CMakeLists.txt new file mode 100644 index 000000000000..082eaccff5b0 --- /dev/null +++ b/subsys/bluetooth/services/ras/rreq/CMakeLists.txt @@ -0,0 +1,9 @@ +# +# Copyright (c) 2024 Nordic Semiconductor +# +# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause +# + +zephyr_library_sources_ifdef( + CONFIG_BT_RAS_RREQ + ras_rreq.c) diff --git a/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq b/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq index cb58dfc43e84..eebfc57f7359 100644 --- a/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq +++ b/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq @@ -7,6 +7,8 @@ menuconfig BT_RAS_RREQ bool "GATT Ranging Requester Client [EXPERIMENTAL]" select EXPERIMENTAL + select BT_GATT_DM + select BT_GATT_CLIENT if BT_RAS_RREQ @@ -14,4 +16,11 @@ module = BT_RAS_RREQ module-str = RAS_RREQ source "${ZEPHYR_BASE}/subsys/logging/Kconfig.template.log_config" +config BT_RAS_RREQ_MAX_ACTIVE_CONN + int "Number of simultaneously supported RREQ instances" + default BT_MAX_CONN + range 1 BT_MAX_CONN + help + The number of simultaneous connections with an instance of RAS RREQ + endif # BT_RAS_RREQ diff --git a/subsys/bluetooth/services/ras/rreq/ras_rreq.c b/subsys/bluetooth/services/ras/rreq/ras_rreq.c new file mode 100644 index 000000000000..72c21dd3e29a --- /dev/null +++ b/subsys/bluetooth/services/ras/rreq/ras_rreq.c @@ -0,0 +1,861 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../ras_internal.h" + +LOG_MODULE_REGISTER(ras_rreq, CONFIG_BT_RAS_RREQ_LOG_LEVEL); + +enum bt_ras_rreq_cp_state { + BT_RAS_RREQ_CP_STATE_NONE, + BT_RAS_RREQ_CP_STATE_GET_RD_WRITTEN, + BT_RAS_RREQ_CP_STATE_ACK_RD_WRITTEN, +}; + +struct bt_ras_rreq_cp { + struct bt_gatt_subscribe_params subscribe_params; + enum bt_ras_rreq_cp_state state; +}; + +struct bt_ras_on_demand_rd { + int error_status; + struct bt_gatt_subscribe_params subscribe_params; + struct net_buf_simple *ranging_data_out; + bt_ras_rreq_ranging_data_get_complete_t cb; + uint16_t counter_in_progress; + uint8_t next_expected_segment_counter; + bool data_get_in_progress; + bool last_segment_received; +}; + +struct bt_ras_rd_ready { + struct bt_gatt_subscribe_params subscribe_params; + bt_ras_rreq_rd_ready_cb_t cb; +}; + +struct bt_ras_rd_overwritten { + struct bt_gatt_subscribe_params subscribe_params; + bt_ras_rreq_rd_overwritten_cb_t cb; +}; + +static struct bt_ras_rreq { + struct bt_conn *conn; + struct bt_ras_rreq_cp cp; + struct bt_ras_on_demand_rd on_demand_rd; + struct bt_ras_rd_ready rd_ready; + struct bt_ras_rd_overwritten rd_overwritten; +} rreq_pool[CONFIG_BT_RAS_RREQ_MAX_ACTIVE_CONN]; + +static struct bt_ras_rreq *ras_rreq_find(struct bt_conn *conn) +{ + if (conn == NULL) { + return NULL; + } + + ARRAY_FOR_EACH_PTR(rreq_pool, rreq) { + if (rreq->conn == conn) { + return rreq; + } + } + + return NULL; +} + +static int ras_rreq_alloc(struct bt_conn *conn) +{ + struct bt_ras_rreq *rreq = NULL; + + if (ras_rreq_find(conn) != NULL) { + return -EALREADY; + } + + ARRAY_FOR_EACH_PTR(rreq_pool, rreq_iter) { + if (rreq_iter->conn == NULL) { + rreq = rreq_iter; + break; + } + } + + if (rreq == NULL) { + return -ENOMEM; + } + + LOG_DBG("conn %p new rreq %p", (void *)conn, (void *)rreq); + + memset(rreq, 0, sizeof(struct bt_ras_rreq)); + rreq->conn = bt_conn_ref(conn); + + return 0; +} + +static void disconnected(struct bt_conn *conn, uint8_t reason) +{ + ARG_UNUSED(reason); + + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return; + } + + if (rreq->on_demand_rd.data_get_in_progress) { + rreq->on_demand_rd.cb(conn, rreq->on_demand_rd.counter_in_progress, -ENOTCONN); + } + + bt_ras_rreq_free(conn); +} + +BT_CONN_CB_DEFINE(conn_callbacks) = { + .disconnected = disconnected, +}; + +static uint8_t ranging_data_ready_notify_func(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + if (data == NULL) { + LOG_DBG("Ranging data ready unsubscribed"); + return BT_GATT_ITER_STOP; + } + + if (length != sizeof(uint16_t)) { + LOG_WRN("Received Ranging Data Ready Indication with invalid size"); + return BT_GATT_ITER_CONTINUE; + } + + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (rreq == NULL) { + LOG_WRN("Ranging data ready notification received without associated RREQ context, " + "unsubscribing"); + return BT_GATT_ITER_STOP; + } + + uint16_t ranging_counter = *(uint16_t *)data; + + if (rreq->rd_ready.cb) { + rreq->rd_ready.cb(conn, ranging_counter); + } + + return BT_GATT_ITER_CONTINUE; +} + +static void data_receive_finished(struct bt_ras_rreq *rreq) +{ + if (rreq->on_demand_rd.error_status == 0 && !rreq->on_demand_rd.last_segment_received) { + LOG_WRN("Ranging data completed with missing segments"); + rreq->on_demand_rd.error_status = -ENODATA; + } + + rreq->on_demand_rd.data_get_in_progress = false; + + rreq->on_demand_rd.cb(rreq->conn, rreq->on_demand_rd.counter_in_progress, + rreq->on_demand_rd.error_status); +} + +static uint8_t ranging_data_overwritten_notify_func(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + if (data == NULL) { + LOG_DBG("Ranging data overwritten unsubscribed"); + return BT_GATT_ITER_STOP; + } + + if (length != sizeof(uint16_t)) { + LOG_WRN("Ranging Data Overwritten Indication size error"); + return BT_GATT_ITER_CONTINUE; + } + + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (rreq == NULL) { + LOG_WRN("Ranging data overwritten notification received without associated RREQ " + "context, unsubscribing"); + return BT_GATT_ITER_STOP; + } + + uint16_t ranging_counter = *(uint16_t *)data; + + if (rreq->on_demand_rd.data_get_in_progress && + rreq->on_demand_rd.counter_in_progress == ranging_counter) { + if (rreq->cp.state != BT_RAS_RREQ_CP_STATE_NONE) { + LOG_DBG("Overwritten received while writing to RAS-CP, will continue " + "waiting for RAS-CP response"); + return BT_GATT_ITER_CONTINUE; + } + + LOG_DBG("Ranging counter %d overwritten whilst receiving", ranging_counter); + rreq->on_demand_rd.error_status = -EACCES; + data_receive_finished(rreq); + + return BT_GATT_ITER_CONTINUE; + } + + if (rreq->rd_overwritten.cb) { + rreq->rd_overwritten.cb(conn, ranging_counter); + } + + return BT_GATT_ITER_CONTINUE; +} + +static void ack_ranging_data(struct bt_ras_rreq *rreq) +{ + NET_BUF_SIMPLE_DEFINE(ack_buf, RASCP_CMD_OPCODE_LEN + sizeof(uint16_t)); + net_buf_simple_add_u8(&ack_buf, RASCP_OPCODE_ACK_RD); + net_buf_simple_add_le16(&ack_buf, rreq->on_demand_rd.counter_in_progress); + + int err = bt_gatt_write_without_response(rreq->conn, rreq->cp.subscribe_params.value_handle, + ack_buf.data, ack_buf.len, false); + + if (err) { + LOG_WRN("ACK ranging data write failed, err %d. Ending data receive process " + "here, peer will just have to wait for timeout.", + err); + data_receive_finished(rreq); + } + + rreq->cp.state = BT_RAS_RREQ_CP_STATE_ACK_RD_WRITTEN; + LOG_DBG("Ack Ranging data for counter %d", rreq->on_demand_rd.counter_in_progress); +} + +static void handle_rsp_code(uint8_t rsp_code, struct bt_ras_rreq *rreq) +{ + switch (rreq->cp.state) { + case BT_RAS_RREQ_CP_STATE_NONE: { + if (rreq->on_demand_rd.data_get_in_progress && + rsp_code == RASCP_RESPONSE_PROCEDURE_NOT_COMPLETED) { + LOG_DBG("Ranging counter %d aborted whilst receiving", + rreq->on_demand_rd.counter_in_progress); + rreq->on_demand_rd.error_status = -EACCES; + data_receive_finished(rreq); + break; + } + + LOG_WRN("Unexpected Response code received %d", rsp_code); + break; + } + case BT_RAS_RREQ_CP_STATE_GET_RD_WRITTEN: { + __ASSERT_NO_MSG(rreq->on_demand_rd.data_get_in_progress); + rreq->cp.state = BT_RAS_RREQ_CP_STATE_NONE; + + if (rsp_code != RASCP_RESPONSE_SUCCESS) { + LOG_DBG("Get Ranging Data returned an error %d", rsp_code); + rreq->on_demand_rd.error_status = -ENOENT; + data_receive_finished(rreq); + break; + } + + LOG_DBG("Get Ranging Data Success"); + + break; + } + case BT_RAS_RREQ_CP_STATE_ACK_RD_WRITTEN: { + __ASSERT_NO_MSG(rreq->on_demand_rd.data_get_in_progress); + rreq->cp.state = BT_RAS_RREQ_CP_STATE_NONE; + if (rsp_code != RASCP_RESPONSE_SUCCESS) { + LOG_WRN("ACK Ranging Data returned an error %d. This should have no " + "impact, so continue.", + rsp_code); + } + + data_receive_finished(rreq); + break; + } + default: + LOG_WRN("Unexpected Response code received %d", rsp_code); + break; + } +} + +static uint8_t ras_cp_notify_func(struct bt_conn *conn, struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + if (data == NULL) { + LOG_DBG("RAS CP unsubscribed"); + return BT_GATT_ITER_STOP; + } + + struct net_buf_simple rsp; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (rreq == NULL) { + LOG_WRN("RAS CP notification received without associated RREQ context, " + "unsubscribing"); + return BT_GATT_ITER_STOP; + } + + net_buf_simple_init_with_data(&rsp, (uint8_t *)data, length); + + uint8_t opcode = net_buf_simple_pull_u8(&rsp); + + switch (opcode) { + case RASCP_RSP_OPCODE_COMPLETE_RD_RSP: { + if (rsp.len != RASCP_RSP_OPCODE_COMPLETE_RD_RSP_LEN) { + LOG_WRN("Received complete ranging data response with invalid length: %d", + length); + break; + } + + uint16_t ranging_counter = net_buf_simple_pull_le16(&rsp); + + if (!rreq->on_demand_rd.data_get_in_progress || + rreq->on_demand_rd.counter_in_progress != ranging_counter) { + LOG_WRN("Received complete ranging data response with unexpected ranging " + "counter %d", + ranging_counter); + break; + } + + ack_ranging_data(rreq); + break; + } + case RASCP_RSP_OPCODE_RSP_CODE: { + if (rsp.len != RASCP_RSP_OPCODE_RSP_CODE_LEN) { + LOG_WRN("Received response opcode with invalid length: %d", length); + break; + } + + uint8_t rsp_code = net_buf_simple_pull_u8(&rsp); + + handle_rsp_code(rsp_code, rreq); + + break; + } + default: { + LOG_WRN("Received unknown RAS-CP opcode: %d", opcode); + break; + } + } + + return BT_GATT_ITER_CONTINUE; +} + +static uint8_t ras_on_demand_ranging_data_notify_func(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) +{ + LOG_DBG("On-demand Ranging Data notification received"); + + if (data == NULL) { + LOG_DBG("On demand ranging data unsubscribed"); + return BT_GATT_ITER_STOP; + } + + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (rreq == NULL) { + LOG_WRN("On-demand ranging data notification received without associated RREQ " + "context, unsubscribing"); + return BT_GATT_ITER_STOP; + } + + if (!rreq->on_demand_rd.data_get_in_progress) { + LOG_WRN("Unexpected On-demand Ranging Data notification received"); + return BT_GATT_ITER_CONTINUE; + } + + if (rreq->on_demand_rd.last_segment_received) { + LOG_WRN("On-demand Ranging Data notification received after last segment"); + return BT_GATT_ITER_CONTINUE; + } + + if (rreq->on_demand_rd.error_status) { + /* Already had an error receiving this ranging counter, so exit here. */ + return BT_GATT_ITER_CONTINUE; + } + + if (length < 2) { + LOG_WRN("On-demand Ranging Data notification received invalid length"); + rreq->on_demand_rd.error_status = -EINVAL; + return BT_GATT_ITER_CONTINUE; + } + + struct net_buf_simple segment; + + net_buf_simple_init_with_data(&segment, (uint8_t *)data, length); + + uint8_t segmentation_header = net_buf_simple_pull_u8(&segment); + + bool first_segment = segmentation_header & BIT(0); + bool last_segment = segmentation_header & BIT(1); + uint8_t rolling_segment_counter = segmentation_header >> 2; + + if (first_segment && rolling_segment_counter != 0) { + LOG_WRN("On-demand Ranging Data notification received invalid " + "rolling_segment_counter %d", + rolling_segment_counter); + rreq->on_demand_rd.error_status = -EINVAL; + return BT_GATT_ITER_CONTINUE; + } + + if (rreq->on_demand_rd.next_expected_segment_counter != rolling_segment_counter) { + LOG_WRN("No support for receiving segments out of order, expected counter %d, " + "received counter %d", + rreq->on_demand_rd.next_expected_segment_counter, rolling_segment_counter); + rreq->on_demand_rd.error_status = -ENODATA; + return BT_GATT_ITER_CONTINUE; + } + + uint16_t ranging_data_segment_length = segment.len; + + if (net_buf_simple_tailroom(rreq->on_demand_rd.ranging_data_out) < + ranging_data_segment_length) { + LOG_WRN("Ranging data out buffer not large enough for next segment"); + rreq->on_demand_rd.error_status = -ENOMEM; + return BT_GATT_ITER_CONTINUE; + } + + uint8_t *ranging_data_segment = + net_buf_simple_pull_mem(&segment, ranging_data_segment_length); + net_buf_simple_add_mem(rreq->on_demand_rd.ranging_data_out, ranging_data_segment, + ranging_data_segment_length); + + if (last_segment) { + rreq->on_demand_rd.last_segment_received = true; + } + + /* Segment counter is between 0-63. */ + rreq->on_demand_rd.next_expected_segment_counter = + (rolling_segment_counter + 1) & BIT_MASK(6); + + return BT_GATT_ITER_CONTINUE; +} + +static void subscribed_func(struct bt_conn *conn, uint8_t err, + struct bt_gatt_subscribe_params *params) +{ + if (err) { + LOG_ERR("Subscribe to ccc_handle %d failed, err %d", params->ccc_handle, err); + } +} + +static int ras_cp_subscribe_params_populate(struct bt_gatt_dm *dm, struct bt_ras_rreq *rreq) +{ + const struct bt_gatt_dm_attr *gatt_chrc; + const struct bt_gatt_dm_attr *gatt_desc; + + /* RAS-CP characteristic (mandatory) */ + gatt_chrc = bt_gatt_dm_char_by_uuid(dm, BT_UUID_RAS_CP); + if (!gatt_chrc) { + LOG_WRN("Could not locate mandatory RAS CP characteristic"); + return -EINVAL; + } + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_RAS_CP); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory RAS CP descriptor"); + return -EINVAL; + } + rreq->cp.subscribe_params.value_handle = gatt_desc->handle; + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_GATT_CCC); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory RAS CP CCC"); + return -EINVAL; + } + rreq->cp.subscribe_params.ccc_handle = gatt_desc->handle; + + rreq->cp.subscribe_params.notify = ras_cp_notify_func; + rreq->cp.subscribe_params.value = BT_GATT_CCC_INDICATE; + rreq->cp.subscribe_params.subscribe = subscribed_func; + + return 0; +} + +int bt_ras_rreq_cp_subscribe(struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return -EINVAL; + } + + err = bt_gatt_subscribe(conn, &rreq->cp.subscribe_params); + if (err) { + LOG_DBG("RAS-CP subscribe failed (err %d)", err); + return err; + } + + return 0; +} + +int bt_ras_rreq_cp_unsubscribe(struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return -EINVAL; + } + + err = bt_gatt_unsubscribe(conn, &rreq->cp.subscribe_params); + if (err) { + LOG_DBG("RAS-CP unsubscribe failed (err %d)", err); + return err; + } + + return 0; +} + +static int ondemand_rd_subscribe_params_populate(struct bt_gatt_dm *dm, struct bt_ras_rreq *rreq) +{ + const struct bt_gatt_dm_attr *gatt_chrc; + const struct bt_gatt_dm_attr *gatt_desc; + + /* RAS On-demand ranging data characteristic (mandatory) */ + gatt_chrc = bt_gatt_dm_char_by_uuid(dm, BT_UUID_RAS_ONDEMAND_RD); + if (!gatt_chrc) { + LOG_WRN("Could not locate mandatory On-demand ranging data characteristic"); + return -EINVAL; + } + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_RAS_ONDEMAND_RD); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory On-demand ranging data descriptor"); + return -EINVAL; + } + rreq->on_demand_rd.subscribe_params.value_handle = gatt_desc->handle; + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_GATT_CCC); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory On-demand ranging data CCC"); + return -EINVAL; + } + rreq->on_demand_rd.subscribe_params.ccc_handle = gatt_desc->handle; + rreq->on_demand_rd.subscribe_params.notify = ras_on_demand_ranging_data_notify_func; + rreq->on_demand_rd.subscribe_params.value = BT_GATT_CCC_NOTIFY | BT_GATT_CCC_INDICATE; + rreq->on_demand_rd.subscribe_params.subscribe = subscribed_func; + + return 0; +} + +static int rd_ready_subscribe_params_populate(struct bt_gatt_dm *dm, struct bt_ras_rreq *rreq) +{ + const struct bt_gatt_dm_attr *gatt_chrc; + const struct bt_gatt_dm_attr *gatt_desc; + + /* RAS Ranging Data Ready characteristic (mandatory) */ + gatt_chrc = bt_gatt_dm_char_by_uuid(dm, BT_UUID_RAS_RD_READY); + if (!gatt_chrc) { + LOG_WRN("Could not locate mandatory ranging data ready characteristic"); + return -EINVAL; + } + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_RAS_RD_READY); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory ranging data ready descriptor"); + return -EINVAL; + } + rreq->rd_ready.subscribe_params.value_handle = gatt_desc->handle; + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_GATT_CCC); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory ranging data ready CCC"); + return -EINVAL; + } + rreq->rd_ready.subscribe_params.ccc_handle = gatt_desc->handle; + rreq->rd_ready.subscribe_params.notify = ranging_data_ready_notify_func; + rreq->rd_ready.subscribe_params.value = BT_GATT_CCC_NOTIFY | BT_GATT_CCC_INDICATE; + rreq->rd_ready.subscribe_params.subscribe = subscribed_func; + + return 0; +} + +static int rd_overwritten_subscribe_params_populate(struct bt_gatt_dm *dm, struct bt_ras_rreq *rreq) +{ + const struct bt_gatt_dm_attr *gatt_chrc; + const struct bt_gatt_dm_attr *gatt_desc; + + /* RAS Ranging Data Overwritten characteristic (mandatory) */ + gatt_chrc = bt_gatt_dm_char_by_uuid(dm, BT_UUID_RAS_RD_OVERWRITTEN); + if (!gatt_chrc) { + LOG_WRN("Could not locate mandatory ranging data overwritten characteristic"); + return -EINVAL; + } + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_RAS_RD_OVERWRITTEN); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory ranging data overwritten descriptor"); + return -EINVAL; + } + rreq->rd_overwritten.subscribe_params.value_handle = gatt_desc->handle; + + gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_GATT_CCC); + if (!gatt_desc) { + LOG_WRN("Could not locate mandatory ranging data overwritten CCC"); + return -EINVAL; + } + rreq->rd_overwritten.subscribe_params.ccc_handle = gatt_desc->handle; + rreq->rd_overwritten.subscribe_params.notify = ranging_data_overwritten_notify_func; + rreq->rd_overwritten.subscribe_params.value = BT_GATT_CCC_NOTIFY | BT_GATT_CCC_INDICATE; + rreq->rd_overwritten.subscribe_params.subscribe = subscribed_func; + + return 0; +} + +int bt_ras_rreq_on_demand_rd_subscribe(struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return -EINVAL; + } + + err = bt_gatt_subscribe(conn, &rreq->on_demand_rd.subscribe_params); + if (err) { + LOG_DBG("On-demand ranging data subscribe failed (err %d)", err); + return err; + } + + LOG_DBG("On-demand ranging data subscribed"); + + return 0; +} + +int bt_ras_rreq_on_demand_rd_unsubscribe(struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return -EINVAL; + } + + err = bt_gatt_unsubscribe(conn, &rreq->on_demand_rd.subscribe_params); + if (err) { + LOG_DBG("On-demand ranging data unsubscribe failed (err %d)", err); + return err; + } + + LOG_DBG("On-demand ranging data unsubscribed"); + + return 0; +} + +int bt_ras_rreq_rd_ready_subscribe(struct bt_conn *conn, bt_ras_rreq_rd_ready_cb_t cb) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq || !cb) { + return -EINVAL; + } + + err = bt_gatt_subscribe(conn, &rreq->rd_ready.subscribe_params); + if (err) { + LOG_DBG("Ranging data ready subscribe failed (err %d)", err); + return err; + } + + LOG_DBG("Ranging data ready subscribed"); + rreq->rd_ready.cb = cb; + + return 0; +} + +int bt_ras_rreq_rd_ready_unsubscribe(struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return -EINVAL; + } + + err = bt_gatt_unsubscribe(conn, &rreq->rd_ready.subscribe_params); + if (err) { + LOG_DBG("Ranging data ready unsubscribe failed (err %d)", err); + return err; + } + + LOG_DBG("Ranging data ready unsubscribed"); + rreq->rd_ready.cb = NULL; + + return 0; +} + +int bt_ras_rreq_rd_overwritten_subscribe(struct bt_conn *conn, bt_ras_rreq_rd_overwritten_cb_t cb) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq || !cb) { + return -EINVAL; + } + + err = bt_gatt_subscribe(conn, &rreq->rd_overwritten.subscribe_params); + if (err) { + LOG_DBG("Ranging data overwritten subscribe failed (err %d)", err); + return err; + } + + LOG_DBG("Ranging data overwritten subscribed"); + rreq->rd_overwritten.cb = cb; + + return 0; +} + +int bt_ras_rreq_rd_overwritten_unsubscribe(struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return -EINVAL; + } + + err = bt_gatt_unsubscribe(conn, &rreq->rd_overwritten.subscribe_params); + if (err) { + LOG_DBG("Ranging data overwritten unsubscribe failed (err %d)", err); + return err; + } + + LOG_DBG("Ranging data overwritten unsubscribed"); + rreq->rd_overwritten.cb = NULL; + + return 0; +} + +void bt_ras_rreq_free(struct bt_conn *conn) +{ + int err = 0; + struct bt_conn_info info; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (!rreq) { + return; + } + + LOG_DBG("Free rreq %p for conn %p", (void *)conn, (void *)rreq); + + err = bt_conn_get_info(conn, &info); + if (err != 0) { + bt_conn_unref(rreq->conn); + rreq->conn = NULL; + return; + } + + if (info.state == BT_CONN_STATE_CONNECTED) { + /* -EINVAL means already not subscribed */ + err = bt_gatt_unsubscribe(conn, &rreq->cp.subscribe_params); + if (err != 0 && err != -EINVAL) { + LOG_WRN("Failed to unsubscribe to RAS-CP: %d", err); + } + + err = bt_gatt_unsubscribe(conn, &rreq->on_demand_rd.subscribe_params); + if (err != 0 && err != -EINVAL) { + LOG_WRN("Failed to unsubscribe to ondemand ranging data: %d", err); + } + + err = bt_gatt_unsubscribe(conn, &rreq->rd_ready.subscribe_params); + if (err != 0 && err != -EINVAL) { + LOG_WRN("Failed to unsubscribe to ranging data ready: %d", err); + } + + err = bt_gatt_unsubscribe(conn, &rreq->rd_overwritten.subscribe_params); + if (err != 0 && err != -EINVAL) { + LOG_WRN("Failed to unsubscribe to ranging data overwritten: %d", err); + } + } + + bt_conn_unref(rreq->conn); + rreq->conn = NULL; +} + +int bt_ras_rreq_alloc_and_assign_handles(struct bt_gatt_dm *dm, struct bt_conn *conn) +{ + int err; + struct bt_ras_rreq *rreq = NULL; + + if (dm == NULL || conn == NULL) { + return -EINVAL; + } + + err = ras_rreq_alloc(conn); + if (err) { + return err; + } + + rreq = ras_rreq_find(conn); + __ASSERT_NO_MSG(rreq); + + err = ondemand_rd_subscribe_params_populate(dm, rreq); + if (err) { + bt_ras_rreq_free(conn); + return err; + } + + err = rd_ready_subscribe_params_populate(dm, rreq); + if (err) { + bt_ras_rreq_free(conn); + return err; + } + + err = rd_overwritten_subscribe_params_populate(dm, rreq); + if (err) { + bt_ras_rreq_free(conn); + return err; + } + + err = ras_cp_subscribe_params_populate(dm, rreq); + if (err) { + bt_ras_rreq_free(conn); + return err; + } + + return 0; +} + +int bt_ras_rreq_cp_get_ranging_data(struct bt_conn *conn, struct net_buf_simple *ranging_data_out, + uint16_t ranging_counter, + bt_ras_rreq_ranging_data_get_complete_t cb) +{ + int err; + struct bt_ras_rreq *rreq = ras_rreq_find(conn); + + if (rreq == NULL || ranging_data_out == NULL || cb == NULL) { + return -EINVAL; + } + + if (rreq->cp.state != BT_RAS_RREQ_CP_STATE_NONE || + rreq->on_demand_rd.data_get_in_progress) { + return -EBUSY; + } + + rreq->on_demand_rd.data_get_in_progress = true; + rreq->on_demand_rd.ranging_data_out = ranging_data_out; + rreq->on_demand_rd.counter_in_progress = ranging_counter; + rreq->on_demand_rd.cb = cb; + rreq->on_demand_rd.next_expected_segment_counter = 0; + rreq->on_demand_rd.last_segment_received = false; + rreq->on_demand_rd.error_status = 0; + + NET_BUF_SIMPLE_DEFINE(get_ranging_data, RASCP_CMD_OPCODE_LEN + sizeof(uint16_t)); + net_buf_simple_add_u8(&get_ranging_data, RASCP_OPCODE_GET_RD); + net_buf_simple_add_le16(&get_ranging_data, rreq->on_demand_rd.counter_in_progress); + + err = bt_gatt_write_without_response(conn, rreq->cp.subscribe_params.value_handle, + get_ranging_data.data, get_ranging_data.len, false); + if (err) { + LOG_DBG("CP Get ranging data written failed, err %d", err); + return err; + } + + rreq->cp.state = BT_RAS_RREQ_CP_STATE_GET_RD_WRITTEN; + + return 0; +}