[PATCH BlueZ v4 1/3] src/shared: Add RAS packet formatting and notification support for CS reflector

Prathibha Madugonde
2026-04-27T17:00:17Z

From: Prathibha Madugonde <prathibha.madugonde@oss.qualcomm.com>

Implement complete RAS data pipeline:
- Handle HCI CS subevent result/continuation events
- Serialize ranging/subevent headers per spec
- GATT notifications for real-time ranging data
---
src/shared/rap.c | 1376 +++++++++++++++++++++++++++++++++++++++++++++-
src/shared/rap.h | 4 +-
2 files changed, 1366 insertions(+), 14 deletions(-)

diff --git a/src/shared/rap.c b/src/shared/rap.c
index ac6de04e0..db924b0e9 100644
--- a/src/shared/rap.c
+++ b/src/shared/rap.c
@@ -33,6 +33,170 @@
/* Total number of attribute handles reserved for the RAS service */
#define RAS_TOTAL_NUM_HANDLES 18

+/* 2(rc+cfg) + 1(tx_pwr) + 1(4 bits antenna_mask, 2 bits reserved,
+ * 2 bits pct_format)
+ */
+#define RAS_RANGING_HEADER_SIZE 4
+#define TOTAL_RAS_RANGING_HEADER_SIZE 5
+#define ATT_OVERHEAD 3 /* 1(opcode) + 2(char handle) */
+#define RAS_STEP_ABORTED_BIT 0x80/* set step aborted */
+#define RAS_SUBEVENT_HEADER_SIZE 8
+
+enum pct_format {
+ IQ = 0,
+ PHASE = 1,
+};
+
+enum ranging_done_status {
+ RANGING_DONE_ALL_RESULTS_COMPLETE = 0x0,
+ RANGING_DONE_PARTIAL_RESULTS = 0x1,
+ RANGING_DONE_ABORTED = 0xF,
+};
+
+enum subevent_done_status {
+ SUBEVENT_DONE_ALL_RESULTS_COMPLETE = 0x0,
+ SUBEVENT_DONE_PARTIAL_RESULTS = 0x1,
+ SUBEVENT_DONE_ABORTED = 0xF,
+};
+
+enum ranging_abort_reason {
+ RANGING_ABORT_NO_ABORT = 0x0,
+ RANGING_ABORT_LOCAL_HOST_OR_REMOTE = 0x1,
+ RANGING_ABORT_INSUFFICIENT_FILTERED_CHANNELS = 0x2,
+ RANGING_ABORT_INSTANT_HAS_PASSED = 0x3,
+ RANGING_ABORT_UNSPECIFIED = 0xF,
+};
+
+enum subevent_abort_reason {
+ SUBEVENT_ABORT_NO_ABORT = 0x0,
+ SUBEVENT_ABORT_LOCAL_HOST_OR_REMOTE = 0x1,
+ SUBEVENT_ABORT_NO_CS_SYNC_RECEIVED = 0x2,
+ SUBEVENT_ABORT_SCHEDULING_CONFLICTS_OR_LIMITED_RESOURCES = 0x3,
+ SUBEVENT_ABORT_UNSPECIFIED = 0xF,
+};
+
+/* Segmentation header: 1 byte
+ * bit 0: first_segment
+ * bit 1: last_segment
+ * bits 2-7: rolling_segment_counter (6 bits)
+ */
+struct segmentation_header {
+ uint8_t first_segment;
+ uint8_t last_segment;
+ uint8_t rolling_segment_counter;
+};
+
+/* Macros to pack/unpack segmentation header */
+#define SEG_HDR_PACK(first, last, counter) \
+ ((uint8_t)(((first) ? 0x01 : 0x00) | \
+ ((last) ? 0x02 : 0x00) | \
+ (((counter) & 0x3F) << 2)))
+
+#define SEG_HDR_UNPACK_FIRST(byte) ((byte) & 0x01)
+#define SEG_HDR_UNPACK_LAST(byte) (((byte) >> 1) & 0x01)
+#define SEG_HDR_UNPACK_COUNTER(byte) (((byte) >> 2) & 0x3F)
+
+/* Ranging header: 4 bytes
+ * 0-1: ranging_counter (12 bits) | configuration_id (4 bits)
+ * [little-endian]
+ * byte 2: selected_tx_power (signed)
+ * byte 3: antenna_paths_mask (4 bits) | reserved (2 bits) |
+ * pct_format (2 bits)
+ */
+struct ranging_header {
+ uint16_t ranging_counter;
+ uint8_t configuration_id;
+ int8_t selected_tx_power;
+ uint8_t antenna_paths_mask;
+ uint8_t pct_format;
+} __packed;
+
+/* Macros to pack/unpack ranging header */
+#define RANGING_HDR_PACK_BYTE0_1(rc, cfg) \
+ ((uint16_t)(((rc) & 0x0FFF) | (((cfg) & 0x0F) << 12)))
+
+#define RANGING_HDR_PACK_BYTE3(ant_mask, pct_fmt) \
+ ((uint8_t)(((ant_mask) & 0x0F) | (((pct_fmt) & 0x03) << 6)))
+
+#define RANGING_HDR_UNPACK_RC(byte0_1) \
+ ((uint16_t)((byte0_1) & 0x0FFF))
+
+#define RANGING_HDR_UNPACK_CFG(byte0_1) \
+ ((uint8_t)(((byte0_1) >> 12) & 0x0F))
+
+#define RANGING_HDR_UNPACK_ANT_MASK(byte3) \
+ ((uint8_t)((byte3) & 0x0F))
+
+#define RANGING_HDR_UNPACK_PCT_FMT(byte3) \
+ ((uint8_t)(((byte3) >> 6) & 0x03))
+
+struct ras_subevent_header {
+ uint16_t start_acl_conn_event;
+ uint16_t frequency_compensation;
+ uint8_t ranging_done_status;
+ uint8_t subevent_done_status;
+ uint8_t ranging_abort_reason;
+ uint8_t subevent_abort_reason;
+ int8_t reference_power_level;
+ uint8_t num_steps_reported;
+};
+
+struct ras_subevent {
+ struct ras_subevent_header subevent_header;
+ uint8_t subevent_data[];
+};
+
+/* Role maps to Core CS roles (initiator/reflector) */
+enum cs_role {
+ CS_ROLE_INITIATOR = 0x00,
+ CS_ROLE_REFLECTOR = 0x01,
+};
+
+#define CS_INVALID_CONFIG_ID 0xFF
+/* Minimal enums (align to controller values if needed) */
+enum cs_procedure_done_status {
+ CS_PROC_ALL_RESULTS_COMPLETE = 0x00,
+ CS_PROC_PARTIAL_RESULTS = 0x01,
+ CS_PROC_ABORTED = 0x02
+};
+
+/* Main cs_procedure_data */
+struct cs_procedure_data {
+ /* Identity and counters */
+ uint16_t counter;
+ uint8_t num_antenna_paths;
+ /* Flags and status */
+ enum cs_procedure_done_status local_status;
+ enum cs_procedure_done_status remote_status;
+ bool contains_complete_subevent_;
+ /* RAS aggregation */
+ struct segmentation_header segmentation_header_;
+ struct ranging_header ranging_header_;
+ struct iovec ras_raw_data_; /* raw concatenated */
+ uint16_t ras_raw_data_index_;
+ struct ras_subevent_header ras_subevent_header_;
+ struct iovec ras_subevent_data_; /* buffer per subevent */
+ uint8_t ras_subevent_counter_;
+ /* Reference power levels */
+ int8_t initiator_reference_power_level;
+ int8_t reflector_reference_power_level;
+ bool ranging_header_prepended_;
+ bool ras_subevent_header_emitted;
+};
+
+struct cstracker {
+ enum cs_role role; /* INITIATOR/REFLECTOR */
+ uint8_t config_id; /* CS_INVALID_CONFIG_ID */
+ uint8_t selected_tx_power; /* PROC_ENABLE_COMPLETE */
+ uint8_t rtt_type; /* RTT type */
+ struct cs_procedure_data *current_proc;
+ /* Cached header values for CONT events (per-connection state) */
+ uint16_t last_proc_counter;
+ uint16_t last_start_acl_conn_evt_counter;
+ uint16_t last_freq_comp;
+ int8_t last_ref_pwr_lvl;
+};
+
/* Ranging Service context */
struct ras {
struct bt_rap_db *rapdb;
@@ -43,9 +207,17 @@ struct ras {
struct gatt_db_attribute *realtime_chrc;
struct gatt_db_attribute *realtime_chrc_ccc;
struct gatt_db_attribute *ondemand_chrc;
+ struct gatt_db_attribute *ondemand_ccc;
struct gatt_db_attribute *cp_chrc;
+ struct gatt_db_attribute *cp_ccc;
struct gatt_db_attribute *ready_chrc;
+ struct gatt_db_attribute *ready_ccc;
struct gatt_db_attribute *overwritten_chrc;
+ struct gatt_db_attribute *overwritten_ccc;
+
+ /* CCC state tracking for mutual exclusivity */
+ uint16_t realtime_ccc_value;
+ uint16_t ondemand_ccc_value;
};

struct bt_rap_db {
@@ -70,6 +242,7 @@ struct bt_rap {
bt_rap_destroy_func_t debug_destroy;
void *debug_data;
void *user_data;
+ struct cstracker *resptracker;
};

static struct queue *rap_db;
@@ -90,6 +263,209 @@ struct bt_rap_ready {
void *data;
};

+uint16_t default_ras_mtu = 247; /*Section 3.1.2 of RAP 1.0*/
+uint8_t ras_segment_header_size = 1;
+
+static struct cs_procedure_data *cs_procedure_data_create(
+ uint16_t procedure_counter,
+ uint8_t num_antenna_paths,
+ uint8_t configuration_id,
+ uint8_t selected_tx_power)
+{
+ struct cs_procedure_data *d;
+ uint8_t i;
+
+ d = calloc(1, sizeof(struct cs_procedure_data));
+
+ if (!d)
+ return NULL;
+
+ d->counter = procedure_counter;
+ d->num_antenna_paths = num_antenna_paths;
+ d->local_status = CS_PROC_PARTIAL_RESULTS;
+ d->remote_status = CS_PROC_PARTIAL_RESULTS;
+ d->contains_complete_subevent_ = false;
+ d->segmentation_header_.first_segment = 1;
+ d->segmentation_header_.last_segment = 0;
+ d->segmentation_header_.rolling_segment_counter = 0;
+ d->ranging_header_.ranging_counter = procedure_counter;
+ d->ranging_header_.configuration_id = configuration_id;
+ d->ranging_header_.selected_tx_power = selected_tx_power;
+ d->ranging_header_.antenna_paths_mask = 0;
+
+ for (i = 0; i < num_antenna_paths; i++)
+ d->ranging_header_.antenna_paths_mask |= (1u << i);
+
+ d->ranging_header_.pct_format = IQ;
+ memset(&d->ras_raw_data_, 0, sizeof(d->ras_raw_data_));
+ d->ras_raw_data_index_ = 0;
+ memset(&d->ras_subevent_data_, 0, sizeof(d->ras_subevent_data_));
+ d->ras_subevent_counter_ = 0;
+ d->initiator_reference_power_level = 0;
+ d->reflector_reference_power_level = 0;
+ d->ranging_header_prepended_ = false;
+ d->ras_subevent_header_emitted = false;
+
+ return d;
+}
+
+static void cs_procedure_data_destroy(struct cs_procedure_data *d)
+{
+ if (!d)
+ return;
+
+ free(d->ras_raw_data_.iov_base);
+ free(d->ras_subevent_data_.iov_base);
+ free(d);
+}
+
+static void cs_pd_set_local_status(struct cs_procedure_data *d,
+ enum cs_procedure_done_status s)
+{
+ if (d)
+ d->local_status = s;
+}
+
+static void cs_pd_set_remote_status(struct cs_procedure_data *d,
+ enum cs_procedure_done_status s)
+{
+ if (d)
+ d->remote_status = s;
+}
+
+static void cs_pd_set_reference_power_levels(struct cs_procedure_data *d,
+ int8_t init_lvl, int8_t ref_lvl)
+{
+ if (!d)
+ return;
+
+ d->initiator_reference_power_level = init_lvl;
+ d->reflector_reference_power_level = ref_lvl;
+}
+
+static void cs_pd_ras_begin_subevent(struct cs_procedure_data *d,
+ uint16_t start_acl_conn_event,
+ uint16_t frequency_compensation,
+ int8_t reference_power_level)
+{
+ if (!d)
+ return;
+
+ d->ras_subevent_counter_++;
+ d->ras_subevent_header_.start_acl_conn_event = start_acl_conn_event;
+ d->ras_subevent_header_.frequency_compensation =
+ frequency_compensation;
+ d->ras_subevent_header_.reference_power_level = reference_power_level;
+ d->ras_subevent_header_.num_steps_reported = 0;
+ d->ras_subevent_header_emitted = false;
+ d->ras_subevent_data_.iov_len = 0;
+}
+
+static bool cs_pd_ras_append_subevent_bytes(struct cs_procedure_data *d,
+ const uint8_t *bytes, size_t len)
+{
+ if (!d || !bytes || len == 0)
+ return false;
+
+ return util_iov_append(&d->ras_subevent_data_, bytes, len) != NULL;
+}
+
+static inline size_t serialize_ras_subevent_header(
+ const struct ras_subevent_header *h,
+ uint8_t *out, size_t out_len)
+{
+ uint16_t start_le;
+ uint16_t freq_le;
+
+ if (!h || !out || out_len < RAS_SUBEVENT_HEADER_SIZE)
+ return 0;
+
+ start_le = (uint16_t)h->start_acl_conn_event;
+ out[0] = (uint8_t)(start_le & 0xFF);
+ out[1] = (uint8_t)((start_le >> 8) & 0xFF);
+
+ freq_le = (uint16_t)h->frequency_compensation;
+ out[2] = (uint8_t)(freq_le & 0xFF);
+ out[3] = (uint8_t)((freq_le >> 8) & 0xFF);
+
+ out[4] = (uint8_t)((h->ranging_done_status & 0x0F) |
+ ((h->subevent_done_status & 0x0F) << 4));
+
+ out[5] = (uint8_t)((h->ranging_abort_reason & 0x0F) |
+ ((h->subevent_abort_reason & 0x0F) << 4));
+
+ out[6] = (uint8_t)h->reference_power_level;
+ out[7] = (uint8_t)h->num_steps_reported;
+
+ return RAS_SUBEVENT_HEADER_SIZE;
+}
+
+static bool cs_pd_ras_commit_subevent(struct cs_procedure_data *d,
+ uint8_t num_steps_reported,
+ uint8_t ranging_done_status,
+ uint8_t subevent_done_status,
+ uint8_t ranging_abort_reason,
+ uint8_t subevent_abort_reason)
+{
+ size_t hdr_sz;
+ size_t payload_sz;
+ size_t total;
+ uint8_t *buf;
+ size_t w;
+ bool ok;
+
+ if (!d)
+ return false;
+
+ d->ras_subevent_header_.num_steps_reported =
+ (uint8_t)(d->ras_subevent_header_.num_steps_reported +
+ num_steps_reported);
+ d->ras_subevent_header_.ranging_done_status = ranging_done_status;
+ d->ras_subevent_header_.subevent_done_status = subevent_done_status;
+ d->ras_subevent_header_.ranging_abort_reason = ranging_abort_reason;
+ d->ras_subevent_header_.subevent_abort_reason = subevent_abort_reason;
+
+ if (subevent_done_status == SUBEVENT_DONE_ALL_RESULTS_COMPLETE)
+ d->contains_complete_subevent_ = true;
+
+ if (subevent_done_status == SUBEVENT_DONE_PARTIAL_RESULTS)
+ return true;
+
+ if (!d->ras_subevent_header_emitted) {
+ hdr_sz = RAS_SUBEVENT_HEADER_SIZE;
+ payload_sz = d->ras_subevent_data_.iov_len;
+ total = hdr_sz + payload_sz;
+ buf = (uint8_t *)malloc(total);
+
+ if (!buf)
+ return false;
+
+ w = serialize_ras_subevent_header(&d->ras_subevent_header_,
+ buf, total);
+
+ if (w != hdr_sz) {
+ free(buf);
+ return false;
+ }
+
+ if (payload_sz > 0)
+ memcpy(buf + hdr_sz,
+ (const uint8_t *)d->ras_subevent_data_.iov_base,
+ payload_sz);
+
+ ok = util_iov_append(&d->ras_raw_data_, buf, total) != NULL;
+ free(buf);
+
+ if (!ok)
+ return false;
+
+ d->ras_subevent_data_.iov_len = 0;
+ d->ras_subevent_header_emitted = true;
+ }
+
+ return true;
+}
+
static struct ras *rap_get_ras(struct bt_rap *rap)
{
if (!rap)
@@ -155,6 +531,11 @@ static void rap_free(void *data)

rap_db_free(rap->rrapdb);

+ if (rap->resptracker) {
+ free(rap->resptracker);
+ rap->resptracker = NULL;
+ }
+
queue_destroy(rap->notify, free);
queue_destroy(rap->pending, NULL);
queue_destroy(rap->ready_cbs, rap_ready_free);
@@ -240,6 +621,22 @@ bool bt_rap_set_debug(struct bt_rap *rap, bt_rap_debug_func_t func,
return true;
}

+static void cs_tracker_init(struct cstracker *t)
+{
+ if (!t)
+ return;
+
+ memset(t, 0, sizeof(*t));
+ t->role = CS_ROLE_REFLECTOR;
+ t->config_id = CS_INVALID_CONFIG_ID;
+ t->rtt_type = 0;
+ t->selected_tx_power = 0;
+ t->last_proc_counter = 0;
+ t->last_start_acl_conn_evt_counter = 0;
+ t->last_freq_comp = 0;
+ t->last_ref_pwr_lvl = 0;
+}
+
static void ras_features_read_cb(struct gatt_db_attribute *attrib,
unsigned int id, uint16_t offset,
uint8_t opcode, struct bt_att *att,
@@ -304,6 +701,225 @@ static void ras_data_overwritten_read_cb(struct gatt_db_attribute *attrib,
gatt_db_attribute_read_result(attrib, id, 0, value, sizeof(value));
}

+/* CCC write callbacks for custom handling */
+static void ras_realtime_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, struct bt_att *att,
+ void *user_data)
+{
+ struct ras *ras = user_data;
+ uint16_t ccc_value;
+
+ if (!ras) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_UNLIKELY);
+ return;
+ }
+
+ if (offset) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_OFFSET);
+ return;
+ }
+
+ if (len != 2) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN);
+ return;
+ }
+
+ ccc_value = get_le16(value);
+
+ if (ccc_value != 0x0000 && ccc_value != 0x0001 &&
+ ccc_value != 0x0002 && ccc_value != 0x0003) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_WRITE_REQUEST_REJECTED);
+ return;
+ }
+
+ /* Check mutual exclusivity: reject if trying to enable realtime
+ * while ondemand is already enabled
+ */
+ if (ccc_value != 0x0000 && ras->ondemand_ccc_value != 0x0000) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_CCC_IMPROPERLY_CONFIGURED);
+ return;
+ }
+
+ /* Update state */
+ ras->realtime_ccc_value = ccc_value;
+
+ gatt_db_attribute_write_result(attrib, id, 0);
+}
+
+static void ras_ondemand_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, struct bt_att *att,
+ void *user_data)
+{
+ struct ras *ras = user_data;
+ uint16_t ccc_value;
+
+ if (!ras) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_UNLIKELY);
+ return;
+ }
+
+ if (offset) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_OFFSET);
+ return;
+ }
+
+ if (len != 2) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN);
+ return;
+ }
+
+ ccc_value = get_le16(value);
+
+ if (ccc_value != 0x0000 && ccc_value != 0x0001 && ccc_value != 0x0002 &&
+ ccc_value != 0x0003) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_WRITE_REQUEST_REJECTED);
+ return;
+ }
+
+ /* Check mutual exclusivity: reject if trying to enable ondemand
+ * while realtime is already enabled
+ */
+ if (ccc_value != 0x0000 && ras->realtime_ccc_value != 0x0000) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_CCC_IMPROPERLY_CONFIGURED);
+ return;
+ }
+
+ /* Update state */
+ ras->ondemand_ccc_value = ccc_value;
+
+ gatt_db_attribute_write_result(attrib, id, 0);
+}
+
+static void ras_cp_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, struct bt_att *att,
+ void *user_data)
+{
+ struct ras *ras = user_data;
+ uint16_t ccc_value;
+
+ if (!ras) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_UNLIKELY);
+ return;
+ }
+
+ if (offset) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_OFFSET);
+ return;
+ }
+
+ if (len != 2) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN);
+ return;
+ }
+
+ ccc_value = get_le16(value);
+
+ if (ccc_value != 0x0000 && ccc_value != 0x0001 &&
+ ccc_value != 0x0002) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_WRITE_REQUEST_REJECTED);
+ return;
+ }
+
+ gatt_db_attribute_write_result(attrib, id, 0);
+}
+
+static void ras_ready_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, struct bt_att *att,
+ void *user_data)
+{
+ struct ras *ras = user_data;
+ uint16_t ccc_value;
+
+ if (!ras) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_UNLIKELY);
+ return;
+ }
+
+ if (offset) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_OFFSET);
+ return;
+ }
+
+ if (len != 2) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN);
+ return;
+ }
+
+ ccc_value = get_le16(value);
+
+ if (ccc_value != 0x0000 && ccc_value != 0x0001 &&
+ ccc_value != 0x0002 && ccc_value != 0x0003) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_WRITE_REQUEST_REJECTED);
+ return;
+ }
+
+ gatt_db_attribute_write_result(attrib, id, 0);
+}
+
+static void ras_overwritten_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, struct bt_att *att,
+ void *user_data)
+{
+ struct ras *ras = user_data;
+ uint16_t ccc_value;
+
+ if (!ras) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_UNLIKELY);
+ return;
+ }
+
+ if (offset) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_OFFSET);
+ return;
+ }
+
+ if (len != 2) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN);
+ return;
+ }
+
+ ccc_value = get_le16(value);
+
+ if (ccc_value != 0x0000 && ccc_value != 0x0001 &&
+ ccc_value != 0x0002 && ccc_value != 0x0003) {
+ gatt_db_attribute_write_result(attrib, id,
+ BT_ERROR_WRITE_REQUEST_REJECTED);
+ return;
+ }
+
+ gatt_db_attribute_write_result(attrib, id, 0);
+}
/* Service registration – store attribute pointers */
static struct ras *register_ras_service(struct gatt_db *db)
{
@@ -349,9 +965,9 @@ static struct ras *register_ras_service(struct gatt_db *db)
NULL, NULL, ras);

