[PATCH BlueZ v7 1/3] src/shared: Add RAS packet format and notification support

[Prathibha Madugonde <prathibha.madugonde@oss.qualcomm.com>]

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 | 1258 +++++++++++++++++++++++++++++++++++++++++++++-
 src/shared/rap.h |    4 +-
 2 files changed, 1248 insertions(+), 14 deletions(-)

diff --git a/src/shared/rap.c b/src/shared/rap.c
index ac6de04e0..b554726b0 100644
--- a/src/shared/rap.c
+++ b/src/shared/rap.c
@@ -14,6 +14,7 @@
 #include <errno.h>
 
 #include "bluetooth/bluetooth.h"
+#include "bluetooth/hci.h"
 #include "bluetooth/uuid.h"
 
 #include "src/shared/queue.h"
@@ -33,6 +34,180 @@
 /* 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)))
+
+struct ranging_header {
+	/* Byte 0-1: 12-bit counter + 4-bit config_id */
+	uint8_t counter_config[2];
+	int8_t selected_tx_power;   /* Byte 2: selected TX power */
+	/* Byte 3: 4-bit antenna_mask + 2-bit reserved + 2-bit pct_format */
+	uint8_t antenna_pct;
+} __packed;
+
+static inline void ranging_header_set_counter(struct ranging_header *hdr,
+						uint16_t counter)
+{
+	/* Counter is 12 bits, stored in lower 12 bits of first 2 bytes */
+	hdr->counter_config[0] = counter & 0xFF;
+	hdr->counter_config[1] = (hdr->counter_config[1] & 0xF0) |
+				((counter >> 8) & 0x0F);
+}
+
+static inline void ranging_header_set_config_id(struct ranging_header *hdr,
+						uint8_t config_id)
+{
+	/* Config ID is 4 bits, stored in upper 4 bits of byte 1 */
+	hdr->counter_config[1] = (hdr->counter_config[1] & 0x0F) |
+				((config_id & 0x0F) << 4);
+}
+
+static inline void ranging_header_set_antenna_mask(
+					struct ranging_header *hdr,
+					uint8_t mask)
+{
+	/* Antenna mask is 4 bits, stored in lower 4 bits of byte 3 */
+	hdr->antenna_pct = (hdr->antenna_pct & 0xF0) | (mask & 0x0F);
+}
+
+static inline void ranging_header_set_pct_format(struct ranging_header *hdr,
+						uint8_t format)
+{
+	/* PCT format is 2 bits, stored in bits 6-7 of byte 3 */
+	hdr->antenna_pct = (hdr->antenna_pct & 0x3F) |
+				((format & 0x03) << 6);
+}
+
+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;
+};
+
+/* Macros to pack/unpack RAS subevent header status fields */
+#define RAS_DONE_STATUS_PACK(ranging, subevent) \
+	((uint8_t)(((ranging) & 0x0F) | (((subevent) & 0x0F) << 4)))
+
+#define RAS_ABORT_REASON_PACK(ranging, subevent) \
+	((uint8_t)(((ranging) & 0x0F) | (((subevent) & 0x0F) << 4)))
+
+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 */
+	int8_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 +218,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 +253,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 +274,204 @@ 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,
+					int8_t selected_tx_power)
+{
+	struct cs_procedure_data *d;
+	uint8_t i;
+	uint8_t antenna_mask = 0;
+
+	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;
+
+	/* Initialize ranging header using helper functions */
+	memset(&d->ranging_header_, 0, sizeof(d->ranging_header_));
+	ranging_header_set_counter(&d->ranging_header_, procedure_counter);
+	ranging_header_set_config_id(&d->ranging_header_, configuration_id);
+	d->ranging_header_.selected_tx_power = selected_tx_power;
+
+	/* Build antenna mask */
+	for (i = 0; i < num_antenna_paths; i++)
+		antenna_mask |= (1u << i);
+	ranging_header_set_antenna_mask(&d->ranging_header_, antenna_mask);
+
+	ranging_header_set_pct_format(&d->ranging_header_, 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)
+{
+
+	if (!h || !out || out_len < RAS_SUBEVENT_HEADER_SIZE)
+		return 0;
+
+	put_le16(h->start_acl_conn_event, out + 0);
+	put_le16(h->frequency_compensation, out + 2);
+	out[4] = RAS_DONE_STATUS_PACK(h->ranging_done_status,
+					h->subevent_done_status);
+	out[5] = RAS_ABORT_REASON_PACK(h->ranging_abort_reason,
+					h->subevent_abort_reason);
+	out[6] = h->reference_power_level;
+	out[7] = 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 +537,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 +627,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 +707,70 @@ static void ras_data_overwritten_read_cb(struct gatt_db_attribute *attrib,
 	gatt_db_attribute_read_result(attrib, id, 0, value, sizeof(value));
 }
 
+static void ras_ranging_data_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;
+	bool is_realtime;
+	uint16_t *this_ccc;
+	uint16_t *other_ccc;
+
+	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;
+	}
+
+	/* Determine which CCC this is */
+	is_realtime = (attrib == ras->realtime_chrc_ccc);
+	this_ccc = is_realtime ? &ras->realtime_ccc_value :
+				 &ras->ondemand_ccc_value;
+	other_ccc = is_realtime ? &ras->ondemand_ccc_value :
+				  &ras->realtime_ccc_value;
+
+	/* Check mutual exclusivity: reject if trying to enable realtime
+	 * while ondemand is already enabled.
+	 * Test case: RAS/SR/SPE/BI-11-C [Client enables both Real-time
+	 * Ranging Data and On-demand Ranging Data notifications or
+	 * indications]
+	 */
+	if (ccc_value != 0x0000 && *other_ccc != 0x0000) {
+		gatt_db_attribute_write_result(attrib, id,
+					BT_ERROR_CCC_IMPROPERLY_CONFIGURED);
+		return;
+	}
+
+	/* Update state */
+	*this_ccc = ccc_value;
+
+	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 +816,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_ranging_data_ccc_write_cb, ras);
 
 	/* On-demand Ranging Data */
 	bt_uuid16_create(&uuid, RAS_ONDEMAND_DATA_UUID);
