diff --git a/include/bluetooth/services/ras.h b/include/bluetooth/services/ras.h index 6e63deae6da5..d35ff1b09d41 100644 --- a/include/bluetooth/services/ras.h +++ b/include/bluetooth/services/ras.h @@ -11,6 +11,7 @@ #include #include #include +#include /** @file * @defgroup bt_ras Ranging Service API @@ -54,7 +55,38 @@ extern "C" { #define BT_RAS_RANGING_HEADER_LEN 4 #define BT_RAS_SUBEVENT_HEADER_LEN 8 #define BT_RAS_STEP_MODE_LEN 1 -#define BT_RAS_MAX_STEP_DATA_LEN 35 + +#define BT_RAS_MAX_SUBEVENTS_PER_PROCEDURE 32 +#define BT_RAS_MAX_STEPS_PER_PROCEDURE 256 + +#define BT_RAS_STEP_MODE_2_3_ANT_DEPENDENT_LEN(antenna_paths) \ + ((antenna_paths + 1) * sizeof(struct bt_hci_le_cs_step_data_tone_info)) + +#define BT_RAS_STEP_MODE_0_MAX_LEN \ + MAX(sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator), \ + sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector)) +#define BT_RAS_STEP_MODE_1_MAX_LEN (sizeof(struct bt_hci_le_cs_step_data_mode_1)) +#define BT_RAS_STEP_MODE_2_MAX_LEN \ + (sizeof(struct bt_hci_le_cs_step_data_mode_2) + \ + BT_RAS_STEP_MODE_2_3_ANT_DEPENDENT_LEN(CONFIG_BT_RAS_MAX_ANTENNA_PATHS)) +#define BT_RAS_STEP_MODE_3_MAX_LEN \ + (sizeof(struct bt_hci_le_cs_step_data_mode_3) + \ + BT_RAS_STEP_MODE_2_3_ANT_DEPENDENT_LEN(CONFIG_BT_RAS_MAX_ANTENNA_PATHS)) + +#define BT_RAS_STEP_MODE_0_1_MAX_LEN MAX(BT_RAS_STEP_MODE_0_MAX_LEN, BT_RAS_STEP_MODE_1_MAX_LEN) +#define BT_RAS_STEP_MODE_0_1_2_MAX_LEN MAX(BT_RAS_STEP_MODE_0_1_MAX_LEN, BT_RAS_STEP_MODE_2_MAX_LEN) + +#if defined(CONFIG_BT_RAS_MODE_3_SUPPORTED) +#define BT_RAS_MAX_STEP_DATA_LEN MAX(BT_RAS_STEP_MODE_0_1_2_MAX_LEN, BT_RAS_STEP_MODE_3_MAX_LEN) +#else +#define BT_RAS_MAX_STEP_DATA_LEN BT_RAS_STEP_MODE_0_1_2_MAX_LEN +#endif + +#define BT_RAS_PROCEDURE_MEM \ + (BT_RAS_RANGING_HEADER_LEN + \ + (BT_RAS_MAX_SUBEVENTS_PER_PROCEDURE * BT_RAS_SUBEVENT_HEADER_LEN) + \ + (BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_STEP_MODE_LEN) + \ + (BT_RAS_MAX_STEPS_PER_PROCEDURE * BT_RAS_MAX_STEP_DATA_LEN)) /** @brief Ranging Header structure as defined in RAS Specification, Table 3.7. */ struct ras_ranging_header { @@ -130,6 +162,156 @@ struct ras_subevent_header { } __packed; BUILD_ASSERT(sizeof(struct ras_subevent_header) == BT_RAS_SUBEVENT_HEADER_LEN); +/** @brief RAS Ranging Data Buffer callback structure. */ +struct bt_ras_rd_buffer_cb { + /** @brief New ranging data has been received from the local controller. + * + * This callback notifies the application that the ranging data buffer + * has reassembled a complete ranging procedure from the local controller. + * + * @param conn Connection object. + * @param ranging_counter Ranging counter of the stored procedure. + */ + void (*new_ranging_data_received)(struct bt_conn *conn, uint16_t ranging_counter); + + /** @brief Ranging data has been overwritten. + * + * This callback notifies the application that the ranging data buffer + * has overwritten a stored procedure due to running out of buffers + * to store a newer procedure from the local controller. + * + * @param conn Connection object. + * @param ranging_counter Ranging counter of the overwritten procedure. + */ + void (*ranging_data_overwritten)(struct bt_conn *conn, uint16_t ranging_counter); + + sys_snode_t node; +}; + +/** @brief RAS Ranging Data buffer structure. + * + * Provides storage and metadata to store a complete Ranging Data body + * as defined in RAS Specification, Section 3.2.1.2. + * Buffers can be accessed by the application and RRSP concurrently, and will not + * be overwritten while any references are held via @ref bt_ras_rd_buffer_claim. + * + * @note The following CS subevent fields are not included by specification: + * subevent count, step channel, step length. + */ +struct ras_rd_buffer { + /** Connection with an RRSP instance owning this buffer. */ + struct bt_conn *conn; + /** CS Procedure Ranging Counter stored in this buffer. */ + uint16_t ranging_counter; + /** Write cursor into the procedure subevent buffer. */ + uint16_t subevent_cursor; + /** Reference counter for buffer. + * The buffer will not be overwritten with active references. + */ + atomic_t refcount; + /** All ranging data has been written, buffer is ready to send. */ + bool ready; + /** Ranging data is being written to this buffer. */ + bool busy; + /** The peer has ACKed this buffer, the overwritten callback will not be called. */ + bool acked; + /** Complete ranging data procedure buffer. */ + union { + uint8_t buf[BT_RAS_PROCEDURE_MEM]; + struct { + struct ras_ranging_header ranging_header; + uint8_t subevents[]; + } __packed; + } procedure; +}; + +/** @brief Allocate Ranging Responder instance for connection. + * + * This will allocate an instance of the Ranging Responder service for the given connection. + * + * @note This method must not be called if CONFIG_BT_RAS_RRSP_AUTO_ALLOC_INSTANCE is enabled. + * @note The number of supported instances can be set using CONFIG_BT_RAS_RRSP_MAX_ACTIVE_CONN. + * + * @param conn Connection instance. + * + * @return Zero in case of success and error code in case of error. + */ +int bt_ras_rrsp_alloc(struct bt_conn *conn); + +/** @brief Free Ranging Responder instance for connection. + * + * This will free an allocated instance of the Ranging Responder service for + * the given connection, if one has been allocated. + * If the connection has no instance allocated, this method has no effect. + * + * @note This method must not be called if CONFIG_BT_RAS_RRSP_AUTO_ALLOC_INSTANCE is enabled. + * + * @param conn Connection instance. + */ +void bt_ras_rrsp_free(struct bt_conn *conn); + +/** @brief Register ranging data buffer callbacks. + * + * Register callbacks to monitor ranging data buffer state. + * + * @param cb Callback struct. Must point to memory that remains valid. + */ +void bt_ras_rd_buffer_cb_register(struct bt_ras_rd_buffer_cb *cb); + +/** @brief Check if a given ranging counter is available. + * + * Checks if the given ranging counter is stored in the buffer and + * has a valid complete ranging data body stored. + * + * @param conn Connection instance. + * @param ranging_counter CS procedure ranging counter. + * + * @retval true A buffer storing this ranging counter exists and can be claimed. + * @retval false A buffer storing this ranging counter does not exist. + */ +bool bt_ras_rd_buffer_ready_check(struct bt_conn *conn, uint16_t ranging_counter); + +/** @brief Claim a buffer with a given ranging counter. + * + * Returns a pointer to a buffer storing a valid complete ranging data body with + * the requested procedure counter, and increments its reference counter. + * + * @param conn Connection instance. + * @param ranging_counter CS procedure ranging counter. + * + * @return Pointer to ranging data buffer structure or NULL if no such buffer exists. + */ +struct ras_rd_buffer *bt_ras_rd_buffer_claim(struct bt_conn *conn, uint16_t ranging_counter); + +/** @brief Release a claimed ranging data buffer. + * + * Returns a buffer and decrements its reference counter. + * The buffer will stay available until overwritten by newer ranging data, if + * it has no remaining references. + * + * @param buf Pointer to claimed ranging data buffer. + * + * @retval 0 Success. + * @retval -EINVAL Invalid buffer provided. + */ +int bt_ras_rd_buffer_release(struct ras_rd_buffer *buf); + +/** @brief Pull bytes from a ranging data buffer. + * + * Utility method to consume up to max_data_len bytes from a buffer. + * The provided read_cursor will be used as the initial offset and updated. + * + * @param buf Pointer to claimed ranging data buffer. + * @param out_buf Destination to copy up to max_data_len bytes to. + * @param max_data_len Maximum amount of bytes to copy from the buffer. + * @param read_cursor Current offset into procedure subevent buffer, will be read and written to. + * @param empty Set to true if all data has been read from the ranging data buffer. + * + * @return Number of bytes written into out_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); + #ifdef __cplusplus } #endif diff --git a/subsys/bluetooth/services/ras/Kconfig.ras b/subsys/bluetooth/services/ras/Kconfig.ras index 9035f4fcf831..d919295f1ac7 100644 --- a/subsys/bluetooth/services/ras/Kconfig.ras +++ b/subsys/bluetooth/services/ras/Kconfig.ras @@ -4,10 +4,34 @@ # SPDX-License-Identifier: LicenseRef-Nordic-5-Clause # -config BT_RAS - bool +menuconfig BT_RAS + bool "Ranging Service [EXPERIMENTAL]" + depends on BT_CHANNEL_SOUNDING + select BT_NRF_SERVICES + select EXPERIMENTAL help - Common Bluetooth GATT Ranging Service modules. + Bluetooth GATT Ranging Service modules - RREQ and RRSP. + +if BT_RAS rsource "rreq/Kconfig.ras_rreq" rsource "rrsp/Kconfig.ras_rrsp" + +config BT_RAS_MAX_ANTENNA_PATHS + int "Maximum number of antenna paths supported" + default 4 + range 1 4 + help + The number of antenna paths per step that can be stored inside RAS. + Must match the supported CS capabilities of the local device. + This affects the per-instance memory usage of RAS. + +config BT_RAS_MODE_3_SUPPORTED + bool "Support storing Mode 3 CS steps" + default y + help + If enabled, RAS will allocate memory for storing Mode 3 CS steps. + Must match the supported CS capabilities of the local device. + This affects the per-instance memory usage of RAS. + +endif # BT_RAS diff --git a/subsys/bluetooth/services/ras/ras_internal.h b/subsys/bluetooth/services/ras/ras_internal.h index c176d0dc5791..3456617bf4fa 100644 --- a/subsys/bluetooth/services/ras/ras_internal.h +++ b/subsys/bluetooth/services/ras/ras_internal.h @@ -22,6 +22,7 @@ extern "C" { #define RASCP_CMD_PARAMS_OFFSET RASCP_CMD_OPCODE_LEN #define RASCP_CMD_PARAMS_MAX_LEN 4 #define RASCP_WRITE_MAX_LEN (RASCP_CMD_OPCODE_LEN + RASCP_CMD_PARAMS_MAX_LEN) +#define RASCP_ACK_DATA_TIMEOUT K_SECONDS(5) /** @brief RAS Control Point opcodes as defined in RAS Specification, Table 3.10. */ enum rascp_opcode { diff --git a/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq b/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq index 8665216da550..cb58dfc43e84 100644 --- a/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq +++ b/subsys/bluetooth/services/ras/rreq/Kconfig.ras_rreq @@ -5,11 +5,8 @@ # menuconfig BT_RAS_RREQ - bool "Enable GATT Ranging Requester Client [EXPERIMENTAL]" - depends on BT_CHANNEL_SOUNDING + bool "GATT Ranging Requester Client [EXPERIMENTAL]" select EXPERIMENTAL - select BT_NRF_SERVICES - select BT_RAS if BT_RAS_RREQ diff --git a/subsys/bluetooth/services/ras/rrsp/CMakeLists.txt b/subsys/bluetooth/services/ras/rrsp/CMakeLists.txt index b7f2fe3568e7..c5d3255233ca 100644 --- a/subsys/bluetooth/services/ras/rrsp/CMakeLists.txt +++ b/subsys/bluetooth/services/ras/rrsp/CMakeLists.txt @@ -6,4 +6,5 @@ zephyr_library_sources_ifdef( CONFIG_BT_RAS_RRSP - ras_rrsp.c) + ras_rrsp.c + ras_rd_buffer.c) diff --git a/subsys/bluetooth/services/ras/rrsp/Kconfig.ras_rrsp b/subsys/bluetooth/services/ras/rrsp/Kconfig.ras_rrsp index ea0818e25864..4ef48d7b99f0 100644 --- a/subsys/bluetooth/services/ras/rrsp/Kconfig.ras_rrsp +++ b/subsys/bluetooth/services/ras/rrsp/Kconfig.ras_rrsp @@ -5,14 +5,29 @@ # menuconfig BT_RAS_RRSP - bool "Enable GATT Ranging Responder Server [EXPERIMENTAL]" - depends on BT_CHANNEL_SOUNDING + bool "GATT Ranging Responder Server [EXPERIMENTAL]" select EXPERIMENTAL - select BT_NRF_SERVICES - select BT_RAS if BT_RAS_RRSP +config BT_RAS_RRSP_AUTO_ALLOC_INSTANCE + bool "Automatically allocate RRSP instances to new connections" + default y + +config BT_RAS_RRSP_MAX_ACTIVE_CONN + int "Number of simultaneously supported RRSP instances" + default BT_MAX_CONN + range 1 BT_MAX_CONN + help + The number of simultaneous connections with an instance of RAS RRSP + +config BT_RAS_RRSP_RD_BUFFERS_PER_CONN + int "Number of ranging data buffers per connection" + default 1 + range 1 10 + help + The number of ranging procedures that can be stored inside RRSP. + module = BT_RAS_RRSP module-str = RAS_RRSP source "${ZEPHYR_BASE}/subsys/logging/Kconfig.template.log_config" diff --git a/subsys/bluetooth/services/ras/rrsp/ras_rd_buffer.c b/subsys/bluetooth/services/ras/rrsp/ras_rd_buffer.c new file mode 100644 index 000000000000..dbcc36904d47 --- /dev/null +++ b/subsys/bluetooth/services/ras/rrsp/ras_rd_buffer.c @@ -0,0 +1,351 @@ +/* + * Copyright (c) 2024 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../ras_internal.h" + +LOG_MODULE_DECLARE(ras_rrsp, CONFIG_BT_RAS_RRSP_LOG_LEVEL); + +#define RD_POOL_SIZE (CONFIG_BT_RAS_RRSP_MAX_ACTIVE_CONN * CONFIG_BT_RAS_RRSP_RD_BUFFERS_PER_CONN) +#define DROP_PROCEDURE_COUNTER_EMPTY (-1) + +BUILD_ASSERT(RD_POOL_SIZE <= UINT8_MAX); + +static struct ras_rd_buffer rd_buffer_pool[RD_POOL_SIZE]; +static int8_t tx_power_cache[CONFIG_BT_MAX_CONN]; +static int32_t drop_procedure_counter[CONFIG_BT_MAX_CONN]; +static sys_slist_t callback_list = SYS_SLIST_STATIC_INIT(&callback_list); + +static void notify_new_rd_stored(struct bt_conn *conn, uint16_t ranging_counter) +{ + struct bt_ras_rd_buffer_cb *cb; + + SYS_SLIST_FOR_EACH_CONTAINER(&callback_list, cb, node) { + if (cb->new_ranging_data_received) { + cb->new_ranging_data_received(conn, ranging_counter); + } + } +} + +static void notify_rd_overwritten(struct bt_conn *conn, uint16_t ranging_counter) +{ + struct bt_ras_rd_buffer_cb *cb; + + SYS_SLIST_FOR_EACH_CONTAINER(&callback_list, cb, node) { + if (cb->ranging_data_overwritten) { + cb->ranging_data_overwritten(conn, ranging_counter); + } + } +} + +static struct ras_rd_buffer *rd_buffer_get(struct bt_conn *conn, uint16_t ranging_counter, + bool ready, bool busy) +{ + for (uint8_t i = 0; i < ARRAY_SIZE(rd_buffer_pool); i++) { + if (rd_buffer_pool[i].conn == conn && + rd_buffer_pool[i].ranging_counter == ranging_counter && + rd_buffer_pool[i].ready == ready && rd_buffer_pool[i].busy == busy) { + return &rd_buffer_pool[i]; + } + } + + return NULL; +} + +static void rd_buffer_init(struct bt_conn *conn, struct ras_rd_buffer *buf, + uint16_t ranging_counter) +{ + buf->conn = bt_conn_ref(conn); + buf->ranging_counter = ranging_counter; + buf->ready = false; + buf->busy = true; + buf->acked = false; + buf->subevent_cursor = 0; + atomic_clear(&buf->refcount); +} + +static void rd_buffer_free(struct ras_rd_buffer *buf) +{ + if (buf->conn) { + bt_conn_unref(buf->conn); + } + + buf->conn = NULL; + buf->ready = false; + buf->busy = false; + buf->acked = false; + buf->refcount = 0; + buf->subevent_cursor = 0; + atomic_clear(&buf->refcount); +} + +static struct ras_rd_buffer *rd_buffer_alloc(struct bt_conn *conn, uint16_t ranging_counter) +{ + uint16_t conn_buffer_count = 0; + uint16_t oldest_ranging_counter = UINT16_MAX; + struct ras_rd_buffer *available_free_buffer = NULL; + struct ras_rd_buffer *available_oldest_buffer = NULL; + + for (uint8_t i = 0; i < ARRAY_SIZE(rd_buffer_pool); i++) { + if (rd_buffer_pool[i].conn == conn) { + conn_buffer_count++; + + /* Only overwrite buffers that have ranging data stored + * and are not being read. + */ + if (rd_buffer_pool[i].ready && !rd_buffer_pool[i].busy && + atomic_get(&rd_buffer_pool[i].refcount) == 0 && + rd_buffer_pool[i].ranging_counter < oldest_ranging_counter) { + oldest_ranging_counter = rd_buffer_pool[i].ranging_counter; + available_oldest_buffer = &rd_buffer_pool[i]; + } + } + + if (available_free_buffer == NULL && rd_buffer_pool[i].conn == NULL) { + available_free_buffer = &rd_buffer_pool[i]; + } + } + + /* Allocate the buffer straight away if the connection has not reached + * the maximum number of buffers allocated. + */ + if (conn_buffer_count < CONFIG_BT_RAS_RRSP_RD_BUFFERS_PER_CONN) { + rd_buffer_init(conn, available_free_buffer, ranging_counter); + + return available_free_buffer; + } + + /* Overwrite the oldest stored ranging buffer that is not in use */ + if (available_oldest_buffer != NULL) { + if (!available_oldest_buffer->acked) { + /* Only notify if the peer has not read the buffer yet. */ + notify_rd_overwritten(conn, oldest_ranging_counter); + } + rd_buffer_free(available_oldest_buffer); + + rd_buffer_init(conn, available_oldest_buffer, ranging_counter); + + return available_oldest_buffer; + } + + /* Could not allocate a buffer */ + return NULL; +} + +static void cs_procedure_enabled(struct bt_conn *conn, + struct bt_conn_le_cs_procedure_enable_complete *params) +{ + uint8_t conn_index = bt_conn_index(conn); + + __ASSERT_NO_MSG(conn_index < ARRAY_SIZE(tx_power_cache)); + + tx_power_cache[conn_index] = params->selected_tx_power; + drop_procedure_counter[conn_index] = DROP_PROCEDURE_COUNTER_EMPTY; +} + +static bool process_step_data(struct bt_le_cs_subevent_step *step, void *user_data) +{ + struct ras_rd_buffer *buf = (struct ras_rd_buffer *)user_data; + + uint16_t buffer_len = buf->subevent_cursor + BT_RAS_STEP_MODE_LEN + step->data_len; + uint16_t buffer_size = BT_RAS_PROCEDURE_MEM - BT_RAS_RANGING_HEADER_LEN; + + if (buffer_len > buffer_size) { + uint8_t conn_index = bt_conn_index(buf->conn); + + __ASSERT_NO_MSG(conn_index < ARRAY_SIZE(drop_procedure_counter)); + LOG_ERR("Out of buffer space: attempted to store %u bytes, buffer size: %u", + buffer_len, buffer_size); + drop_procedure_counter[conn_index] = buf->ranging_counter; + + return false; + } + + buf->procedure.subevents[buf->subevent_cursor] = step->mode; + buf->subevent_cursor += BT_RAS_STEP_MODE_LEN; + + memcpy(&buf->procedure.subevents[buf->subevent_cursor], step->data, step->data_len); + buf->subevent_cursor += step->data_len; + + return true; +} + +static void subevent_data_available(struct bt_conn *conn, + struct bt_conn_le_cs_subevent_result *result) +{ + LOG_DBG("Procedure %u", result->header.procedure_counter); + struct ras_rd_buffer *buf = + rd_buffer_get(conn, result->header.procedure_counter, false, true); + + uint8_t conn_index = bt_conn_index(conn); + + __ASSERT_NO_MSG(conn_index < ARRAY_SIZE(tx_power_cache)); + __ASSERT_NO_MSG(conn_index < ARRAY_SIZE(drop_procedure_counter)); + + if (result->header.subevent_done_status == BT_CONN_LE_CS_SUBEVENT_ABORTED) { + /* If this subevent was aborted, drop the entire procedure for now. */ + drop_procedure_counter[conn_index] = result->header.procedure_counter; + } + + if (drop_procedure_counter[conn_index] == result->header.procedure_counter) { + /* This procedure will not be sent to the peer, so ignore all data. */ + LOG_WRN("Dropping subevent data for procedure %u", + result->header.procedure_counter); + + if (buf) { + rd_buffer_free(buf); + } + + return; + } + + drop_procedure_counter[conn_index] = DROP_PROCEDURE_COUNTER_EMPTY; + + if (!buf) { + /* First subevent - allocate a buffer */ + buf = rd_buffer_alloc(conn, result->header.procedure_counter); + + if (!buf) { + LOG_ERR("Failed to allocate buffer for procedure %u", + result->header.procedure_counter); + drop_procedure_counter[conn_index] = result->header.procedure_counter; + + return; + } + + buf->procedure.ranging_header.ranging_counter = result->header.procedure_counter; + buf->procedure.ranging_header.config_id = result->header.config_id; + buf->procedure.ranging_header.selected_tx_power = tx_power_cache[conn_index]; + buf->procedure.ranging_header.antenna_paths_mask = + BIT_MASK(result->header.num_antenna_paths); + } + + struct ras_subevent_header *hdr = + (struct ras_subevent_header *)&buf->procedure.subevents[buf->subevent_cursor]; + uint16_t buffer_size = BT_RAS_PROCEDURE_MEM - BT_RAS_RANGING_HEADER_LEN; + + buf->subevent_cursor += sizeof(struct ras_subevent_header); + + if (buf->subevent_cursor > buffer_size) { + LOG_ERR("Out of buffer space: attempted to store %u bytes, buffer size: %u", + buf->subevent_cursor, buffer_size); + drop_procedure_counter[conn_index] = buf->ranging_counter; + + rd_buffer_free(buf); + + return; + } + + hdr->start_acl_conn_event = result->header.start_acl_conn_event; + hdr->freq_compensation = result->header.frequency_compensation; + hdr->ranging_done_status = result->header.procedure_done_status; + hdr->subevent_done_status = result->header.subevent_done_status; + hdr->ranging_abort_reason = result->header.procedure_abort_reason; + hdr->subevent_abort_reason = result->header.subevent_abort_reason; + hdr->ref_power_level = result->header.reference_power_level; + hdr->num_steps_reported = result->header.num_steps_reported; + + if (result->step_data_buf) { + bt_le_cs_step_data_parse(result->step_data_buf, process_step_data, buf); + } + + /* process_step_data might have requested dropping this procedure. */ + bool drop = (drop_procedure_counter[conn_index] == result->header.procedure_counter); + + if (hdr->ranging_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE && !drop) { + buf->ready = true; + buf->busy = false; + notify_new_rd_stored(conn, result->header.procedure_counter); + } else { + rd_buffer_free(buf); + } +} + +static void disconnected(struct bt_conn *conn, uint8_t reason) +{ + ARG_UNUSED(reason); + + for (uint8_t i = 0; i < ARRAY_SIZE(rd_buffer_pool); i++) { + if (rd_buffer_pool[i].conn == conn) { + rd_buffer_free(&rd_buffer_pool[i]); + } + } +} + +BT_CONN_CB_DEFINE(conn_callbacks) = { + .le_cs_procedure_enabled = cs_procedure_enabled, + .le_cs_subevent_data_available = subevent_data_available, + .disconnected = disconnected, +}; + +void bt_ras_rd_buffer_cb_register(struct bt_ras_rd_buffer_cb *cb) +{ + sys_slist_append(&callback_list, &cb->node); +} + +bool bt_ras_rd_buffer_ready_check(struct bt_conn *conn, uint16_t ranging_counter) +{ + struct ras_rd_buffer *buf = rd_buffer_get(conn, ranging_counter, true, false); + + return buf != NULL; +} + +struct ras_rd_buffer *bt_ras_rd_buffer_claim(struct bt_conn *conn, uint16_t ranging_counter) +{ + struct ras_rd_buffer *buf = rd_buffer_get(conn, ranging_counter, true, false); + + if (buf) { + atomic_inc(&buf->refcount); + return buf; + } + + return NULL; +} + +int bt_ras_rd_buffer_release(struct ras_rd_buffer *buf) +{ + if (!buf || atomic_get(&buf->refcount) == 0) { + return -EINVAL; + } + + atomic_dec(&buf->refcount); + + /* Not freeing the buffer as it may be requested again by the app. + * It will get overwritten when new subevent data is available. + */ + + return 0; +} + +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) +{ + if (!buf->ready) { + return 0; + } + + uint16_t buf_len = sizeof(struct ras_ranging_header) + buf->subevent_cursor; + uint16_t remaining = buf_len - (*read_cursor); + uint16_t pull_bytes = MIN(max_data_len, remaining); + + __ASSERT_NO_MSG(*read_cursor <= buf_len); + + memcpy(out_buf, &buf->procedure.buf[*read_cursor], pull_bytes); + *read_cursor += pull_bytes; + *empty = (remaining == pull_bytes); + + return pull_bytes; +} diff --git a/subsys/bluetooth/services/ras/rrsp/ras_rrsp.c b/subsys/bluetooth/services/ras/rrsp/ras_rrsp.c index 467849f9923a..b553227152ba 100644 --- a/subsys/bluetooth/services/ras/rrsp/ras_rrsp.c +++ b/subsys/bluetooth/services/ras/rrsp/ras_rrsp.c @@ -8,21 +8,683 @@ #include #include #include +#include +#include #include +#include "../ras_internal.h" + LOG_MODULE_REGISTER(ras_rrsp, CONFIG_BT_RAS_RRSP_LOG_LEVEL); +#define RRSP_WQ_STACK_SIZE 1024 +#define RRSP_WQ_PRIORITY K_PRIO_PREEMPT(K_LOWEST_APPLICATION_THREAD_PRIO) +K_THREAD_STACK_DEFINE(rrsp_wq_stack_area, RRSP_WQ_STACK_SIZE); + +NET_BUF_SIMPLE_DEFINE_STATIC(segment_buf, CONFIG_BT_L2CAP_TX_MTU); + +static struct bt_ras_rrsp { + struct bt_conn *conn; + + struct ras_rd_buffer *active_buf; + struct k_work send_data_work; + struct k_work rascp_work; + struct k_work status_work; + struct k_timer rascp_timeout; + + struct bt_gatt_indicate_params ondemand_ind_params; + struct bt_gatt_indicate_params rascp_ind_params; + struct bt_gatt_indicate_params rd_status_params; + + uint8_t rascp_cmd_buf[RASCP_WRITE_MAX_LEN]; + uint8_t rascp_cmd_len; + + uint16_t ready_ranging_counter; + uint16_t overwritten_ranging_counter; + uint16_t segment_counter; + uint16_t active_buf_read_cursor; + + bool streaming; + bool notify_ready; + bool notify_overwritten; + bool handle_rascp_timeout; +} rrsp_pool[CONFIG_BT_RAS_RRSP_MAX_ACTIVE_CONN]; + +static struct k_work_q rrsp_wq; static uint32_t ras_features; /* No optional features supported. */ -static ssize_t ras_features_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, - void *buf, uint16_t len, uint16_t offset) +static void send_data_work_handler(struct k_work *work); +static void rascp_work_handler(struct k_work *work); +static void status_work_handler(struct k_work *work); +static void rascp_timeout_handler(struct k_timer *timer); + +static int ondemand_rd_notify_or_indicate(struct bt_conn *conn, struct net_buf_simple *buf); +static int rd_status_notify_or_indicate(struct bt_conn *conn, const struct bt_uuid *uuid, + uint16_t ranging_counter); + +static void rascp_send_complete_rd_rsp(struct bt_conn *conn, uint16_t ranging_counter); +static void rascp_cmd_handle(struct bt_ras_rrsp *rrsp); + +static struct bt_ras_rrsp *rrsp_find(struct bt_conn *conn) +{ + for (size_t i = 0; i < ARRAY_SIZE(rrsp_pool); i++) { + struct bt_ras_rrsp *rrsp = &rrsp_pool[i]; + + if (rrsp->conn == conn) { + return rrsp; + } + } + + return NULL; +} + +int bt_ras_rrsp_alloc(struct bt_conn *conn) +{ + struct bt_ras_rrsp *rrsp = NULL; + + if (rrsp_find(conn) != NULL) { + return -EALREADY; + } + + for (size_t i = 0; i < ARRAY_SIZE(rrsp_pool); i++) { + if (rrsp_pool[i].conn == NULL) { + rrsp = &rrsp_pool[i]; + break; + } + } + + if (rrsp == NULL) { + return -ENOMEM; + } + + LOG_DBG("conn %p new rrsp %p", (void *)conn, (void *)rrsp); + + memset(rrsp, 0, sizeof(struct bt_ras_rrsp)); + rrsp->conn = bt_conn_ref(conn); + + k_work_init(&rrsp->send_data_work, send_data_work_handler); + k_work_init(&rrsp->rascp_work, rascp_work_handler); + k_work_init(&rrsp->status_work, status_work_handler); + k_timer_init(&rrsp->rascp_timeout, rascp_timeout_handler, NULL); + + return 0; +} + +void bt_ras_rrsp_free(struct bt_conn *conn) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (rrsp) { + LOG_DBG("conn %p rrsp %p", (void *)rrsp->conn, (void *)rrsp); + + (void)k_work_cancel(&rrsp->send_data_work); + (void)k_work_cancel(&rrsp->rascp_work); + (void)k_work_cancel(&rrsp->status_work); + k_timer_stop(&rrsp->rascp_timeout); + + k_work_queue_drain(&rrsp_wq, false); + + bt_conn_unref(rrsp->conn); + rrsp->conn = NULL; + } +} + +static ssize_t ras_features_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) { return bt_gatt_attr_read(conn, attr, buf, len, offset, &ras_features, sizeof(ras_features)); } +static ssize_t rd_ready_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (!rrsp) { + return BT_GATT_ERR(BT_ATT_ERR_READ_NOT_PERMITTED); + } + + return bt_gatt_attr_read(conn, attr, buf, len, offset, &rrsp->ready_ranging_counter, + sizeof(uint16_t)); +} + +static ssize_t rd_overwritten_read(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, + uint16_t len, uint16_t offset) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (!rrsp) { + return BT_GATT_ERR(BT_ATT_ERR_READ_NOT_PERMITTED); + } + + return bt_gatt_attr_read(conn, attr, buf, len, offset, &rrsp->overwritten_ranging_counter, + sizeof(uint16_t)); +} + +static ssize_t ras_cp_write(struct bt_conn *conn, struct bt_gatt_attr const *attr, void const *buf, + uint16_t len, uint16_t offset, uint8_t flags) +{ + LOG_HEXDUMP_DBG(buf, len, "Write request"); + + if (!bt_gatt_is_subscribed(conn, attr, BT_GATT_CCC_INDICATE)) { + LOG_DBG("Not subscribed"); + return BT_GATT_ERR(RAS_ATT_ERROR_CCC_CONFIG); + } + + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (!rrsp || k_work_is_pending(&rrsp->rascp_work) || len > RASCP_WRITE_MAX_LEN) { + LOG_DBG("Write rejected"); + return BT_GATT_ERR(RAS_ATT_ERROR_WRITE_REQ_REJECTED); + } + + memcpy(rrsp->rascp_cmd_buf, buf, len); + rrsp->rascp_cmd_len = (uint8_t)len; + + k_work_submit_to_queue(&rrsp_wq, &rrsp->rascp_work); + + return len; +} + +static void ondemand_rd_ccc_cfg_changed(struct bt_gatt_attr const *attr, uint16_t value) +{ + LOG_DBG("On-demand Ranging Data CCCD changed: %u", value); +} + +static void ras_cp_ccc_cfg_changed(struct bt_gatt_attr const *attr, uint16_t value) +{ + LOG_DBG("RAS-CP CCCD changed: %u", value); +} + +static void rd_ready_ccc_cfg_changed(struct bt_gatt_attr const *attr, uint16_t value) +{ + LOG_DBG("Ranging Data Ready CCCD changed: %u", value); +} + +static void rd_overwritten_ccc_cfg_changed(struct bt_gatt_attr const *attr, uint16_t value) +{ + LOG_DBG("Ranging Data Overwritten CCCD changed: %u", value); +} + +#if defined(CONFIG_BT_RAS_RRSP_AUTO_ALLOC_INSTANCE) +static void connected(struct bt_conn *conn, uint8_t err) +{ + char addr[BT_ADDR_LE_STR_LEN]; + + if (err) { + return; + } + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + LOG_DBG("Allocating RRSP for %s\n", addr); + + bt_ras_rrsp_alloc(conn); +} + +static void disconnected(struct bt_conn *conn, uint8_t reason) +{ + char addr[BT_ADDR_LE_STR_LEN]; + + bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr)); + LOG_DBG("Freeing RRSP for %s (reason 0x%02x)", addr, reason); + + bt_ras_rrsp_free(conn); +} + +BT_CONN_CB_DEFINE(conn_callbacks) = { + .connected = connected, + .disconnected = disconnected, +}; +#endif + +static int rd_segment_send(struct bt_ras_rrsp *rrsp) +{ + int err; + + __ASSERT_NO_MSG(rrsp->conn); + + /* By spec, up to (ATT_MTU-4) octets of the data to be transported + * shall be used to fill the characteristic message. + * An extra byte is reserved for the segment header + */ + uint16_t max_data_len = bt_gatt_get_mtu(rrsp->conn) - (4 + sizeof(struct ras_seg_header)); + + /* segment_buf is only accessed by the RRSP WQ, so is safe to reuse. */ + net_buf_simple_reset(&segment_buf); + + struct ras_segment *ras_segment = + net_buf_simple_add(&segment_buf, sizeof(struct ras_segment) + max_data_len); + if (!ras_segment) { + LOG_ERR("Cannot allocate segment buffer"); + return -ENOMEM; + } + + bool first_seg = (rrsp->active_buf_read_cursor == 0); + bool last_seg; + uint16_t actual_data_len = + bt_ras_rd_buffer_bytes_pull(rrsp->active_buf, ras_segment->data, max_data_len, + &rrsp->active_buf_read_cursor, &last_seg); + + LOG_DBG("Got %u bytes (max: %u)", actual_data_len, max_data_len); + + if (actual_data_len) { + ras_segment->header.first_seg = first_seg; + ras_segment->header.last_seg = last_seg; + ras_segment->header.seg_counter = rrsp->segment_counter & BIT_MASK(6); + + (void)net_buf_simple_remove_mem(&segment_buf, (max_data_len - actual_data_len)); + + err = ondemand_rd_notify_or_indicate(rrsp->conn, &segment_buf); + if (err) { + LOG_WRN("ondemand_rd_notify_or_indicate failed err %d", err); + + /* Keep retrying */ + rrsp->active_buf_read_cursor -= actual_data_len; + + return err; + } + + rrsp->segment_counter++; + + LOG_DBG("Segment with RSC %d sent", rrsp->segment_counter); + } + + if (!last_seg) { + k_work_submit_to_queue(&rrsp_wq, &rrsp->send_data_work); + } else { + LOG_DBG("All segments sent"); + rascp_send_complete_rd_rsp(rrsp->conn, rrsp->active_buf->ranging_counter); + rrsp->streaming = false; + k_work_cancel(&rrsp->send_data_work); + k_timer_start(&rrsp->rascp_timeout, RASCP_ACK_DATA_TIMEOUT, K_NO_WAIT); + } + + return 0; +} + +static void send_data_work_handler(struct k_work *work) +{ + struct bt_ras_rrsp *rrsp = CONTAINER_OF(work, struct bt_ras_rrsp, send_data_work); + + if (!rrsp->streaming || !rrsp->active_buf) { + return; + } + + int err = rd_segment_send(rrsp); + + if (err) { + /* Will keep retrying. */ + LOG_WRN("Failed to send segment: %d", err); + } +} + +static void rascp_work_handler(struct k_work *work) +{ + struct bt_ras_rrsp *rrsp = CONTAINER_OF(work, struct bt_ras_rrsp, rascp_work); + + LOG_DBG("rrsp %p", rrsp); + + rascp_cmd_handle(rrsp); +} + +static void status_work_handler(struct k_work *work) +{ + struct bt_ras_rrsp *rrsp = CONTAINER_OF(work, struct bt_ras_rrsp, status_work); + + LOG_DBG("rrsp %p", rrsp); + + if (rrsp->handle_rascp_timeout) { + /* The procedure is considered to have failed if the peer does not ACK + * the data within 5 seconds of rascp_send_complete_rd_rsp being called. + */ + (void)bt_ras_rd_buffer_release(rrsp->active_buf); + rrsp->active_buf = NULL; + rrsp->active_buf_read_cursor = 0; + rrsp->handle_rascp_timeout = false; + } + + if (rrsp->notify_overwritten) { + int err = rd_status_notify_or_indicate(rrsp->conn, BT_UUID_RAS_RD_OVERWRITTEN, + rrsp->overwritten_ranging_counter); + if (err) { + LOG_WRN("Notify overwritten failed: %d", err); + } + + rrsp->notify_overwritten = false; + } + + if (rrsp->notify_ready) { + int err = rd_status_notify_or_indicate(rrsp->conn, BT_UUID_RAS_RD_READY, + rrsp->ready_ranging_counter); + if (err) { + LOG_WRN("Notify ready failed: %d", err); + } + + rrsp->notify_ready = false; + } +} + +static void rascp_timeout_handler(struct k_timer *timer) +{ + struct bt_ras_rrsp *rrsp = CONTAINER_OF(timer, struct bt_ras_rrsp, rascp_timeout); + + LOG_DBG("rrsp %p", rrsp); + + LOG_WRN("RAS-CP timeout"); + + rrsp->handle_rascp_timeout = true; + k_work_submit_to_queue(&rrsp_wq, &rrsp->status_work); +} + +static void rd_ready_handle(struct bt_conn *conn, uint16_t ranging_counter) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (rrsp) { + rrsp->ready_ranging_counter = ranging_counter; + rrsp->notify_ready = true; + k_work_submit_to_queue(&rrsp_wq, &rrsp->status_work); + } +} + +static void rd_overwritten_handle(struct bt_conn *conn, uint16_t ranging_counter) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (rrsp) { + rrsp->overwritten_ranging_counter = ranging_counter; + rrsp->notify_overwritten = true; + k_work_submit_to_queue(&rrsp_wq, &rrsp->status_work); + } +} + +static struct bt_ras_rd_buffer_cb rd_buffer_callbacks = { + .new_ranging_data_received = rd_ready_handle, + .ranging_data_overwritten = rd_overwritten_handle, +}; + +static int ras_rrsp_init(void) +{ + const struct k_work_queue_config cfg = {.name = "BT RAS RRSP WQ"}; + + k_work_queue_init(&rrsp_wq); + k_work_queue_start(&rrsp_wq, rrsp_wq_stack_area, K_THREAD_STACK_SIZEOF(rrsp_wq_stack_area), + RRSP_WQ_PRIORITY, &cfg); + + bt_ras_rd_buffer_cb_register(&rd_buffer_callbacks); + + return 0; +} + +SYS_INIT(ras_rrsp_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY); + BT_GATT_SERVICE_DEFINE(rrsp_svc, BT_GATT_PRIMARY_SERVICE(BT_UUID_RANGING_SERVICE), /* RAS Features */ - BT_GATT_CHARACTERISTIC(BT_UUID_RAS_FEATURES, BT_GATT_CHRC_READ, + BT_GATT_CHARACTERISTIC(BT_UUID_RAS_FEATURES, + BT_GATT_CHRC_READ, BT_GATT_PERM_READ_ENCRYPT, ras_features_read, NULL, NULL), + /* On-demand Ranging Data */ + BT_GATT_CHARACTERISTIC(BT_UUID_RAS_ONDEMAND_RD, + BT_GATT_CHRC_INDICATE | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_NONE, + NULL, NULL, NULL), + BT_GATT_CCC(ondemand_rd_ccc_cfg_changed, + BT_GATT_PERM_READ_ENCRYPT | BT_GATT_PERM_WRITE_ENCRYPT), + /* RAS-CP */ + BT_GATT_CHARACTERISTIC(BT_UUID_RAS_CP, + BT_GATT_CHRC_WRITE_WITHOUT_RESP | BT_GATT_CHRC_INDICATE, + BT_GATT_PERM_WRITE_ENCRYPT, + NULL, ras_cp_write, NULL), + BT_GATT_CCC(ras_cp_ccc_cfg_changed, + BT_GATT_PERM_READ_ENCRYPT | BT_GATT_PERM_WRITE_ENCRYPT), + /* Ranging Data Ready */ + BT_GATT_CHARACTERISTIC(BT_UUID_RAS_RD_READY, + BT_GATT_CHRC_READ | BT_GATT_CHRC_INDICATE | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_READ_ENCRYPT, + rd_ready_read, NULL, NULL), + BT_GATT_CCC(rd_ready_ccc_cfg_changed, + BT_GATT_PERM_READ_ENCRYPT | BT_GATT_PERM_WRITE_ENCRYPT), + /* Ranging Data Overwritten */ + BT_GATT_CHARACTERISTIC(BT_UUID_RAS_RD_OVERWRITTEN, + BT_GATT_CHRC_READ | BT_GATT_CHRC_INDICATE | BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_READ_ENCRYPT, + rd_overwritten_read, NULL, NULL), + BT_GATT_CCC(rd_overwritten_ccc_cfg_changed, + BT_GATT_PERM_READ_ENCRYPT | BT_GATT_PERM_WRITE_ENCRYPT), ); + +static void ondemand_rd_notify_sent_cb(struct bt_conn *conn, void *user_data) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (rrsp) { + LOG_DBG(""); + k_work_submit_to_queue(&rrsp_wq, &rrsp->send_data_work); + } +} + +static void ondemand_rd_indicate_sent_cb(struct bt_conn *conn, + struct bt_gatt_indicate_params *params, uint8_t err) +{ + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + if (rrsp) { + LOG_DBG(""); + k_work_submit_to_queue(&rrsp_wq, &rrsp->send_data_work); + } +} + +static int ondemand_rd_notify_or_indicate(struct bt_conn *conn, struct net_buf_simple *buf) +{ + struct bt_gatt_attr *attr = + bt_gatt_find_by_uuid(rrsp_svc.attrs, 1, BT_UUID_RAS_ONDEMAND_RD); + + __ASSERT_NO_MSG(attr); + + if (bt_gatt_is_subscribed(conn, attr, BT_GATT_CCC_NOTIFY)) { + struct bt_gatt_notify_params params = {0}; + + params.attr = attr; + params.uuid = NULL; + params.data = buf->data; + params.len = buf->len; + params.func = ondemand_rd_notify_sent_cb; + + return bt_gatt_notify_cb(conn, ¶ms); + } else if (bt_gatt_is_subscribed(conn, attr, BT_GATT_CCC_INDICATE)) { + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + __ASSERT_NO_MSG(rrsp); + + rrsp->ondemand_ind_params.attr = attr; + rrsp->ondemand_ind_params.uuid = NULL; + rrsp->ondemand_ind_params.data = buf->data; + rrsp->ondemand_ind_params.len = buf->len; + rrsp->ondemand_ind_params.func = ondemand_rd_indicate_sent_cb; + rrsp->ondemand_ind_params.destroy = NULL; + + return bt_gatt_indicate(conn, &rrsp->ondemand_ind_params); + } + + LOG_WRN("Peer is not subscribed to ranging data characteristic."); + return -EINVAL; +} + +static int rascp_indicate(struct bt_conn *conn, struct net_buf_simple *rsp) +{ + struct bt_gatt_attr *attr = bt_gatt_find_by_uuid(rrsp_svc.attrs, 1, BT_UUID_RAS_CP); + + __ASSERT_NO_MSG(attr); + + if (bt_gatt_is_subscribed(conn, attr, BT_GATT_CCC_INDICATE)) { + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + __ASSERT_NO_MSG(rrsp); + + rrsp->rascp_ind_params.attr = attr; + rrsp->rascp_ind_params.uuid = NULL; + rrsp->rascp_ind_params.data = rsp->data; + rrsp->rascp_ind_params.len = rsp->len; + rrsp->rascp_ind_params.func = NULL; + rrsp->rascp_ind_params.destroy = NULL; + + return bt_gatt_indicate(conn, &rrsp->rascp_ind_params); + } + + LOG_WRN("Peer is not subscribed to RAS-CP."); + return -EINVAL; +} + +static int rd_status_notify_or_indicate(struct bt_conn *conn, const struct bt_uuid *uuid, + uint16_t ranging_counter) +{ + struct bt_gatt_attr *attr = bt_gatt_find_by_uuid(rrsp_svc.attrs, 1, uuid); + + __ASSERT_NO_MSG(attr); + + if (bt_gatt_is_subscribed(conn, attr, BT_GATT_CCC_NOTIFY)) { + return bt_gatt_notify(conn, attr, &ranging_counter, sizeof(ranging_counter)); + } else if (bt_gatt_is_subscribed(conn, attr, BT_GATT_CCC_INDICATE)) { + struct bt_ras_rrsp *rrsp = rrsp_find(conn); + + __ASSERT_NO_MSG(rrsp); + + rrsp->rd_status_params.attr = attr; + rrsp->rd_status_params.uuid = NULL; + rrsp->rd_status_params.data = &ranging_counter; + rrsp->rd_status_params.len = sizeof(ranging_counter); + rrsp->rd_status_params.func = NULL; + rrsp->rd_status_params.destroy = NULL; + + return bt_gatt_indicate(conn, &rrsp->rd_status_params); + } + + LOG_WRN("Peer is not subscribed to status characteristic."); + return -EINVAL; +} + +static void rascp_send_complete_rd_rsp(struct bt_conn *conn, uint16_t ranging_counter) +{ + NET_BUF_SIMPLE_DEFINE(rsp, RASCP_CMD_OPCODE_LEN + RASCP_RSP_OPCODE_COMPLETE_RD_RSP_LEN); + + net_buf_simple_add_u8(&rsp, RASCP_RSP_OPCODE_COMPLETE_RD_RSP); + net_buf_simple_add_le16(&rsp, ranging_counter); + int err = rascp_indicate(conn, &rsp); + + if (err) { + LOG_WRN("Failed to send indication: %d", err); + } +} + +static void rascp_send_rsp_code(struct bt_conn *conn, enum rascp_rsp_code rsp_code) +{ + NET_BUF_SIMPLE_DEFINE(rsp, RASCP_CMD_OPCODE_LEN + RASCP_RSP_OPCODE_RSP_CODE_LEN); + + net_buf_simple_add_u8(&rsp, RASCP_RSP_OPCODE_RSP_CODE); + net_buf_simple_add_u8(&rsp, rsp_code); + + int err = rascp_indicate(conn, &rsp); + + if (err) { + LOG_WRN("Failed to send indication: %d", err); + } +} + +static void rascp_cmd_handle(struct bt_ras_rrsp *rrsp) +{ + struct net_buf_simple req; + + net_buf_simple_init_with_data(&req, rrsp->rascp_cmd_buf, rrsp->rascp_cmd_len); + + uint8_t opcode = net_buf_simple_pull_u8(&req); + uint8_t param_len = MIN(req.len, RASCP_CMD_PARAMS_MAX_LEN); + + if (rrsp->streaming) { + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_SERVER_BUSY); + return; + } + + switch (opcode) { + case RASCP_OPCODE_GET_RD: { + if (param_len != sizeof(uint16_t)) { + LOG_WRN("Invalid length %d", param_len); + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_INVALID_PARAMETER); + return; + } + + uint16_t ranging_counter = net_buf_simple_pull_le16(&req); + + LOG_DBG("GET_RD %d", ranging_counter); + + struct bt_gatt_attr *attr = + bt_gatt_find_by_uuid(rrsp_svc.attrs, 1, BT_UUID_RAS_ONDEMAND_RD); + + __ASSERT_NO_MSG(attr); + + if (!bt_gatt_is_subscribed(rrsp->conn, attr, BT_GATT_CCC_INDICATE) && + !bt_gatt_is_subscribed(rrsp->conn, attr, BT_GATT_CCC_NOTIFY)) { + /* Ranging data can't be sent to the peer. */ + LOG_WRN("Peer not subscribed to ranging data characteristic."); + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_INVALID_PARAMETER); + return; + } + + if (rrsp->active_buf) { + /* Disallow getting new data until the active one has been ACKed. */ + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_SERVER_BUSY); + return; + } + + if (!bt_ras_rd_buffer_ready_check(rrsp->conn, ranging_counter)) { + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_NO_RECORDS_FOUND); + return; + } + + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_SUCCESS); + + rrsp->active_buf = bt_ras_rd_buffer_claim(rrsp->conn, ranging_counter); + rrsp->active_buf_read_cursor = 0; + rrsp->segment_counter = 0; + rrsp->streaming = true; + + k_work_submit_to_queue(&rrsp_wq, &rrsp->send_data_work); + + break; + } + case RASCP_OPCODE_ACK_RD: { + if (param_len != sizeof(uint16_t)) { + LOG_WRN("Invalid length %d", param_len); + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_INVALID_PARAMETER); + return; + } + + uint16_t ranging_counter = net_buf_simple_pull_le16(&req); + + LOG_DBG("ACK_RD %d", ranging_counter); + + if (!rrsp->active_buf || rrsp->active_buf->ranging_counter != ranging_counter) { + /* Only allow ACKing the currently requested ranging counter. */ + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_NO_RECORDS_FOUND); + return; + } + + k_timer_stop(&rrsp->rascp_timeout); + rrsp->handle_rascp_timeout = false; + + rrsp->active_buf->acked = true; + + bt_ras_rd_buffer_release(rrsp->active_buf); + rrsp->active_buf = NULL; + rrsp->active_buf_read_cursor = 0; + + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_SUCCESS); + + break; + } + default: { + LOG_INF("Opcode %x invalid or unsupported", opcode); + rascp_send_rsp_code(rrsp->conn, RASCP_RESPONSE_OPCODE_NOT_SUPPORTED); + break; + } + } +}