ras->realtime_chrc_ccc =
- gatt_db_service_add_ccc(ras->svc,
- BT_ATT_PERM_READ |
- BT_ATT_PERM_WRITE);
+ gatt_db_service_add_ccc_custom(ras->svc,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ ras_realtime_ccc_write_cb, ras);

/* On-demand Ranging Data */
bt_uuid16_create(&uuid, RAS_ONDEMAND_DATA_UUID);
@@ -364,8 +980,9 @@ static struct ras *register_ras_service(struct gatt_db *db)
ras_ondemand_read_cb, NULL,
ras);

- gatt_db_service_add_ccc(ras->svc,
- BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
+ ras->ondemand_ccc = gatt_db_service_add_ccc_custom(ras->svc,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ ras_ondemand_ccc_write_cb, ras);

/* RAS Control Point */
bt_uuid16_create(&uuid, RAS_CONTROL_POINT_UUID);
@@ -379,8 +996,9 @@ static struct ras *register_ras_service(struct gatt_db *db)
ras_control_point_write_cb,
ras);

- gatt_db_service_add_ccc(ras->svc,
- BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
+ ras->cp_ccc = gatt_db_service_add_ccc_custom(ras->svc,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ ras_cp_ccc_write_cb, ras);

/* RAS Data Ready */
bt_uuid16_create(&uuid, RAS_DATA_READY_UUID);
@@ -394,8 +1012,9 @@ static struct ras *register_ras_service(struct gatt_db *db)
ras_data_ready_read_cb, NULL,
ras);