@@ -364,8 +831,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_ranging_data_ccc_write_cb, ras);
 
 	/* RAS Control Point */
 	bt_uuid16_create(&uuid, RAS_CONTROL_POINT_UUID);
@@ -379,8 +847,8 @@ 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(ras->svc,
+					BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
 
 	/* RAS Data Ready */
 	bt_uuid16_create(&uuid, RAS_DATA_READY_UUID);
@@ -394,8 +862,8 @@ 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(ras->svc,
+					BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
 
 	/* RAS Data Overwritten */
 	bt_uuid16_create(&uuid, RAS_DATA_OVERWRITTEN_UUID);
@@ -409,8 +877,8 @@ 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(ras->svc,
+					BT_ATT_PERM_READ | BT_ATT_PERM_WRITE);
 
 	/* Activate the service */
 	gatt_db_service_set_active(ras->svc, true);
@@ -503,32 +971,780 @@ 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;
+
+	out[0] = SEG_HDR_PACK(s->first_segment, s->last_segment,
+				s->rolling_segment_counter);
+
+	return 1;
+}
+
+static inline bool serialize_ranging_header_iov(const struct ranging_header *r,
+						struct iovec *iov)
+{
+	if (!r || !iov)
+		return false;
+
+	/* Serialize the per-byte packed fields using util_iov_push functions */
+	if (!util_iov_push_le16(iov, get_le16(r->counter_config)))
+		return false;
+
+	if (!util_iov_push_u8(iov, r->selected_tx_power))
+		return false;
+
+	if (!util_iov_push_u8(iov, r->antenna_pct))
+		return false;
+
+	return true;
+}
+
+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 - optimized to avoid unnecessary malloc/copy
+ * by using realloc and memmove instead of malloc/memcpy/free pattern.
+ * This reduces memory allocations and is more cache-friendly.
+ */
+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;
+
+	/* Use realloc to potentially expand in-place */
+	new_base = realloc(iov->iov_base, new_len);
+
+	if (!new_base)
+		return false;
+
+	/* Move existing data forward to make room at the beginning */
+	if (iov->iov_len > 0)
+		memmove((uint8_t *)new_base + len, new_base, iov->iov_len);
+
+	/* Copy new data to the beginning */
+	memcpy(new_base, bytes, len);
+
+	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)
+{
+	struct iovec temp_iov = { 0 };
+	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;
+
+	temp_iov.iov_base = malloc(4);
+	if (!temp_iov.iov_base)
+		return false;
+	temp_iov.iov_len = 0;
+
+	/* Serialize ranging header into temporary iovec */
+	if (!serialize_ranging_header_iov(&d->ranging_header_, &temp_iov)) {
+		free(temp_iov.iov_base);
+		return false;
+	}
+
+	/* Prepend the serialized header to ras_raw_data_ */
+	ok = iov_prepend_bytes(&d->ras_raw_data_, temp_iov.iov_base,
+				temp_iov.iov_len);
+
+	/* Free temporary iovec buffer */
+	free(temp_iov.iov_base);
+
+	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;
+	}
+}
+
+static void process_cs_mode_zero(struct bt_rap *rap,
+				struct cs_procedure_data *proc,
+				const struct cs_step_data *step,
+				uint8_t idx, uint8_t mode_byte)
+{
+	const uint8_t *payload;
+	uint8_t plen;
+
+	/* 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);
+}
+
+static void process_cs_mode_one(struct bt_rap *rap,
+				struct cs_procedure_data *proc,
+				const struct cs_step_data *step,
+				uint8_t idx, uint8_t mode_byte)
+{
+	const struct cs_mode_one_data *m1 =
+		&step->step_mode_data.mode_one_data;
+	struct cstracker *resptracker = rap->resptracker;
+	struct iovec temp_iov = { 0 };
+	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;
+	bool include_pct;
+
+	temp_iov.iov_base = malloc(64);
+	if (!temp_iov.iov_base) {
+		DBG(rap, "Mode1 ERROR: malloc failed!");
+		return;
+	}
+	temp_iov.iov_len = 0;
+
+	include_pct = (cs_rtt_type == 0x01 || cs_rtt_type == 0x02);
+
+	if (!util_iov_push_u8(&temp_iov, m1->packet_quality) ||
+	    !util_iov_push_u8(&temp_iov, m1->packet_nadm) ||
+	    !util_iov_push_u8(&temp_iov, m1->packet_rssi_dbm))
+		goto done;
+
+	/* 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;
+
+	if (!util_iov_push_le16(&temp_iov, time_val) ||
+	    !util_iov_push_u8(&temp_iov, m1->packet_ant))
+		goto done;
+
+	if (include_pct) {
+		/* PCT1 (3 bytes LE) - 12-bit I + 12-bit Q */
+		pct1 = ((uint32_t)(m1->packet_pct1.i_sample & 0x0FFF)) |
+			(((uint32_t)(m1->packet_pct1.q_sample & 0x0FFF)) <<
+			12);
+		if (!util_iov_push_le24(&temp_iov, pct1))
+			goto done;
+
+		/* PCT2 (3 bytes LE) */
+		pct2 = ((uint32_t)(m1->packet_pct2.i_sample & 0x0FFF)) |
+			(((uint32_t)(m1->packet_pct2.q_sample & 0x0FFF)) <<
+			12);
+		if (!util_iov_push_le24(&temp_iov, pct2))
+			goto done;
+	}
+
+	cs_pd_ras_append_subevent_bytes(proc, temp_iov.iov_base,
+					temp_iov.iov_len);
+
+done:
+	free(temp_iov.iov_base);
+
+	DBG(rap, "step[%u]: mode=0x%02x Mode1 serialized payload_len=%zu "
+		"role=%s rtt_type=0x%02x pct=%s",
+		idx, mode_byte, temp_iov.iov_len,
+		cs_role == CS_ROLE_INITIATOR ? "INIT" : "REFL", cs_rtt_type,
+		include_pct ? "YES" : "NO");
+}
+
+static void process_cs_mode_two(struct bt_rap *rap,
+				struct cs_procedure_data *proc,
+				const struct cs_step_data *step,
+				uint8_t num_ant_paths,
+				uint8_t idx, uint8_t mode_byte)
+{
+	const struct cs_mode_two_data *m2 =
+		&step->step_mode_data.mode_two_data;
+	struct iovec temp_iov = { 0 };
+	uint8_t k;
+	uint8_t num_paths = (num_ant_paths + 1) < 5 ?
+		(num_ant_paths + 1) : 5;
+
+	temp_iov.iov_base = malloc(128);
+	if (!temp_iov.iov_base) {
+		DBG(rap, "Mode2 ERROR: malloc failed!");
+		return;
+	}
+	temp_iov.iov_len = 0;
+
+	if (!util_iov_push_u8(&temp_iov, m2->ant_perm_index))
+		goto done;
+
+	/* Serialize each path: PCT (3 bytes LE) + 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);
+		if (!util_iov_push_le24(&temp_iov, pct) ||
+		    !util_iov_push_u8(&temp_iov,
+				m2->tone_quality_indicator[k]))
+			goto done;
+	}
+
+	cs_pd_ras_append_subevent_bytes(proc, temp_iov.iov_base,
+					temp_iov.iov_len);
+
+done:
+	free(temp_iov.iov_base);
+
+	DBG(rap, "step[%u]: mode=0x%02x Mode2 serialized payload_len=%zu "
+		"paths=%u",
+		idx, mode_byte, temp_iov.iov_len, num_paths);
+}
+
+static void process_cs_mode_three(struct bt_rap *rap,
+				struct cs_procedure_data *proc,
+				const struct cs_step_data *step,
+				uint8_t num_ant_paths,
+				uint8_t idx, uint8_t mode_byte)
+{
+	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;
+	struct cstracker *resptracker = rap->resptracker;
+	struct iovec temp_iov = { 0 };
+	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;
+	uint8_t k;
+	uint8_t num_paths = (num_ant_paths + 1) < 5 ?
+		(num_ant_paths + 1) : 5;
+	bool include_pct;
+
+	temp_iov.iov_base = malloc(128);
+	if (!temp_iov.iov_base) {
+		DBG(rap, "Mode3 ERROR: malloc failed!");
+		return;
+	}
+	temp_iov.iov_len = 0;
+
+	/* Determine if PCT samples should be included */
+	include_pct = (cs_rtt_type == 0x01 || cs_rtt_type == 0x02);
+
+	if (!util_iov_push_u8(&temp_iov, m1->packet_quality) ||
+	    !util_iov_push_u8(&temp_iov, m1->packet_nadm) ||
+	    !util_iov_push_u8(&temp_iov, m1->packet_rssi_dbm))
+		goto done;
+
+	/* 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;
+
+	if (!util_iov_push_le16(&temp_iov, time_val) ||
+	    !util_iov_push_u8(&temp_iov, m1->packet_ant))
+		goto done;
+
+	/* PCT samples if RTT type contains sounding sequence */
+	if (include_pct) {
+		/* PCT1 (3 bytes LE) - 12-bit I + 12-bit Q */
+		pct1 = ((uint32_t)(m1->packet_pct1.i_sample & 0x0FFF)) |
+			(((uint32_t)(m1->packet_pct1.q_sample & 0x0FFF)) <<
+			12);
+
+		if (!util_iov_push_le24(&temp_iov, pct1))
+			goto done;
+
+		/* PCT2 (3 bytes LE) */
+		pct2 = ((uint32_t)(m1->packet_pct2.i_sample & 0x0FFF)) |
+			(((uint32_t)(m1->packet_pct2.q_sample & 0x0FFF)) <<
+			12);
+
+		if (!util_iov_push_le24(&temp_iov, pct2))
+			goto done;
+	}
+
+	if (!util_iov_push_u8(&temp_iov, m2->ant_perm_index))
+		goto done;
+
+	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);
+		if (!util_iov_push_le24(&temp_iov, pct) ||
+		    !util_iov_push_u8(&temp_iov,
+				m2->tone_quality_indicator[k]))
+			goto done;
+	}
+
+	cs_pd_ras_append_subevent_bytes(proc, temp_iov.iov_base,
+					temp_iov.iov_len);
+
+done:
+	free(temp_iov.iov_base);
+
+	DBG(rap, "=== Mode3 END: step[%u] payload_len=%zu paths=%u role=%s "
+		"rtt_type=0x%02x pct=%s ===",
+		idx, temp_iov.iov_len, num_paths,
+		cs_role == CS_ROLE_INITIATOR ? "INIT" : "REFL",
+		cs_rtt_type, include_pct ? "YES" : "NO");
+}
+
+static void process_cs_mode_step(struct bt_rap *rap,
+				struct cs_procedure_data *proc,
+				const struct cs_step_data *step,
+				uint8_t num_ant_paths,
+				uint8_t idx)
+{
+	const uint8_t mode = step->step_mode;
+	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, step->step_chnl, payload_len,
+		step_aborted ? "YES" : "NO");
+
+	mode_byte = step->step_mode;
+
+	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);
+		return;
+	}
+
+	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:
+		process_cs_mode_zero(rap, proc, step, idx, mode_byte);
+		break;
+	case CS_MODE_ONE:
+		process_cs_mode_one(rap, proc, step, idx, mode_byte);
+		break;
+	case CS_MODE_TWO:
+		process_cs_mode_two(rap, proc, step, num_ant_paths, idx,
+					mode_byte);
+		break;
+	case CS_MODE_THREE:
+		process_cs_mode_three(rap, proc, step, num_ant_paths, idx,
+					mode_byte);
+		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;
+	}
+}
+
+/* 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 using helper function */
+	for (idx = 0; idx < num_steps_reported; idx++)
+		process_cs_mode_step(rap, proc, &steps[idx], num_ant_paths,
+					idx);
+
+	/* 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 +1760,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 +2020,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