- gatt_db_service_add_ccc(ras->svc,
- BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
+ ras->ready_ccc = gatt_db_service_add_ccc_custom(ras->svc,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ ras_ready_ccc_write_cb, ras);

/* RAS Data Overwritten */
bt_uuid16_create(&uuid, RAS_DATA_OVERWRITTEN_UUID);
@@ -409,8 +1028,9 @@ static struct ras *register_ras_service(struct gatt_db *db)
ras_data_overwritten_read_cb,
NULL, ras);

- gatt_db_service_add_ccc(ras->svc,
- BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
+ ras->overwritten_ccc = gatt_db_service_add_ccc_custom(ras->svc,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ ras_overwritten_ccc_write_cb, ras);

/* Activate the service */
gatt_db_service_set_active(ras->svc, true);
@@ -503,32 +1123,746 @@ bool bt_rap_unregister(unsigned int id)
return true;
}

+static inline size_t serialize_segmentation_header(
+ const struct segmentation_header *s,
+ uint8_t *out, size_t out_len)
+{
+ if (!s || !out || out_len < 1)
+ return 0;
+
+ /* [0] bit0: first, bit1: last, bits2..7: rolling counter */
+ out[0] = (s->first_segment ? 0x01 : 0x00) |
+ (s->last_segment ? 0x02 : 0x00) |
+ ((s->rolling_segment_counter & 0x3F) << 2);
+
+ return 1;
+}
+
+static inline size_t serialize_ranging_header(const struct ranging_header *r,
+ uint8_t *out, size_t out_len)
+{
+ uint16_t rcid;
+
+ if (!r || !out || out_len < RAS_RANGING_HEADER_SIZE)
+ return 0;
+
+ /* Little-endian pack: [rc (12 bits) | cfg_id (4 bits)] */
+ rcid = RANGING_HDR_PACK_BYTE0_1(r->ranging_counter,
+ r->configuration_id);
+
+ put_le16(rcid, out + 0);
+ out[2] = (uint8_t)r->selected_tx_power;
+ /* Byte 3: antenna_paths_mask | reserved | pct_format */
+ out[3] = RANGING_HDR_PACK_BYTE3(r->antenna_paths_mask,
+ r->pct_format);
+
+ return RAS_RANGING_HEADER_SIZE;
+}
+
+static inline uint16_t ras_att_value_payload_max(struct bt_rap *rap)
+{
+ struct bt_att *att = bt_rap_get_att(rap);
+ uint16_t mtu = att ? bt_att_get_mtu(att) : default_ras_mtu;
+
+ return (uint16_t)(mtu > ATT_OVERHEAD ?
+ (mtu - ATT_OVERHEAD - TOTAL_RAS_RANGING_HEADER_SIZE -
+ ras_segment_header_size) : 0);
+}
+
+/* Prepend data to an iovec */
+static bool iov_prepend_bytes(struct iovec *iov, const uint8_t *bytes,
+ size_t len)
+{
+ size_t new_len;
+ void *new_base;
+
+ if (!iov || !bytes || len == 0)
+ return false;
+
+ new_len = iov->iov_len + len;
+ new_base = malloc(new_len);
+
+ if (!new_base)
+ return false;
+
+ memcpy(new_base, bytes, len);
+
+ if (iov->iov_len > 0)
+ memcpy((uint8_t *)new_base + len, iov->iov_base,
+ iov->iov_len);
+
+ free(iov->iov_base);
+ iov->iov_base = new_base;
+ iov->iov_len = new_len;
+
+ return true;
+}
+
+/* Append the 4-byte RangingHeader to ras_raw_data_ on first segment */
+static bool ras_maybe_prepend_ranging_header(struct cs_procedure_data *d)
+{
+ uint8_t hdr[RAS_RANGING_HEADER_SIZE];
+ size_t w;
+ bool ok;
+
+ if (!d)
+ return false;
+
+ if (d->ranging_header_prepended_)
+ return false;
+
+ if (!d->segmentation_header_.first_segment)
+ return false;
+
+ if (d->ras_raw_data_index_ != 0)
+ return false;
+
+ w = serialize_ranging_header(&d->ranging_header_, hdr, sizeof(hdr));
+
+ if (w != RAS_RANGING_HEADER_SIZE)
+ return false;
+
+ ok = iov_prepend_bytes(&d->ras_raw_data_, hdr, w);
+
+ if (ok)
+ d->ranging_header_prepended_ = true;
+
+ return ok;
+}
+
+static void send_ras_segment_data(struct bt_rap *rap,
+ struct cs_procedure_data *proc)
+{
+ struct ras *ras;
+ uint16_t value_max;
+ const uint16_t header_len = ras_segment_header_size;
+ uint16_t raw_payload_size;
+ bool ok;
+
+ if (!rap || !proc)
+ return;
+
+ if (!rap->lrapdb || !rap->lrapdb->ras)
+ return;
+ ras = rap->lrapdb->ras;
+ value_max = ras_att_value_payload_max(rap);
+
+ if (value_max == 0) {
+ DBG(rap, "value_max=0 (MTU not available?)");
+ return;
+ }
+
+ if (value_max <= header_len) {
+ DBG(rap, "value_max(%u) too small for header", value_max);
+ return;
+ }
+
+ raw_payload_size = (uint16_t)(value_max - header_len);
+
+ /* Convert tail recursion to loop */
+ while (true) {
+ size_t total_len = proc->ras_raw_data_.iov_len;
+ size_t index = proc->ras_raw_data_index_;
+ size_t unsent_data_size;
+ uint16_t copy_size;
+ uint16_t seg_len;
+ uint8_t *seg;
+ uint16_t wr = 0;
+
+ if (index > total_len)
+ index = total_len;
+
+ unsent_data_size = total_len - index;
+
+ if (unsent_data_size == 0)
+ return;
+
+ /* Set last_segment if procedure complete or fits in segment */
+ if ((proc->local_status != CS_PROC_PARTIAL_RESULTS &&
+ unsent_data_size <= raw_payload_size) ||
+ (proc->contains_complete_subevent_ &&
+ unsent_data_size <= raw_payload_size)) {
+ proc->segmentation_header_.last_segment = 1;
+ } else {
+ proc->segmentation_header_.last_segment = 0;
+ }
+
+ /* Wait for more data if needed and not last segment */
+ if (unsent_data_size < raw_payload_size &&
+ proc->segmentation_header_.last_segment == 0) {
+ DBG(rap, "waiting for more data (unsent=%zu < "
+ "payload=%u)", unsent_data_size,
+ raw_payload_size);
+ return;
+ }
+
+ copy_size = (uint16_t)((unsent_data_size < raw_payload_size) ?
+ unsent_data_size : raw_payload_size);
+ seg_len = (uint16_t)(header_len + copy_size);
+ seg = (uint8_t *)malloc(seg_len);
+
+ if (!seg) {
+ DBG(rap, "OOM (%u)", seg_len);
+ return;
+ }
+
+ wr += (uint16_t)serialize_segmentation_header(
+ &proc->segmentation_header_, seg + wr,
+ seg_len - wr);
+ memcpy(seg + wr,
+ (const uint8_t *)proc->ras_raw_data_.iov_base + index,
+ copy_size);
+ wr += copy_size;
+
+ /* Try sending to real-time characteristic */
+ if (ras->realtime_chrc)
+ ok = gatt_db_attribute_notify(ras->realtime_chrc, seg,
+ wr, bt_rap_get_att(rap));
+
+ /* Try sending to on-demand characteristic */
+ if (ras->ondemand_chrc)
+ ok = gatt_db_attribute_notify(ras->ondemand_chrc, seg,
+ wr, bt_rap_get_att(rap));
+
+ free(seg);
+
+ if (!ok) {
+ DBG(rap, "Failed to send RAS notification");
+ return;
+ }
+
+ /* Advance read cursor and update segmentation state */
+ proc->ras_raw_data_index_ += copy_size;
+ proc->segmentation_header_.first_segment = 0;
+ proc->segmentation_header_.rolling_segment_counter =
+ (uint8_t)((proc->segmentation_header_
+ .rolling_segment_counter + 1) & 0x3F);
+
+ if (proc->segmentation_header_.last_segment ||
+ proc->ras_raw_data_index_ >=
+ proc->ras_raw_data_.iov_len) {
+ DBG(rap, "RAS clear ras buffers");
+ proc->ras_raw_data_.iov_len = 0;
+ proc->ras_raw_data_index_ = 0;
+ proc->ranging_header_prepended_ = false;
+ return;
+ }
+ }
+}
+
+static inline void resptracker_reset_current_proc(struct cstracker *t)
+{
+ if (!t)
+ return;
+
+ if (t->current_proc) {
+ cs_procedure_data_destroy(t->current_proc);
+ t->current_proc = NULL;
+ }
+}
+/* Unified local subevent handler */
+static void handle_local_subevent_result(struct bt_rap *rap,
+ bool has_header_fields,
+ uint8_t config_id,
+ uint8_t num_ant_paths,
+ uint16_t proc_counter,
+ uint16_t start_acl_conn_evt_counter,
+ uint16_t freq_comp,
+ int8_t ref_pwr_lvl,
+ uint8_t proc_done_status,
+ uint8_t subevt_done_status,
+ uint8_t abort_reason,
+ uint8_t num_steps_reported,
+ const void *step_bytes)
+{
+ struct cstracker *resptracker;
+ struct cs_procedure_data *proc;
+ const struct cs_step_data *steps;
+ uint8_t idx;
+
+ if (!rap || !rap->resptracker || !step_bytes)
+ return;
+
+ resptracker = rap->resptracker;
+
+ if (resptracker->current_proc) {
+ struct cs_procedure_data *cur = resptracker->current_proc;
+
+ if (has_header_fields && cur->counter != proc_counter) {
+ /* Safety: a new procedure; destroy the previous one */
+ resptracker_reset_current_proc(resptracker);
+ }
+ }
+
+ proc = resptracker->current_proc;
+ /* Cache header info from a RESULT event for later CONT usage */
+ if (has_header_fields) {
+ resptracker->last_proc_counter = proc_counter;
+ resptracker->last_start_acl_conn_evt_counter =
+ start_acl_conn_evt_counter;
+ resptracker->last_freq_comp = freq_comp;
+ resptracker->last_ref_pwr_lvl = ref_pwr_lvl;
+ }
+
+ /* Create the procedure on first use */
+ if (!proc) {
+ uint16_t create_counter = has_header_fields ? proc_counter :
+ resptracker->last_proc_counter;
+
+ proc = cs_procedure_data_create(create_counter,
+ num_ant_paths,
+ config_id,
+ resptracker->selected_tx_power);
+ if (!proc)
+ return;
+
+ resptracker->current_proc = proc;
+
+ /* Reference power levels and status defaults */
+ cs_pd_set_reference_power_levels(proc,
+ has_header_fields ? ref_pwr_lvl :
+ resptracker->last_ref_pwr_lvl,
+ has_header_fields ? ref_pwr_lvl :
+ resptracker->last_ref_pwr_lvl);
+ cs_pd_set_local_status(proc,
+ (enum cs_procedure_done_status)proc_done_status);
+ cs_pd_set_remote_status(proc,
+ (enum cs_procedure_done_status)subevt_done_status);
+ }
+
+ /* Begin a new RAS subevent only when we have header fields */
+ if (has_header_fields) {
+ cs_pd_ras_begin_subevent(proc,
+ start_acl_conn_evt_counter,
+ freq_comp,
+ ref_pwr_lvl);
+ }
+
+ /* step_bytes points to an array of struct cs_step_data */
+ steps = (const struct cs_step_data *)step_bytes;
+
+ /* Process each step */
+ for (idx = 0; idx < num_steps_reported; idx++) {
+ const struct cs_step_data *step = &steps[idx];
+ const uint8_t mode = step->step_mode;
+ const uint8_t channel = step->step_chnl;
+ const uint8_t payload_len = step->step_data_length;
+ uint8_t mode_byte;
+ uint8_t mode_type;
+ const uint8_t *payload;
+ uint8_t plen;
+ bool step_aborted;
+
+ /* Check if step is aborted: bit 7 of step_mode or
+ * 0 payload len
+ */
+ step_aborted = (mode & RAS_STEP_ABORTED_BIT) ||
+ (payload_len == 0);
+
+ DBG(rap, "step[%u]: mode=0x%02x channel=%u payload_len=%u "
+ "aborted=%s", idx, mode, channel, payload_len,
+ step_aborted ? "YES" : "NO");
+
+ /* Slim-step serialization for RAS:
+ * - 1 byte: mode (bit7 set if aborted)
+ * - payload: exactly step_data_length bytes (raw mode data)
+ */
+ mode_byte = step->step_mode;
+ payload = (const uint8_t *)&step->step_mode_data;
+ plen = step->step_data_length;
+
+ if (step_aborted) {
+ /* Ensure abort bit is set */
+ mode_byte |= RAS_STEP_ABORTED_BIT;
+ cs_pd_ras_append_subevent_bytes(proc, &mode_byte, 1);
+ /* No payload when aborted - per RAS spec Table 3.8 */
+ DBG(rap, "step[%u]: mode=0x%02x aborted, "
+ "no payload sent", idx, mode_byte);
+ } else {
+ mode_type = mode & 0x03;
+
+ /* Mode byte first (without abort bit) */
+ cs_pd_ras_append_subevent_bytes(proc, &mode_byte, 1);
+
+ switch (mode_type) {
+ case CS_MODE_ZERO: {
+ /* Mode 0: use raw structure bytes */
+ payload =
+ (const uint8_t *)&step->step_mode_data;
+ plen = step->step_data_length;
+ cs_pd_ras_append_subevent_bytes(proc,
+ payload, plen);
+ DBG(rap, "step[%u]: mode=0x%02x Mode0 "
+ "payload_len=%u sent",
+ idx, mode_byte, (unsigned int)plen);
+ break;
+ }
+ case CS_MODE_ONE: {
+ const struct cs_mode_one_data *m1 =
+ &step->step_mode_data.mode_one_data;
+ uint8_t buf[16]; /* Max Mode 1 size */
+ uint8_t *p = buf;
+ uint16_t time_val;
+ uint32_t pct1;
+ uint32_t pct2;
+ enum cs_role cs_role = resptracker->role;
+ uint8_t cs_rtt_type = resptracker->rtt_type;
+
+ *p++ = m1->packet_quality;
+ *p++ = m1->packet_nadm;
+ *p++ = m1->packet_rssi_dbm;
+
+ /* Time value (2 bytes LE) - use the
+ * appropriate field based on role
+ */
+ if (cs_role == CS_ROLE_REFLECTOR)
+ time_val = m1->tod_toa_refl;
+ else
+ time_val = m1->toa_tod_init;
+
+ *p++ = (uint8_t)(time_val & 0xFF);
+ *p++ = (uint8_t)((time_val >> 8) & 0xFF);
+
+ *p++ = m1->packet_ant;
+
+ /* PCT samples if RTT type contains
+ * sounding sequence
+ */
+ if (cs_rtt_type == 0x01 ||
+ cs_rtt_type == 0x02) {
+ pct1 = ((uint32_t)(m1->packet_pct1
+ .i_sample &
+ 0x0FFF)) |
+ (((uint32_t)(m1->packet_pct1
+ .q_sample &
+ 0x0FFF)) << 12);
+ *p++ = (uint8_t)(pct1 & 0xFF);
+ *p++ = (uint8_t)((pct1 >> 8) & 0xFF);
+ *p++ = (uint8_t)((pct1 >> 16) & 0xFF);
+
+ /* PCT2 (3 bytes) */
+ pct2 = ((uint32_t)(m1->packet_pct2
+ .i_sample &
+ 0x0FFF)) |
+ (((uint32_t)(m1->packet_pct2
+ .q_sample &
+ 0x0FFF)) << 12);
+ *p++ = (uint8_t)(pct2 & 0xFF);
+ *p++ = (uint8_t)((pct2 >> 8) & 0xFF);
+ *p++ = (uint8_t)((pct2 >> 16) & 0xFF);
+ }
+
+ plen = (uint8_t)(p - buf);
+ cs_pd_ras_append_subevent_bytes(proc, buf,
+ plen);
+ DBG(rap, "step[%u]: mode=0x%02x Mode1 "
+ "serialized payload_len=%u role=%s "
+ "rtt_type=0x%02x",
+ idx, mode_byte, (unsigned int)plen,
+ cs_role == CS_ROLE_INITIATOR ?
+ "INIT" : "REFL", cs_rtt_type);
+ break;
+ }
+ case CS_MODE_TWO: {
+ const struct cs_mode_two_data *m2 =
+ &step->step_mode_data.mode_two_data;
+ /* Max Mode 2 size: 1 + 5*(3+1) = 21 bytes */
+ uint8_t buf[64];
+ uint8_t *p = buf;
+ uint8_t k;
+ uint8_t num_paths = (num_ant_paths + 1) < 5 ?
+ (num_ant_paths + 1) : 5;
+
+ *p++ = m2->ant_perm_index;
+
+ /* Serialize each path: PCT (3 bytes) +
+ * quality (1 byte)
+ */
+ for (k = 0; k < num_paths; k++) {
+ /* Convert 4-byte structure PCT to
+ * 3-byte wire format
+ */
+ uint32_t pct =
+ ((uint32_t)(m2->tone_pct[k]
+ .i_sample &
+ 0x0FFF)) |
+ (((uint32_t)(m2->tone_pct[k]
+ .q_sample &
+ 0x0FFF)) << 12);
+ *p++ = (uint8_t)(pct & 0xFF);
+ *p++ = (uint8_t)((pct >> 8) & 0xFF);
+ *p++ = (uint8_t)((pct >> 16) & 0xFF);
+
+ *p++ = m2->tone_quality_indicator[k];
+ }
+
+ plen = (uint8_t)(p - buf);
+ cs_pd_ras_append_subevent_bytes(proc, buf,
+ plen);
+ DBG(rap, "step[%u]: mode=0x%02x Mode2 "
+ "serialized payload_len=%u paths=%u",
+ idx, mode_byte, (unsigned int)plen,
+ num_paths);
+ break;
+ }
+ case CS_MODE_THREE: {
+ const struct cs_mode_three_data *m3 =
+ &step->step_mode_data.mode_three_data;
+ const struct cs_mode_one_data *m1 =
+ &m3->mode_one_data;
+ const struct cs_mode_two_data *m2 =
+ &m3->mode_two_data;
+ uint8_t buf[80]; /* Max Mode 3 size */
+ uint8_t *p = buf;
+ uint8_t k;
+ uint32_t pct1;
+ uint32_t pct2;
+ uint8_t num_paths = (num_ant_paths + 1) < 5 ?
+ (num_ant_paths + 1) : 5;
+ uint8_t cs_rtt_type = resptracker->rtt_type;
+ enum cs_role cs_role = resptracker->role;
+ uint16_t time_val;
+ bool include_pct;
+
+ /* Determine if PCT samples should be
+ * included
+ */
+ include_pct = (cs_rtt_type == 0x01 ||
+ cs_rtt_type == 0x02) &&
+ (cs_role == CS_ROLE_INITIATOR ||
+ cs_role == CS_ROLE_REFLECTOR);
+
+ /* Mode 1 portion - basic fields */
+ *p++ = m1->packet_quality;
+ *p++ = m1->packet_nadm;
+ *p++ = m1->packet_rssi_dbm;
+
+ /* Time value based on role */
+ if (cs_role == CS_ROLE_REFLECTOR)
+ time_val = m1->tod_toa_refl;
+ else
+ time_val = m1->toa_tod_init;
+
+ *p++ = (uint8_t)(time_val & 0xFF);
+ *p++ = (uint8_t)((time_val >> 8) & 0xFF);
+
+ *p++ = m1->packet_ant;
+
+ /* PCT samples if conditions met */
+ if (include_pct) {
+ pct1 = ((uint32_t)(m1->packet_pct1
+ .i_sample &
+ 0x0FFF)) |
+ (((uint32_t)(m1->packet_pct1
+ .q_sample &
+ 0x0FFF)) << 12);
+ *p++ = (uint8_t)(pct1 & 0xFF);
+ *p++ = (uint8_t)((pct1 >> 8) & 0xFF);
+ *p++ = (uint8_t)((pct1 >> 16) & 0xFF);
+
+ pct2 = ((uint32_t)(m1->packet_pct2
+ .i_sample &
+ 0x0FFF)) |
+ (((uint32_t)(m1->packet_pct2
+ .q_sample &
+ 0x0FFF)) << 12);
+ *p++ = (uint8_t)(pct2 & 0xFF);
+ *p++ = (uint8_t)((pct2 >> 8) & 0xFF);
+ *p++ = (uint8_t)((pct2 >> 16) & 0xFF);
+ }
+
+ /* Mode 2 portion */
+ *p++ = m2->ant_perm_index;
+
+ for (k = 0; k < num_paths; k++) {
+ uint32_t pct =
+ ((uint32_t)(m2->tone_pct[k]
+ .i_sample &
+ 0x0FFF)) |
+ (((uint32_t)(m2->tone_pct[k]
+ .q_sample &
+ 0x0FFF)) << 12);
+ *p++ = (uint8_t)(pct & 0xFF);
+ *p++ = (uint8_t)((pct >> 8) & 0xFF);
+ *p++ = (uint8_t)((pct >> 16) & 0xFF);
+
+ *p++ = m2->tone_quality_indicator[k];
+ }
+
+ plen = (uint8_t)(p - buf);
+ cs_pd_ras_append_subevent_bytes(proc, buf,
+ plen);
+ DBG(rap, "step[%u]: mode=0x%02x Mode3 "
+ "serialized payload_len=%u paths=%u "
+ "role=%s rtt_type=0x%02x pct=%s",
+ idx, mode_byte, (unsigned int)plen,
+ num_paths,
+ cs_role == CS_ROLE_INITIATOR ?
+ "INIT" : "REFL",
+ cs_rtt_type,
+ include_pct ? "YES" : "NO");
+ break;
+ }
+ default:
+ /* Unknown mode: use raw structure bytes */
+ payload =
+ (const uint8_t *)&step->step_mode_data;
+ plen = step->step_data_length;
+ cs_pd_ras_append_subevent_bytes(proc, payload,
+ plen);
+ DBG(rap, "step[%u]: mode=0x%02x unknown mode, "
+ "payload_len=%u sent",
+ idx, mode_byte, (unsigned int)plen);
+ break;
+ }
+ }
+ }
+
+ /* Update status for this chunk */
+ cs_pd_set_local_status(proc,
+ (enum cs_procedure_done_status)proc_done_status);
+ cs_pd_set_remote_status(proc,
+ (enum cs_procedure_done_status)subevt_done_status);
+
+ /* Commit subevent chunk (RESULT or CONT) */
+ cs_pd_ras_commit_subevent(proc,
+ num_steps_reported,
+ proc_done_status,
+ subevt_done_status,
+ abort_reason & 0x0F,
+ (abort_reason >> 4) & 0x0F);
+
+ /* Ensure first segment body starts with the 4-byte RangingHeader */
+ ras_maybe_prepend_ranging_header(proc);
+
+ if (subevt_done_status != SUBEVENT_DONE_PARTIAL_RESULTS)
+ /* Send RAS raw segment data */
+ send_ras_segment_data(rap, proc);
+
+ /* Procedure complete? Clean up */
+ if (proc_done_status == CS_PROC_ALL_RESULTS_COMPLETE) {
+ DBG(rap, "Destroying CsProcedureData counter=%u and "
+ "clearing current_proc", proc->counter);
+ resptracker_reset_current_proc(resptracker);
+ /* Reset cached header values for next procedure */
+ resptracker->last_proc_counter = 0;
+ resptracker->last_start_acl_conn_evt_counter = 0;
+ resptracker->last_freq_comp = 0;
+ resptracker->last_ref_pwr_lvl = 0;
+ }
+}
+
+static void form_ras_data_with_cs_subevent_result(struct bt_rap *rap,
+ const struct rap_ev_cs_subevent_result *data,
+ uint16_t length)
+{
+ size_t base_len = offsetof(struct rap_ev_cs_subevent_result,
+ step_data);
+
+ if (!rap || !rap->resptracker || !data)
+ return;
+
+ /* Defensive check: base header must be present */
+ if (length < base_len)
+ return;
+
+ DBG(rap, "Received CS subevent result subevent: len=%d", length);
+
+ handle_local_subevent_result(rap,
+ true, /* has header fields */
+ data->config_id,
+ data->num_ant_paths,
+ data->proc_counter,
+ data->start_acl_conn_evt_counter,
+ data->freq_comp,
+ data->ref_pwr_lvl,
+ data->proc_done_status,
+ data->subevt_done_status,
+ data->abort_reason,
+ data->num_steps_reported,
+ data->step_data); /* start of steps */
+}
+static void form_ras_data_with_cs_subevent_result_cont(struct bt_rap *rap,
+ const struct rap_ev_cs_subevent_result_cont *cont,
+ uint16_t length)
+{
+ size_t base_len = offsetof(struct rap_ev_cs_subevent_result_cont,
+ step_data);
+ struct cstracker *resptracker;
+
+ if (!rap || !rap->resptracker || !cont)
+ return;
+
+ if (length < base_len)
+ return;
+
+ DBG(rap, "Received CS subevent result continue subevent: len=%d",
+ length);
+
+ resptracker = rap->resptracker;
+
+ /* Use cached header values captured from the last RESULT event */
+ handle_local_subevent_result(rap,
+ false, /* CONT has no header fields */
+ cont->config_id,
+ cont->num_ant_paths,
+ resptracker->last_proc_counter,
+ resptracker->last_start_acl_conn_evt_counter,
+ resptracker->last_freq_comp,
+ resptracker->last_ref_pwr_lvl,
+ cont->proc_done_status,
+ cont->subevt_done_status,
+ cont->abort_reason,
+ cont->num_steps_reported,
+ cont->step_data);
+}
void bt_rap_hci_cs_subevent_result_cont_callback(uint16_t length,
const void *param,
void *user_data)
{
+ const struct rap_ev_cs_subevent_result_cont *cont = param;
struct bt_rap *rap = user_data;

DBG(rap, "Received CS subevent CONT: len=%d", length);
+
+ form_ras_data_with_cs_subevent_result_cont(rap, cont, length);
}

void bt_rap_hci_cs_subevent_result_callback(uint16_t length,
const void *param,
void *user_data)
{
+ const struct rap_ev_cs_subevent_result *data = param;
struct bt_rap *rap = user_data;

DBG(rap, "Received CS subevent: len=%d", length);
+
+ /* Populate CsProcedureData and send RAS payload */
+ form_ras_data_with_cs_subevent_result(rap, data, length);
}

void bt_rap_hci_cs_procedure_enable_complete_callback(uint16_t length,
const void *param,
void *user_data)
{
+ const struct rap_ev_cs_proc_enable_cmplt *data = param;
struct bt_rap *rap = user_data;
+ struct cstracker *resptracker;

DBG(rap, "Received CS procedure enable complete subevent: len=%d",
length);
+
+ if (!rap->resptracker) {
+ resptracker = new0(struct cstracker, 1);
+ cs_tracker_init(resptracker);
+ rap->resptracker = resptracker;
+ }
+
+ resptracker = rap->resptracker;
+
+ /* Populate responder tracker */
+ resptracker->config_id = data->config_id;
+ resptracker->selected_tx_power = data->sel_tx_pwr;
}

void bt_rap_hci_cs_sec_enable_complete_callback(uint16_t length,
@@ -544,9 +1878,27 @@ void bt_rap_hci_cs_config_complete_callback(uint16_t length,
const void *param,
void *user_data)
{
+ const struct rap_ev_cs_config_cmplt *data = param;
struct bt_rap *rap = user_data;
+ struct cstracker *resptracker;
+
+ if (!rap)
+ return;

DBG(rap, "Received CS config complete subevent: len=%d", length);
+
+ if (!rap->resptracker) {
+ resptracker = new0(struct cstracker, 1);
+ cs_tracker_init(resptracker);
+ rap->resptracker = resptracker;
+ }
+
+ resptracker = rap->resptracker;
+
+ /* Basic fields */
+ resptracker->config_id = data->config_id;
+ resptracker->role = data->role;
+ resptracker->rtt_type = data->rtt_type;
}

struct bt_rap *bt_rap_new(struct gatt_db *ldb, struct gatt_db *rdb)
@@ -786,7 +2138,7 @@ bool bt_rap_attach(struct bt_rap *rap, struct bt_gatt_client *client)

bt_uuid16_create(&uuid, RAS_UUID16);

- gatt_db_foreach_service(rap->lrapdb->db, &uuid,
+ gatt_db_foreach_service(rap->rrapdb->db, &uuid,
foreach_rap_service, rap);

return true;
diff --git a/src/shared/rap.h b/src/shared/rap.h
index 15ddea295..c9431aecd 100644
--- a/src/shared/rap.h
+++ b/src/shared/rap.h
@@ -97,11 +97,11 @@ struct cs_mode_zero_data {

struct cs_mode_one_data {
uint8_t packet_quality;
- uint8_t packet_rssi_dbm;
- uint8_t packet_ant;
uint8_t packet_nadm;
+ uint8_t packet_rssi_dbm;
int16_t toa_tod_init;
int16_t tod_toa_refl;
+ uint8_t packet_ant;
struct pct_iq_sample packet_pct1;
struct pct_iq_sample packet_pct2;
};
--
2.34.1