1 /* cs.c - Bluetooth Channel Sounding handling */
2
3 /*
4 * Copyright (c) 2024 Nordic Semiconductor ASA
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/sys/byteorder.h>
10 #include <zephyr/bluetooth/cs.h>
11 #include <zephyr/bluetooth/hci.h>
12 #include <zephyr/bluetooth/hci_types.h>
13
14 #include "conn_internal.h"
15
16 #define LOG_LEVEL CONFIG_BT_HCI_CORE_LOG_LEVEL
17 #include <zephyr/logging/log.h>
18 LOG_MODULE_REGISTER(bt_cs);
19
20 #if defined(CONFIG_BT_CHANNEL_SOUNDING)
21 #if defined(CONFIG_BT_CHANNEL_SOUNDING_TEST)
22 static struct bt_le_cs_test_cb cs_test_callbacks;
23 #endif
24
25 struct reassembly_buf_meta_data {
26 uint16_t conn_handle;
27 };
28
29 static void clear_on_disconnect(struct bt_conn *conn, uint8_t reason);
30
31 NET_BUF_POOL_FIXED_DEFINE(reassembly_buf_pool, CONFIG_BT_CHANNEL_SOUNDING_REASSEMBLY_BUFFER_CNT,
32 CONFIG_BT_CHANNEL_SOUNDING_REASSEMBLY_BUFFER_SIZE,
33 sizeof(struct reassembly_buf_meta_data), NULL);
34
35 static sys_slist_t reassembly_bufs = SYS_SLIST_STATIC_INIT(&reassembly_bufs);
36
37 struct bt_conn_le_cs_subevent_result reassembled_result;
38
39 BT_CONN_CB_DEFINE(cs_conn_callbacks) = {
40 .disconnected = clear_on_disconnect,
41 };
42
43 /** @brief Allocates new reassembly buffer identified by the connection handle
44 *
45 * @param conn_handle Connection handle
46 * @return struct net_buf* Reassembly buffer, NULL if allocation fails
47 */
alloc_reassembly_buf(uint16_t conn_handle)48 static struct net_buf *alloc_reassembly_buf(uint16_t conn_handle)
49 {
50 struct net_buf *buf = net_buf_alloc(&reassembly_buf_pool, K_NO_WAIT);
51
52 if (!buf) {
53 LOG_ERR("Failed to allocate new reassembly buffer");
54 return NULL;
55 }
56
57 struct reassembly_buf_meta_data *buf_meta_data =
58 (struct reassembly_buf_meta_data *)buf->user_data;
59
60 buf_meta_data->conn_handle = conn_handle;
61 net_buf_slist_put(&reassembly_bufs, buf);
62
63 LOG_DBG("Allocated new reassembly buffer for conn handle %d", conn_handle);
64 return buf;
65 }
66
67 /** @brief Frees a reassembly buffer
68 *
69 * @note Takes the ownership of the pointer and sets it to NULL
70 *
71 * @param buf Double pointer to reassembly buffer
72 */
free_reassembly_buf(struct net_buf ** buf)73 static void free_reassembly_buf(struct net_buf **buf)
74 {
75 if (!buf) {
76 LOG_ERR("NULL double pointer was passed when attempting to free reassembly buffer");
77 return;
78 }
79
80 if (!(*buf)) {
81 LOG_WRN("Attempted double free on reassembly buffer");
82 return;
83 }
84
85 struct reassembly_buf_meta_data *buf_meta_data =
86 (struct reassembly_buf_meta_data *)((*buf)->user_data);
87
88 LOG_DBG("De-allocating reassembly buffer for conn handle %d", buf_meta_data->conn_handle);
89 if (!sys_slist_find_and_remove(&reassembly_bufs, &(*buf)->node)) {
90 LOG_WRN("The buffer was not in the list");
91 }
92
93 net_buf_unref(*buf);
94 *buf = NULL;
95 }
96
97 /** @brief Gets the reassembly buffer identified by the connection handle
98 *
99 * @param conn_handle Connection handle
100 * @param allocate Allocates a new reassembly buffer if it's not allocated already
101 * @return struct net_buf* Reassembly buffer, NULL if it doesn't exist or failed when allocating new
102 */
get_reassembly_buf(uint16_t conn_handle,bool allocate)103 static struct net_buf *get_reassembly_buf(uint16_t conn_handle, bool allocate)
104 {
105 sys_snode_t *node;
106
107 SYS_SLIST_FOR_EACH_NODE(&reassembly_bufs, node) {
108 struct net_buf *buf = CONTAINER_OF(node, struct net_buf, node);
109 struct reassembly_buf_meta_data *buf_meta_data =
110 (struct reassembly_buf_meta_data *)(buf->user_data);
111
112 if (buf_meta_data->conn_handle == conn_handle) {
113 return buf;
114 }
115 }
116
117 return allocate ? alloc_reassembly_buf(conn_handle) : NULL;
118 }
119
120 /** @brief Adds step data to a reassembly buffer
121 *
122 * @param reassembly_buf Reassembly buffer
123 * @param data Step data
124 * @param data_len Step data length
125 * @return true if successful, false if there is insufficient space
126 */
add_reassembly_data(struct net_buf * reassembly_buf,const uint8_t * data,uint16_t data_len)127 static bool add_reassembly_data(struct net_buf *reassembly_buf, const uint8_t *data,
128 uint16_t data_len)
129 {
130 if (data_len > net_buf_tailroom(reassembly_buf)) {
131 LOG_ERR("Not enough reassembly buffer space for subevent result");
132 return false;
133 }
134
135 net_buf_add_mem(reassembly_buf, data, data_len);
136 return true;
137 }
138
139 /** @brief Initializes a reassembly buffer from partial step data
140 *
141 * @note Upon first call, this function also registers the disconnection callback
142 * to ensure any dangling reassembly buffer is freed
143 *
144 * @param conn_handle Connection handle
145 * @param steps Step data
146 * @param step_data_len Step data length
147 * @return struct net_buf* Pointer to reassembly buffer, NULL if fails to allocate or insert data
148 */
start_reassembly(uint16_t conn_handle,const uint8_t * steps,uint16_t step_data_len)149 static struct net_buf *start_reassembly(uint16_t conn_handle, const uint8_t *steps,
150 uint16_t step_data_len)
151 {
152 struct net_buf *reassembly_buf = get_reassembly_buf(conn_handle, true);
153
154 if (!reassembly_buf) {
155 LOG_ERR("No buffer allocated for the result reassembly");
156 return NULL;
157 }
158
159 if (reassembly_buf->len) {
160 LOG_WRN("Over-written incomplete CS subevent results");
161 }
162
163 net_buf_reset(reassembly_buf);
164
165 bool success = add_reassembly_data(reassembly_buf, steps, step_data_len);
166
167 return success ? reassembly_buf : NULL;
168 }
169
170 /** @brief Adds more step data to reassembly buffer identified by the connection handle
171 *
172 * @param conn_handle Connection handle
173 * @param steps Step data
174 * @param step_data_len Step data length
175 * @return struct net_buf* Pointer to reassembly buffer, NULL if fails to insert data
176 */
continue_reassembly(uint16_t conn_handle,const uint8_t * steps,uint16_t step_data_len)177 static struct net_buf *continue_reassembly(uint16_t conn_handle, const uint8_t *steps,
178 uint16_t step_data_len)
179 {
180 struct net_buf *reassembly_buf = get_reassembly_buf(conn_handle, false);
181
182 if (!reassembly_buf) {
183 LOG_ERR("No reassembly buffer was allocated for this CS procedure, possibly due to "
184 "an out-of-order subevent result continue event");
185 return NULL;
186 }
187
188 if (!reassembly_buf->len) {
189 LOG_WRN("Discarded out-of-order partial CS subevent results");
190 return NULL;
191 }
192
193 if (!step_data_len) {
194 return reassembly_buf;
195 }
196
197 bool success = add_reassembly_data(reassembly_buf, steps, step_data_len);
198
199 return success ? reassembly_buf : NULL;
200 }
201
202 /**
203 * @brief Disconnect callback to clear any dangling reassembly buffer
204 *
205 * @param conn Connection
206 * @param reason Reason
207 */
clear_on_disconnect(struct bt_conn * conn,uint8_t reason)208 static void clear_on_disconnect(struct bt_conn *conn, uint8_t reason)
209 {
210 struct net_buf *buf = get_reassembly_buf(conn->handle, false);
211
212 if (buf) {
213 free_reassembly_buf(&buf);
214 }
215 }
216
217 /** @brief Invokes user callback for new subevent results
218 *
219 * @param conn Connection context, NULL for CS Test subevent results
220 * @param p_result Pointer to subevent results
221 */
invoke_subevent_result_callback(struct bt_conn * conn,struct bt_conn_le_cs_subevent_result * p_result)222 static void invoke_subevent_result_callback(struct bt_conn *conn,
223 struct bt_conn_le_cs_subevent_result *p_result)
224 {
225 #if defined(CONFIG_BT_CHANNEL_SOUNDING_TEST)
226 if (!conn) {
227 cs_test_callbacks.le_cs_test_subevent_data_available(p_result);
228 } else
229 #endif /* CONFIG_BT_CHANNEL_SOUNDING_TEST */
230 {
231 notify_cs_subevent_result(conn, p_result);
232 }
233 }
234
235 /** @brief Resets reassembly results
236 *
237 */
reset_reassembly_results(void)238 static void reset_reassembly_results(void)
239 {
240 memset(&reassembled_result, 0, sizeof(struct bt_conn_le_cs_subevent_result));
241 }
242
243 /** @brief Converts PCT to a pair of int16_t
244 *
245 */
bt_le_cs_parse_pct(const uint8_t pct[3])246 struct bt_le_cs_iq_sample bt_le_cs_parse_pct(const uint8_t pct[3])
247 {
248 uint32_t pct_u32 = sys_get_le24(pct);
249
250 /* Extract I and Q. */
251 uint16_t i_u16 = pct_u32 & BT_HCI_LE_CS_PCT_I_MASK;
252 uint16_t q_u16 = (pct_u32 & BT_HCI_LE_CS_PCT_Q_MASK) >> 12;
253
254 /* Convert from 12-bit 2's complement to int16_t */
255 int16_t i = (i_u16 ^ BIT(11)) - BIT(11);
256 int16_t q = (q_u16 ^ BIT(11)) - BIT(11);
257
258 return (struct bt_le_cs_iq_sample){.i = i, .q = q};
259 }
260
bt_le_cs_set_valid_chmap_bits(uint8_t channel_map[10])261 void bt_le_cs_set_valid_chmap_bits(uint8_t channel_map[10])
262 {
263 memset(channel_map, 0xFF, 10);
264
265 /** Channels n = 0, 1, 23, 24, 25, 77, and 78 are not allowed and shall be set to zero.
266 * Channel 79 is reserved for future use and shall be set to zero.
267 */
268 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 0, 0);
269 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 1, 0);
270 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 23, 0);
271 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 24, 0);
272 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 25, 0);
273 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 77, 0);
274 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 78, 0);
275 BT_LE_CS_CHANNEL_BIT_SET_VAL(channel_map, 79, 0);
276 }
277
bt_le_cs_read_remote_supported_capabilities(struct bt_conn * conn)278 int bt_le_cs_read_remote_supported_capabilities(struct bt_conn *conn)
279 {
280 struct bt_hci_cp_le_read_remote_supported_capabilities *cp;
281 struct net_buf *buf;
282
283 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_READ_REMOTE_SUPPORTED_CAPABILITIES, sizeof(*cp));
284 if (!buf) {
285 return -ENOBUFS;
286 }
287
288 cp = net_buf_add(buf, sizeof(*cp));
289 cp->handle = sys_cpu_to_le16(conn->handle);
290
291 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_READ_REMOTE_SUPPORTED_CAPABILITIES, buf, NULL);
292 }
293
bt_hci_le_cs_read_remote_supported_capabilities_complete(struct net_buf * buf)294 void bt_hci_le_cs_read_remote_supported_capabilities_complete(struct net_buf *buf)
295 {
296 struct bt_conn *conn;
297 struct bt_conn_le_cs_capabilities remote_cs_capabilities;
298 struct bt_hci_evt_le_cs_read_remote_supported_capabilities_complete *evt;
299
300 if (buf->len < sizeof(*evt)) {
301 LOG_ERR("Unexpected end of buffer");
302 return;
303 }
304
305 evt = net_buf_pull_mem(buf, sizeof(*evt));
306 if (evt->status) {
307 LOG_WRN("Read Remote Supported Capabilities failed (status 0x%02X)", evt->status);
308 return;
309 }
310
311 conn = bt_conn_lookup_handle(sys_le16_to_cpu(evt->conn_handle), BT_CONN_TYPE_LE);
312 if (!conn) {
313 LOG_ERR("Could not lookup connection handle when reading remote CS capabilities");
314 return;
315 }
316
317 remote_cs_capabilities.num_config_supported = evt->num_config_supported;
318 remote_cs_capabilities.max_consecutive_procedures_supported =
319 sys_le16_to_cpu(evt->max_consecutive_procedures_supported);
320 remote_cs_capabilities.num_antennas_supported = evt->num_antennas_supported;
321 remote_cs_capabilities.max_antenna_paths_supported = evt->max_antenna_paths_supported;
322
323 remote_cs_capabilities.initiator_supported =
324 evt->roles_supported & BT_HCI_LE_CS_INITIATOR_ROLE_MASK;
325 remote_cs_capabilities.reflector_supported =
326 evt->roles_supported & BT_HCI_LE_CS_REFLECTOR_ROLE_MASK;
327 remote_cs_capabilities.mode_3_supported =
328 evt->modes_supported & BT_HCI_LE_CS_MODES_SUPPORTED_MODE_3_MASK;
329
330 remote_cs_capabilities.rtt_aa_only_n = evt->rtt_aa_only_n;
331 remote_cs_capabilities.rtt_sounding_n = evt->rtt_sounding_n;
332 remote_cs_capabilities.rtt_random_payload_n = evt->rtt_random_payload_n;
333
334 if (evt->rtt_aa_only_n) {
335 if (evt->rtt_capability & BT_HCI_LE_CS_RTT_AA_ONLY_N_10NS_MASK) {
336 remote_cs_capabilities.rtt_aa_only_precision =
337 BT_CONN_LE_CS_RTT_AA_ONLY_10NS;
338 } else {
339 remote_cs_capabilities.rtt_aa_only_precision =
340 BT_CONN_LE_CS_RTT_AA_ONLY_150NS;
341 }
342 } else {
343 remote_cs_capabilities.rtt_aa_only_precision = BT_CONN_LE_CS_RTT_AA_ONLY_NOT_SUPP;
344 }
345
346 if (evt->rtt_sounding_n) {
347 if (evt->rtt_capability & BT_HCI_LE_CS_RTT_SOUNDING_N_10NS_MASK) {
348 remote_cs_capabilities.rtt_sounding_precision =
349 BT_CONN_LE_CS_RTT_SOUNDING_10NS;
350 } else {
351 remote_cs_capabilities.rtt_sounding_precision =
352 BT_CONN_LE_CS_RTT_SOUNDING_150NS;
353 }
354 } else {
355 remote_cs_capabilities.rtt_sounding_precision = BT_CONN_LE_CS_RTT_SOUNDING_NOT_SUPP;
356 }
357
358 if (evt->rtt_random_payload_n) {
359 if (evt->rtt_capability & BT_HCI_LE_CS_RTT_RANDOM_PAYLOAD_N_10NS_MASK) {
360 remote_cs_capabilities.rtt_random_payload_precision =
361 BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_10NS;
362 } else {
363 remote_cs_capabilities.rtt_random_payload_precision =
364 BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_150NS;
365 }
366 } else {
367 remote_cs_capabilities.rtt_random_payload_precision =
368 BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_NOT_SUPP;
369 }
370
371 remote_cs_capabilities.phase_based_nadm_sounding_supported =
372 sys_le16_to_cpu(evt->nadm_sounding_capability) &
373 BT_HCI_LE_CS_NADM_SOUNDING_CAPABILITY_PHASE_BASED_MASK;
374
375 remote_cs_capabilities.phase_based_nadm_random_supported =
376 sys_le16_to_cpu(evt->nadm_random_capability) &
377 BT_HCI_LE_CS_NADM_RANDOM_CAPABILITY_PHASE_BASED_MASK;
378
379 remote_cs_capabilities.cs_sync_2m_phy_supported =
380 evt->cs_sync_phys_supported & BT_HCI_LE_CS_SYNC_PHYS_2M_MASK;
381
382 remote_cs_capabilities.cs_sync_2m_2bt_phy_supported =
383 evt->cs_sync_phys_supported & BT_HCI_LE_CS_SYNC_PHYS_2M_2BT_MASK;
384
385 remote_cs_capabilities.cs_without_fae_supported =
386 sys_le16_to_cpu(evt->subfeatures_supported) &
387 BT_HCI_LE_CS_SUBFEATURE_NO_TX_FAE_MASK;
388
389 remote_cs_capabilities.chsel_alg_3c_supported =
390 sys_le16_to_cpu(evt->subfeatures_supported) &
391 BT_HCI_LE_CS_SUBFEATURE_CHSEL_ALG_3C_MASK;
392
393 remote_cs_capabilities.pbr_from_rtt_sounding_seq_supported =
394 sys_le16_to_cpu(evt->subfeatures_supported) &
395 BT_HCI_LE_CS_SUBFEATURE_PBR_FROM_RTT_SOUNDING_SEQ_MASK;
396
397 remote_cs_capabilities.t_ip1_times_supported = sys_le16_to_cpu(evt->t_ip1_times_supported);
398 remote_cs_capabilities.t_ip2_times_supported = sys_le16_to_cpu(evt->t_ip2_times_supported);
399 remote_cs_capabilities.t_fcs_times_supported = sys_le16_to_cpu(evt->t_fcs_times_supported);
400 remote_cs_capabilities.t_pm_times_supported = sys_le16_to_cpu(evt->t_pm_times_supported);
401
402 remote_cs_capabilities.t_sw_time = evt->t_sw_time_supported;
403 remote_cs_capabilities.tx_snr_capability = evt->tx_snr_capability;
404
405 notify_remote_cs_capabilities(conn, remote_cs_capabilities);
406
407 bt_conn_unref(conn);
408 }
409
bt_le_cs_set_default_settings(struct bt_conn * conn,const struct bt_le_cs_set_default_settings_param * params)410 int bt_le_cs_set_default_settings(struct bt_conn *conn,
411 const struct bt_le_cs_set_default_settings_param *params)
412 {
413 struct bt_hci_cp_le_cs_set_default_settings *cp;
414 struct net_buf *buf;
415
416 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_SET_DEFAULT_SETTINGS, sizeof(*cp));
417 if (!buf) {
418 return -ENOBUFS;
419 }
420
421 cp = net_buf_add(buf, sizeof(*cp));
422 cp->handle = sys_cpu_to_le16(conn->handle);
423 cp->max_tx_power = params->max_tx_power;
424 cp->cs_sync_antenna_selection = params->cs_sync_antenna_selection;
425 cp->role_enable = 0;
426
427 if (params->enable_initiator_role) {
428 cp->role_enable |= BT_HCI_OP_LE_CS_INITIATOR_ROLE_MASK;
429 }
430
431 if (params->enable_reflector_role) {
432 cp->role_enable |= BT_HCI_OP_LE_CS_REFLECTOR_ROLE_MASK;
433 }
434
435 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_SET_DEFAULT_SETTINGS, buf, NULL);
436 }
437
bt_le_cs_read_remote_fae_table(struct bt_conn * conn)438 int bt_le_cs_read_remote_fae_table(struct bt_conn *conn)
439 {
440 struct bt_hci_cp_le_read_remote_fae_table *cp;
441 struct net_buf *buf;
442
443 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_READ_REMOTE_FAE_TABLE, sizeof(*cp));
444 if (!buf) {
445 return -ENOBUFS;
446 }
447
448 cp = net_buf_add(buf, sizeof(*cp));
449 cp->handle = sys_cpu_to_le16(conn->handle);
450
451 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_READ_REMOTE_FAE_TABLE, buf, NULL);
452 }
453
bt_hci_le_cs_read_remote_fae_table_complete(struct net_buf * buf)454 void bt_hci_le_cs_read_remote_fae_table_complete(struct net_buf *buf)
455 {
456 struct bt_conn *conn;
457 struct bt_conn_le_cs_fae_table fae_table;
458 struct bt_hci_evt_le_cs_read_remote_fae_table_complete *evt;
459
460 if (buf->len < sizeof(*evt)) {
461 LOG_ERR("Unexpected end of buffer");
462 return;
463 }
464
465 evt = net_buf_pull_mem(buf, sizeof(*evt));
466 if (evt->status) {
467 LOG_WRN("Read Remote FAE Table failed with status 0x%02X", evt->status);
468 return;
469 }
470
471 conn = bt_conn_lookup_handle(sys_le16_to_cpu(evt->conn_handle), BT_CONN_TYPE_LE);
472 if (!conn) {
473 LOG_ERR("Could not lookup connection handle when reading remote FAE Table");
474 return;
475 }
476
477 fae_table.remote_fae_table = evt->remote_fae_table;
478 notify_remote_cs_fae_table(conn, fae_table);
479
480 bt_conn_unref(conn);
481 }
482
483 #if defined(CONFIG_BT_CHANNEL_SOUNDING_TEST)
bt_le_cs_test_cb_register(struct bt_le_cs_test_cb cb)484 int bt_le_cs_test_cb_register(struct bt_le_cs_test_cb cb)
485 {
486 cs_test_callbacks = cb;
487 return 0;
488 }
489
bt_le_cs_start_test(const struct bt_le_cs_test_param * params)490 int bt_le_cs_start_test(const struct bt_le_cs_test_param *params)
491 {
492 struct bt_hci_op_le_cs_test *cp;
493 struct net_buf *buf;
494
495 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_TEST, sizeof(*cp));
496 if (!buf) {
497 return -ENOBUFS;
498 }
499
500 cp = net_buf_add(buf, sizeof(*cp));
501
502 cp->main_mode_type = params->main_mode;
503 cp->sub_mode_type = params->sub_mode;
504 cp->main_mode_repetition = params->main_mode_repetition;
505 cp->mode_0_steps = params->mode_0_steps;
506 cp->role = params->role;
507 cp->rtt_type = params->rtt_type;
508 cp->cs_sync_phy = params->cs_sync_phy;
509 cp->cs_sync_antenna_selection = params->cs_sync_antenna_selection;
510 sys_put_le24(params->subevent_len, cp->subevent_len);
511 cp->subevent_interval = sys_cpu_to_le16(params->subevent_interval);
512 cp->max_num_subevents = params->max_num_subevents;
513 cp->transmit_power_level = params->transmit_power_level;
514 cp->t_ip1_time = params->t_ip1_time;
515 cp->t_ip2_time = params->t_ip2_time;
516 cp->t_fcs_time = params->t_fcs_time;
517 cp->t_pm_time = params->t_pm_time;
518 cp->t_sw_time = params->t_sw_time;
519 cp->tone_antenna_config_selection = params->tone_antenna_config_selection;
520
521 cp->reserved = 0;
522
523 cp->snr_control_initiator = params->initiator_snr_control;
524 cp->snr_control_reflector = params->reflector_snr_control;
525 cp->drbg_nonce = sys_cpu_to_le16(params->drbg_nonce);
526 cp->channel_map_repetition = params->override_config_0.channel_map_repetition;
527 cp->override_config = sys_cpu_to_le16(params->override_config);
528
529 uint8_t override_parameters_length = 0;
530
531 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_0_MASK) {
532 const uint8_t num_channels = params->override_config_0.set.num_channels;
533
534 net_buf_add_u8(buf, num_channels);
535 override_parameters_length++;
536 net_buf_add_mem(buf, params->override_config_0.set.channels, num_channels);
537 override_parameters_length += num_channels;
538 } else {
539 net_buf_add_mem(buf, params->override_config_0.not_set.channel_map,
540 sizeof(params->override_config_0.not_set.channel_map));
541 net_buf_add_u8(buf, params->override_config_0.not_set.channel_selection_type);
542 net_buf_add_u8(buf, params->override_config_0.not_set.ch3c_shape);
543 net_buf_add_u8(buf, params->override_config_0.not_set.ch3c_jump);
544
545 override_parameters_length +=
546 (sizeof(params->override_config_0.not_set.channel_map) +
547 sizeof(params->override_config_0.not_set.channel_selection_type) +
548 sizeof(params->override_config_0.not_set.ch3c_shape) +
549 sizeof(params->override_config_0.not_set.ch3c_jump));
550 }
551
552 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_2_MASK) {
553 net_buf_add_mem(buf, ¶ms->override_config_2, sizeof(params->override_config_2));
554 override_parameters_length += sizeof(params->override_config_2);
555 }
556
557 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_3_MASK) {
558 net_buf_add_mem(buf, ¶ms->override_config_3, sizeof(params->override_config_3));
559 override_parameters_length += sizeof(params->override_config_3);
560 }
561
562 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_4_MASK) {
563 net_buf_add_mem(buf, ¶ms->override_config_4, sizeof(params->override_config_4));
564 override_parameters_length += sizeof(params->override_config_4);
565 }
566
567 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_5_MASK) {
568 net_buf_add_le32(buf, params->override_config_5.cs_sync_aa_initiator);
569 net_buf_add_le32(buf, params->override_config_5.cs_sync_aa_reflector);
570 override_parameters_length += sizeof(params->override_config_5);
571 }
572
573 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_6_MASK) {
574 net_buf_add_mem(buf, ¶ms->override_config_6, sizeof(params->override_config_6));
575 override_parameters_length += sizeof(params->override_config_6);
576 }
577
578 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_7_MASK) {
579 net_buf_add_mem(buf, ¶ms->override_config_7, sizeof(params->override_config_7));
580 override_parameters_length += sizeof(params->override_config_7);
581 }
582
583 if (params->override_config & BT_HCI_OP_LE_CS_TEST_OVERRIDE_CONFIG_8_MASK) {
584 net_buf_add_mem(buf, ¶ms->override_config_8, sizeof(params->override_config_8));
585 override_parameters_length += sizeof(params->override_config_8);
586 }
587
588 cp->override_parameters_length = override_parameters_length;
589
590 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_TEST, buf, NULL);
591 }
592 #endif /* CONFIG_BT_CHANNEL_SOUNDING_TEST */
593
bt_hci_le_cs_subevent_result(struct net_buf * buf)594 void bt_hci_le_cs_subevent_result(struct net_buf *buf)
595 {
596 struct bt_conn *conn = NULL;
597 struct bt_hci_evt_le_cs_subevent_result *evt;
598 struct bt_conn_le_cs_subevent_result result;
599 struct bt_conn_le_cs_subevent_result *p_result = &result;
600 struct net_buf_simple step_data_buf;
601 struct net_buf *reassembly_buf = NULL;
602
603 if (buf->len < sizeof(*evt)) {
604 LOG_ERR("Unexpected end of buffer");
605 return;
606 }
607
608 evt = net_buf_pull_mem(buf, sizeof(*evt));
609 uint16_t conn_handle = sys_le16_to_cpu(evt->conn_handle);
610
611 #if defined(CONFIG_BT_CHANNEL_SOUNDING_TEST)
612 if (conn_handle == BT_HCI_LE_CS_TEST_CONN_HANDLE) {
613 if (!cs_test_callbacks.le_cs_test_subevent_data_available) {
614 LOG_WRN("No callback registered. Discarded subevent results from CS Test.");
615 return;
616 }
617 } else
618 #endif /* CONFIG_BT_CHANNEL_SOUNDING_TEST */
619 {
620 conn = bt_conn_lookup_handle(conn_handle, BT_CONN_TYPE_LE);
621 if (!conn) {
622 LOG_ERR("Unknown connection handle when processing subevent results");
623 return;
624 }
625 }
626
627 if (evt->subevent_done_status != BT_HCI_LE_CS_SUBEVENT_DONE_STATUS_PARTIAL) {
628 p_result->step_data_buf = NULL;
629 if (evt->num_steps_reported) {
630 net_buf_simple_init_with_data(&step_data_buf, evt->steps, buf->len);
631 p_result->step_data_buf = &step_data_buf;
632 }
633 } else {
634 if (evt->procedure_done_status != BT_HCI_LE_CS_PROCEDURE_DONE_STATUS_PARTIAL) {
635 LOG_WRN("Procedure status is inconsistent with subevent status. Discarding "
636 "subevent results");
637 goto abort;
638 }
639
640 if (!evt->num_steps_reported) {
641 LOG_WRN("Discarding partial results without step data");
642 goto abort;
643 }
644
645 reassembly_buf = start_reassembly(conn_handle, evt->steps, buf->len);
646 if (!reassembly_buf) {
647 goto abort;
648 }
649
650 p_result = &reassembled_result;
651 p_result->step_data_buf = (struct net_buf_simple *)&reassembly_buf->data;
652 }
653
654 p_result->header.procedure_counter = sys_le16_to_cpu(evt->procedure_counter);
655 p_result->header.frequency_compensation = sys_le16_to_cpu(evt->frequency_compensation);
656 p_result->header.procedure_done_status = evt->procedure_done_status;
657 p_result->header.subevent_done_status = evt->subevent_done_status;
658 p_result->header.procedure_abort_reason = evt->procedure_abort_reason;
659 p_result->header.subevent_abort_reason = evt->subevent_abort_reason;
660 p_result->header.reference_power_level = evt->reference_power_level;
661 p_result->header.num_antenna_paths = evt->num_antenna_paths;
662 p_result->header.num_steps_reported = evt->num_steps_reported;
663 p_result->header.abort_step =
664 evt->subevent_done_status == BT_HCI_LE_CS_SUBEVENT_DONE_STATUS_ABORTED ? 0 : 255;
665
666 p_result->header.config_id = 0;
667 p_result->header.start_acl_conn_event = 0;
668 if (conn) {
669 p_result->header.config_id = evt->config_id;
670 p_result->header.start_acl_conn_event =
671 sys_le16_to_cpu(evt->start_acl_conn_event_counter);
672 }
673
674 if (evt->subevent_done_status != BT_HCI_LE_CS_SUBEVENT_DONE_STATUS_PARTIAL) {
675 invoke_subevent_result_callback(conn, p_result);
676 }
677
678 if (evt->procedure_done_status != BT_CONN_LE_CS_PROCEDURE_INCOMPLETE) {
679 /* We can now clear the any reassembly buffer allocated for this procedure,
680 * to avoid code duplication, we're using the abort label to do so
681 */
682 goto abort;
683 }
684
685 if (conn) {
686 bt_conn_unref(conn);
687 conn = NULL;
688 }
689
690 return;
691
692 abort:
693 if (conn) {
694 bt_conn_unref(conn);
695 conn = NULL;
696 }
697
698 reassembly_buf = get_reassembly_buf(conn_handle, false);
699 if (reassembly_buf) {
700 free_reassembly_buf(&reassembly_buf);
701 }
702 }
703
bt_hci_le_cs_subevent_result_continue(struct net_buf * buf)704 void bt_hci_le_cs_subevent_result_continue(struct net_buf *buf)
705 {
706 struct bt_conn *conn = NULL;
707 struct bt_hci_evt_le_cs_subevent_result_continue *evt;
708 struct net_buf *reassembly_buf = NULL;
709 uint16_t conn_handle;
710
711 if (buf->len < sizeof(*evt)) {
712 LOG_ERR("Unexpected end of buffer");
713 return;
714 }
715
716 evt = net_buf_pull_mem(buf, sizeof(*evt));
717 conn_handle = sys_le16_to_cpu(evt->conn_handle);
718
719 #if defined(CONFIG_BT_CHANNEL_SOUNDING_TEST)
720 if (conn_handle == BT_HCI_LE_CS_TEST_CONN_HANDLE) {
721 if (!cs_test_callbacks.le_cs_test_subevent_data_available) {
722 LOG_WRN("No callback registered. Discarded subevent results from CS Test.");
723 return;
724 }
725 } else
726 #endif /* CONFIG_BT_CHANNEL_SOUNDING_TEST */
727 {
728 conn = bt_conn_lookup_handle(conn_handle, BT_CONN_TYPE_LE);
729 if (!conn) {
730 LOG_ERR("Unknown connection handle when processing subevent results");
731 return;
732 }
733 }
734
735 uint16_t step_data_len = evt->num_steps_reported ? buf->len : 0;
736
737 reassembly_buf = continue_reassembly(conn_handle, evt->steps, step_data_len);
738 if (!reassembly_buf) {
739 goto abort;
740 }
741
742 reassembled_result.header.procedure_done_status = evt->procedure_done_status;
743 reassembled_result.header.subevent_done_status = evt->subevent_done_status;
744 reassembled_result.header.procedure_abort_reason = evt->procedure_abort_reason;
745 reassembled_result.header.subevent_abort_reason = evt->subevent_abort_reason;
746
747 if (evt->num_antenna_paths != reassembled_result.header.num_antenna_paths) {
748 LOG_WRN("Received inconsistent number of antenna paths from the controller: %d, "
749 "previous number was: %d",
750 evt->num_antenna_paths, reassembled_result.header.num_antenna_paths);
751 }
752
753 if (evt->subevent_done_status == BT_HCI_LE_CS_SUBEVENT_DONE_STATUS_ABORTED &&
754 reassembled_result.header.num_steps_reported < reassembled_result.header.abort_step) {
755 reassembled_result.header.abort_step = reassembled_result.header.num_steps_reported;
756 }
757
758 reassembled_result.header.num_steps_reported += evt->num_steps_reported;
759
760 if (evt->subevent_done_status != BT_HCI_LE_CS_SUBEVENT_DONE_STATUS_PARTIAL) {
761 invoke_subevent_result_callback(conn, &reassembled_result);
762 net_buf_reset(reassembly_buf);
763 reset_reassembly_results();
764 }
765
766 if (evt->procedure_done_status != BT_HCI_LE_CS_PROCEDURE_DONE_STATUS_PARTIAL) {
767 if (evt->subevent_done_status == BT_HCI_LE_CS_SUBEVENT_DONE_STATUS_PARTIAL) {
768 LOG_WRN("Procedure status is inconsistent with subevent status. Discarding "
769 "subevent results");
770 goto abort;
771 }
772
773 free_reassembly_buf(&reassembly_buf);
774 }
775
776 if (conn) {
777 bt_conn_unref(conn);
778 conn = NULL;
779 }
780
781 return;
782
783 abort:
784 if (conn) {
785 bt_conn_unref(conn);
786 conn = NULL;
787 }
788
789 if (reassembly_buf) {
790 free_reassembly_buf(&reassembly_buf);
791 }
792 }
793
bt_hci_le_cs_config_complete_event(struct net_buf * buf)794 void bt_hci_le_cs_config_complete_event(struct net_buf *buf)
795 {
796 struct bt_hci_evt_le_cs_config_complete *evt;
797 struct bt_conn_le_cs_config config;
798 struct bt_conn *conn;
799
800 if (buf->len < sizeof(*evt)) {
801 LOG_ERR("Unexpected end of buffer");
802 return;
803 }
804
805 evt = net_buf_pull_mem(buf, sizeof(*evt));
806 if (evt->status) {
807 LOG_WRN("CS Config failed (status 0x%02X)", evt->status);
808 return;
809 }
810
811 conn = bt_conn_lookup_handle(sys_le16_to_cpu(evt->handle), BT_CONN_TYPE_LE);
812 if (!conn) {
813 LOG_ERR("Could not lookup connection handle when reading CS configuration");
814 return;
815 }
816
817 if (evt->action == BT_HCI_LE_CS_CONFIG_ACTION_REMOVED) {
818 notify_cs_config_removed(conn, evt->config_id);
819 bt_conn_unref(conn);
820 return;
821 }
822
823 config.id = evt->config_id;
824 config.main_mode_type = evt->main_mode_type;
825 config.sub_mode_type = evt->sub_mode_type;
826 config.min_main_mode_steps = evt->min_main_mode_steps;
827 config.max_main_mode_steps = evt->max_main_mode_steps;
828 config.main_mode_repetition = evt->main_mode_repetition;
829 config.mode_0_steps = evt->mode_0_steps;
830 config.role = evt->role;
831 config.rtt_type = evt->rtt_type;
832 config.cs_sync_phy = evt->cs_sync_phy;
833 config.channel_map_repetition = evt->channel_map_repetition;
834 config.channel_selection_type = evt->channel_selection_type;
835 config.ch3c_shape = evt->ch3c_shape;
836 config.ch3c_jump = evt->ch3c_jump;
837 config.t_ip1_time_us = evt->t_ip1_time;
838 config.t_ip2_time_us = evt->t_ip2_time;
839 config.t_fcs_time_us = evt->t_fcs_time;
840 config.t_pm_time_us = evt->t_pm_time;
841 memcpy(config.channel_map, evt->channel_map, ARRAY_SIZE(config.channel_map));
842
843 notify_cs_config_created(conn, &config);
844 bt_conn_unref(conn);
845 }
846
bt_le_cs_create_config(struct bt_conn * conn,struct bt_le_cs_create_config_params * params,enum bt_le_cs_create_config_context context)847 int bt_le_cs_create_config(struct bt_conn *conn, struct bt_le_cs_create_config_params *params,
848 enum bt_le_cs_create_config_context context)
849 {
850 struct bt_hci_cp_le_cs_create_config *cp;
851 struct net_buf *buf;
852
853 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_CREATE_CONFIG, sizeof(*cp));
854 if (!buf) {
855 return -ENOBUFS;
856 }
857
858 cp = net_buf_add(buf, sizeof(*cp));
859 cp->handle = sys_cpu_to_le16(conn->handle);
860 cp->config_id = params->id;
861 cp->create_context = context;
862 cp->main_mode_type = params->main_mode_type;
863 cp->sub_mode_type = params->sub_mode_type;
864 cp->min_main_mode_steps = params->min_main_mode_steps;
865 cp->max_main_mode_steps = params->max_main_mode_steps;
866 cp->main_mode_repetition = params->main_mode_repetition;
867 cp->mode_0_steps = params->mode_0_steps;
868 cp->role = params->role;
869 cp->rtt_type = params->rtt_type;
870 cp->cs_sync_phy = params->cs_sync_phy;
871 cp->channel_map_repetition = params->channel_map_repetition;
872 cp->channel_selection_type = params->channel_selection_type;
873 cp->ch3c_shape = params->ch3c_shape;
874 cp->ch3c_jump = params->ch3c_jump;
875 cp->reserved = 0;
876 memcpy(cp->channel_map, params->channel_map, ARRAY_SIZE(cp->channel_map));
877
878 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_CREATE_CONFIG, buf, NULL);
879 }
880
bt_le_cs_remove_config(struct bt_conn * conn,uint8_t config_id)881 int bt_le_cs_remove_config(struct bt_conn *conn, uint8_t config_id)
882 {
883 struct bt_hci_cp_le_cs_remove_config *cp;
884 struct net_buf *buf;
885
886 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_REMOVE_CONFIG, sizeof(*cp));
887 if (!buf) {
888 return -ENOBUFS;
889 }
890
891 cp = net_buf_add(buf, sizeof(*cp));
892 cp->handle = sys_cpu_to_le16(conn->handle);
893 cp->config_id = config_id;
894
895 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_REMOVE_CONFIG, buf, NULL);
896 }
897
bt_le_cs_security_enable(struct bt_conn * conn)898 int bt_le_cs_security_enable(struct bt_conn *conn)
899 {
900 struct bt_hci_cp_le_security_enable *cp;
901 struct net_buf *buf;
902
903 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_SECURITY_ENABLE, sizeof(*cp));
904 if (!buf) {
905 return -ENOBUFS;
906 }
907
908 cp = net_buf_add(buf, sizeof(*cp));
909 cp->handle = sys_cpu_to_le16(conn->handle);
910
911 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_SECURITY_ENABLE, buf, NULL);
912 }
913
bt_le_cs_procedure_enable(struct bt_conn * conn,const struct bt_le_cs_procedure_enable_param * params)914 int bt_le_cs_procedure_enable(struct bt_conn *conn,
915 const struct bt_le_cs_procedure_enable_param *params)
916 {
917 struct bt_hci_cp_le_procedure_enable *cp;
918 struct net_buf *buf;
919
920 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_PROCEDURE_ENABLE, sizeof(*cp));
921 if (!buf) {
922 return -ENOBUFS;
923 }
924
925 cp = net_buf_add(buf, sizeof(*cp));
926 cp->handle = sys_cpu_to_le16(conn->handle);
927 cp->config_id = params->config_id;
928 cp->enable = params->enable;
929
930 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_PROCEDURE_ENABLE, buf, NULL);
931 }
932
bt_le_cs_set_procedure_parameters(struct bt_conn * conn,const struct bt_le_cs_set_procedure_parameters_param * params)933 int bt_le_cs_set_procedure_parameters(struct bt_conn *conn,
934 const struct bt_le_cs_set_procedure_parameters_param *params)
935 {
936 struct bt_hci_cp_le_set_procedure_parameters *cp;
937 struct net_buf *buf;
938
939 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_SET_PROCEDURE_PARAMETERS, sizeof(*cp));
940 if (!buf) {
941 return -ENOBUFS;
942 }
943
944 cp = net_buf_add(buf, sizeof(*cp));
945 cp->handle = sys_cpu_to_le16(conn->handle);
946 cp->config_id = params->config_id;
947 cp->max_procedure_len = sys_cpu_to_le16(params->max_procedure_len);
948 cp->min_procedure_interval = sys_cpu_to_le16(params->min_procedure_interval);
949 cp->max_procedure_interval = sys_cpu_to_le16(params->max_procedure_interval);
950 cp->max_procedure_count = sys_cpu_to_le16(params->max_procedure_count);
951 sys_put_le24(params->min_subevent_len, cp->min_subevent_len);
952 sys_put_le24(params->max_subevent_len, cp->max_subevent_len);
953 cp->tone_antenna_config_selection = params->tone_antenna_config_selection;
954 cp->phy = params->phy;
955 cp->tx_power_delta = params->tx_power_delta;
956 cp->preferred_peer_antenna = params->preferred_peer_antenna;
957 cp->snr_control_initiator = params->snr_control_initiator;
958 cp->snr_control_reflector = params->snr_control_reflector;
959
960 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_SET_PROCEDURE_PARAMETERS, buf, NULL);
961 }
962
bt_le_cs_set_channel_classification(uint8_t channel_classification[10])963 int bt_le_cs_set_channel_classification(uint8_t channel_classification[10])
964 {
965 uint8_t *cp;
966 struct net_buf *buf;
967
968 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_SET_CHANNEL_CLASSIFICATION, 10);
969 if (!buf) {
970 return -ENOBUFS;
971 }
972
973 cp = net_buf_add(buf, 10);
974 memcpy(cp, channel_classification, 10);
975
976 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_SET_CHANNEL_CLASSIFICATION, buf, NULL);
977 }
978
bt_le_cs_read_local_supported_capabilities(struct bt_conn_le_cs_capabilities * ret)979 int bt_le_cs_read_local_supported_capabilities(struct bt_conn_le_cs_capabilities *ret)
980 {
981 struct bt_hci_rp_le_read_local_supported_capabilities *rp;
982 struct net_buf *rsp;
983
984 int err =
985 bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_READ_LOCAL_SUPPORTED_CAPABILITIES, NULL, &rsp);
986
987 if (err) {
988 return err;
989 }
990
991 rp = (void *)rsp->data;
992
993 uint8_t status = rp->status;
994
995 ret->num_config_supported = rp->num_config_supported;
996 ret->max_consecutive_procedures_supported =
997 sys_le16_to_cpu(rp->max_consecutive_procedures_supported);
998 ret->num_antennas_supported = rp->num_antennas_supported;
999 ret->max_antenna_paths_supported = rp->max_antenna_paths_supported;
1000
1001 ret->initiator_supported = rp->roles_supported & BT_HCI_LE_CS_INITIATOR_ROLE_MASK;
1002 ret->reflector_supported = rp->roles_supported & BT_HCI_LE_CS_REFLECTOR_ROLE_MASK;
1003 ret->mode_3_supported = rp->modes_supported & BT_HCI_LE_CS_MODES_SUPPORTED_MODE_3_MASK;
1004
1005 ret->rtt_aa_only_n = rp->rtt_aa_only_n;
1006 ret->rtt_sounding_n = rp->rtt_sounding_n;
1007 ret->rtt_random_payload_n = rp->rtt_random_payload_n;
1008
1009 if (rp->rtt_aa_only_n) {
1010 if (rp->rtt_capability & BT_HCI_LE_CS_RTT_AA_ONLY_N_10NS_MASK) {
1011 ret->rtt_aa_only_precision = BT_CONN_LE_CS_RTT_AA_ONLY_10NS;
1012 } else {
1013 ret->rtt_aa_only_precision = BT_CONN_LE_CS_RTT_AA_ONLY_150NS;
1014 }
1015 } else {
1016 ret->rtt_aa_only_precision = BT_CONN_LE_CS_RTT_AA_ONLY_NOT_SUPP;
1017 }
1018
1019 if (rp->rtt_sounding_n) {
1020 if (rp->rtt_capability & BT_HCI_LE_CS_RTT_SOUNDING_N_10NS_MASK) {
1021 ret->rtt_sounding_precision = BT_CONN_LE_CS_RTT_SOUNDING_10NS;
1022 } else {
1023 ret->rtt_sounding_precision = BT_CONN_LE_CS_RTT_SOUNDING_150NS;
1024 }
1025 } else {
1026 ret->rtt_sounding_precision = BT_CONN_LE_CS_RTT_SOUNDING_NOT_SUPP;
1027 }
1028
1029 if (rp->rtt_random_payload_n) {
1030 if (rp->rtt_capability & BT_HCI_LE_CS_RTT_RANDOM_PAYLOAD_N_10NS_MASK) {
1031 ret->rtt_random_payload_precision = BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_10NS;
1032 } else {
1033 ret->rtt_random_payload_precision = BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_150NS;
1034 }
1035 } else {
1036 ret->rtt_random_payload_precision = BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_NOT_SUPP;
1037 }
1038
1039 ret->phase_based_nadm_sounding_supported =
1040 sys_le16_to_cpu(rp->nadm_sounding_capability) &
1041 BT_HCI_LE_CS_NADM_SOUNDING_CAPABILITY_PHASE_BASED_MASK;
1042
1043 ret->phase_based_nadm_random_supported =
1044 sys_le16_to_cpu(rp->nadm_random_capability) &
1045 BT_HCI_LE_CS_NADM_RANDOM_CAPABILITY_PHASE_BASED_MASK;
1046
1047 ret->cs_sync_2m_phy_supported = rp->cs_sync_phys_supported & BT_HCI_LE_CS_SYNC_PHYS_2M_MASK;
1048
1049 ret->cs_sync_2m_2bt_phy_supported =
1050 rp->cs_sync_phys_supported & BT_HCI_LE_CS_SYNC_PHYS_2M_2BT_MASK;
1051
1052 ret->cs_without_fae_supported =
1053 sys_le16_to_cpu(rp->subfeatures_supported) & BT_HCI_LE_CS_SUBFEATURE_NO_TX_FAE_MASK;
1054
1055 ret->chsel_alg_3c_supported = sys_le16_to_cpu(rp->subfeatures_supported) &
1056 BT_HCI_LE_CS_SUBFEATURE_CHSEL_ALG_3C_MASK;
1057
1058 ret->pbr_from_rtt_sounding_seq_supported =
1059 sys_le16_to_cpu(rp->subfeatures_supported) &
1060 BT_HCI_LE_CS_SUBFEATURE_PBR_FROM_RTT_SOUNDING_SEQ_MASK;
1061
1062 ret->t_ip1_times_supported = sys_le16_to_cpu(rp->t_ip1_times_supported);
1063 ret->t_ip2_times_supported = sys_le16_to_cpu(rp->t_ip2_times_supported);
1064 ret->t_fcs_times_supported = sys_le16_to_cpu(rp->t_fcs_times_supported);
1065 ret->t_pm_times_supported = sys_le16_to_cpu(rp->t_pm_times_supported);
1066
1067 ret->t_sw_time = rp->t_sw_time_supported;
1068 ret->tx_snr_capability = rp->tx_snr_capability;
1069
1070 net_buf_unref(rsp);
1071 return status;
1072 }
1073
bt_le_cs_write_cached_remote_supported_capabilities(struct bt_conn * conn,const struct bt_conn_le_cs_capabilities * params)1074 int bt_le_cs_write_cached_remote_supported_capabilities(
1075 struct bt_conn *conn, const struct bt_conn_le_cs_capabilities *params)
1076 {
1077 struct bt_hci_cp_le_write_cached_remote_supported_capabilities *cp;
1078 struct net_buf *buf;
1079
1080 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_WRITE_CACHED_REMOTE_SUPPORTED_CAPABILITIES,
1081 sizeof(*cp));
1082
1083 if (!buf) {
1084 return -ENOBUFS;
1085 }
1086
1087 cp = net_buf_add(buf, sizeof(*cp));
1088
1089 cp->handle = sys_cpu_to_le16(conn->handle);
1090
1091 cp->num_config_supported = params->num_config_supported;
1092
1093 cp->max_consecutive_procedures_supported =
1094 sys_cpu_to_le16(params->max_consecutive_procedures_supported);
1095
1096 cp->num_antennas_supported = params->num_antennas_supported;
1097 cp->max_antenna_paths_supported = params->max_antenna_paths_supported;
1098
1099 cp->roles_supported = 0;
1100 if (params->initiator_supported) {
1101 cp->roles_supported |= BT_HCI_LE_CS_INITIATOR_ROLE_MASK;
1102 }
1103 if (params->reflector_supported) {
1104 cp->roles_supported |= BT_HCI_LE_CS_REFLECTOR_ROLE_MASK;
1105 }
1106
1107 cp->modes_supported = 0;
1108 if (params->mode_3_supported) {
1109 cp->modes_supported |= BT_HCI_LE_CS_MODES_SUPPORTED_MODE_3_MASK;
1110 }
1111
1112 cp->rtt_aa_only_n = params->rtt_aa_only_n;
1113 cp->rtt_sounding_n = params->rtt_sounding_n;
1114 cp->rtt_random_payload_n = params->rtt_random_payload_n;
1115
1116 cp->rtt_capability = 0;
1117 if (params->rtt_aa_only_precision == BT_CONN_LE_CS_RTT_AA_ONLY_10NS) {
1118 cp->rtt_capability |= BT_HCI_LE_CS_RTT_AA_ONLY_N_10NS_MASK;
1119 }
1120
1121 if (params->rtt_sounding_precision == BT_CONN_LE_CS_RTT_SOUNDING_10NS) {
1122 cp->rtt_capability |= BT_HCI_LE_CS_RTT_SOUNDING_N_10NS_MASK;
1123 }
1124
1125 if (params->rtt_random_payload_precision == BT_CONN_LE_CS_RTT_RANDOM_PAYLOAD_10NS) {
1126 cp->rtt_capability |= BT_HCI_LE_CS_RTT_RANDOM_PAYLOAD_N_10NS_MASK;
1127 }
1128
1129 cp->nadm_sounding_capability = 0;
1130 if (params->phase_based_nadm_sounding_supported) {
1131 cp->nadm_sounding_capability |=
1132 sys_cpu_to_le16(BT_HCI_LE_CS_NADM_SOUNDING_CAPABILITY_PHASE_BASED_MASK);
1133 }
1134
1135 cp->nadm_random_capability = 0;
1136 if (params->phase_based_nadm_random_supported) {
1137 cp->nadm_random_capability |=
1138 sys_cpu_to_le16(BT_HCI_LE_CS_NADM_RANDOM_CAPABILITY_PHASE_BASED_MASK);
1139 }
1140
1141 cp->cs_sync_phys_supported = 0;
1142 if (params->cs_sync_2m_phy_supported) {
1143 cp->cs_sync_phys_supported |= BT_HCI_LE_CS_SYNC_PHYS_2M_MASK;
1144 }
1145 if (params->cs_sync_2m_2bt_phy_supported) {
1146 cp->cs_sync_phys_supported |= BT_HCI_LE_CS_SYNC_PHYS_2M_2BT_MASK;
1147 }
1148
1149 cp->subfeatures_supported = 0;
1150 if (params->cs_without_fae_supported) {
1151 cp->subfeatures_supported |=
1152 sys_cpu_to_le16(BT_HCI_LE_CS_SUBFEATURE_NO_TX_FAE_MASK);
1153 }
1154 if (params->chsel_alg_3c_supported) {
1155 cp->subfeatures_supported |=
1156 sys_cpu_to_le16(BT_HCI_LE_CS_SUBFEATURE_CHSEL_ALG_3C_MASK);
1157 }
1158 if (params->pbr_from_rtt_sounding_seq_supported) {
1159 cp->subfeatures_supported |=
1160 sys_cpu_to_le16(BT_HCI_LE_CS_SUBFEATURE_PBR_FROM_RTT_SOUNDING_SEQ_MASK);
1161 }
1162
1163 cp->t_ip1_times_supported = sys_cpu_to_le16(params->t_ip1_times_supported);
1164 cp->t_ip2_times_supported = sys_cpu_to_le16(params->t_ip2_times_supported);
1165 cp->t_fcs_times_supported = sys_cpu_to_le16(params->t_fcs_times_supported);
1166 cp->t_pm_times_supported = sys_cpu_to_le16(params->t_pm_times_supported);
1167 cp->t_sw_time_supported = params->t_sw_time;
1168 cp->tx_snr_capability = params->tx_snr_capability;
1169
1170 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_WRITE_CACHED_REMOTE_SUPPORTED_CAPABILITIES, buf,
1171 NULL);
1172 }
1173
bt_le_cs_write_cached_remote_fae_table(struct bt_conn * conn,int8_t remote_fae_table[72])1174 int bt_le_cs_write_cached_remote_fae_table(struct bt_conn *conn, int8_t remote_fae_table[72])
1175 {
1176 struct bt_hci_cp_le_write_cached_remote_fae_table *cp;
1177 struct net_buf *buf;
1178
1179 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_WRITE_CACHED_REMOTE_FAE_TABLE, sizeof(*cp));
1180 if (!buf) {
1181 return -ENOBUFS;
1182 }
1183
1184 cp = net_buf_add(buf, sizeof(*cp));
1185
1186 cp->handle = sys_cpu_to_le16(conn->handle);
1187 memcpy(cp->remote_fae_table, remote_fae_table, sizeof(cp->remote_fae_table));
1188
1189 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_WRITE_CACHED_REMOTE_FAE_TABLE, buf, NULL);
1190 }
1191
bt_hci_le_cs_security_enable_complete(struct net_buf * buf)1192 void bt_hci_le_cs_security_enable_complete(struct net_buf *buf)
1193 {
1194 struct bt_conn *conn;
1195
1196 struct bt_hci_evt_le_cs_security_enable_complete *evt;
1197
1198 if (buf->len < sizeof(*evt)) {
1199 LOG_ERR("Unexpected end of buffer");
1200 return;
1201 }
1202
1203 evt = net_buf_pull_mem(buf, sizeof(*evt));
1204 if (evt->status) {
1205 LOG_WRN("Security Enable failed with status 0x%02X", evt->status);
1206 return;
1207 }
1208
1209 conn = bt_conn_lookup_handle(sys_le16_to_cpu(evt->handle), BT_CONN_TYPE_LE);
1210 if (!conn) {
1211 LOG_ERR("Can't lookup conn handle when reading Security Enable Complete event");
1212 return;
1213 }
1214
1215 notify_cs_security_enable_available(conn);
1216
1217 bt_conn_unref(conn);
1218 }
1219
bt_hci_le_cs_procedure_enable_complete(struct net_buf * buf)1220 void bt_hci_le_cs_procedure_enable_complete(struct net_buf *buf)
1221 {
1222 struct bt_conn *conn;
1223
1224 struct bt_hci_evt_le_cs_procedure_enable_complete *evt;
1225 struct bt_conn_le_cs_procedure_enable_complete params;
1226
1227 if (buf->len < sizeof(*evt)) {
1228 LOG_ERR("Unexpected end of buffer");
1229 return;
1230 }
1231
1232 evt = net_buf_pull_mem(buf, sizeof(*evt));
1233 if (evt->status) {
1234 LOG_WRN("Procedure Enable failed with status 0x%02X", evt->status);
1235 return;
1236 }
1237
1238 conn = bt_conn_lookup_handle(sys_le16_to_cpu(evt->handle), BT_CONN_TYPE_LE);
1239 if (!conn) {
1240 LOG_ERR("Can't lookup conn handle when reading Procedure Enable Complete event");
1241 return;
1242 }
1243
1244 if (evt->state == BT_HCI_OP_LE_CS_PROCEDURES_DISABLED) {
1245 struct net_buf *reassembly_buf = get_reassembly_buf(conn->handle, false);
1246
1247 if (reassembly_buf) {
1248 LOG_WRN("De-allocating a dangling reassembly buffer");
1249 free_reassembly_buf(&reassembly_buf);
1250 }
1251 }
1252
1253 params.config_id = evt->config_id;
1254 params.state = evt->state;
1255 params.tone_antenna_config_selection = evt->tone_antenna_config_selection;
1256 params.selected_tx_power = evt->selected_tx_power;
1257 params.subevent_len = sys_get_le24(evt->subevent_len);
1258 params.subevents_per_event = evt->subevents_per_event;
1259 params.subevent_interval = sys_le16_to_cpu(evt->subevent_interval);
1260 params.event_interval = sys_le16_to_cpu(evt->event_interval);
1261 params.procedure_interval = sys_le16_to_cpu(evt->procedure_interval);
1262 params.procedure_count = sys_le16_to_cpu(evt->procedure_count);
1263 params.max_procedure_len = sys_le16_to_cpu(evt->max_procedure_len);
1264
1265 notify_cs_procedure_enable_available(conn, ¶ms);
1266
1267 bt_conn_unref(conn);
1268 }
1269
1270 #if defined(CONFIG_BT_CHANNEL_SOUNDING_TEST)
bt_le_cs_stop_test(void)1271 int bt_le_cs_stop_test(void)
1272 {
1273 struct net_buf *buf;
1274
1275 buf = bt_hci_cmd_create(BT_HCI_OP_LE_CS_TEST_END, 0);
1276 if (!buf) {
1277 return -ENOBUFS;
1278 }
1279
1280 return bt_hci_cmd_send_sync(BT_HCI_OP_LE_CS_TEST_END, buf, NULL);
1281 }
1282
bt_hci_le_cs_test_end_complete(struct net_buf * buf)1283 void bt_hci_le_cs_test_end_complete(struct net_buf *buf)
1284 {
1285 struct bt_hci_evt_le_cs_test_end_complete *evt;
1286
1287 if (buf->len < sizeof(*evt)) {
1288 LOG_ERR("Unexpected end of buffer");
1289 return;
1290 }
1291
1292 evt = net_buf_pull_mem(buf, sizeof(*evt));
1293 if (evt->status) {
1294 LOG_WRN("CS Test End failed with status 0x%02X", evt->status);
1295 return;
1296 }
1297
1298 struct net_buf *reassembly_buf = get_reassembly_buf(BT_HCI_LE_CS_TEST_CONN_HANDLE, false);
1299
1300 if (reassembly_buf) {
1301 LOG_WRN("De-allocating a dangling reassembly buffer");
1302 free_reassembly_buf(&reassembly_buf);
1303 }
1304
1305 if (cs_test_callbacks.le_cs_test_end_complete) {
1306 cs_test_callbacks.le_cs_test_end_complete();
1307 }
1308 }
1309 #endif /* CONFIG_BT_CHANNEL_SOUNDING_TEST */
1310
bt_le_cs_step_data_parse(struct net_buf_simple * step_data_buf,bool (* func)(struct bt_le_cs_subevent_step * step,void * user_data),void * user_data)1311 void bt_le_cs_step_data_parse(struct net_buf_simple *step_data_buf,
1312 bool (*func)(struct bt_le_cs_subevent_step *step, void *user_data),
1313 void *user_data)
1314 {
1315 if (!step_data_buf) {
1316 LOG_INF("Tried to parse empty step data.");
1317 return;
1318 }
1319
1320 while (step_data_buf->len > 1) {
1321 struct bt_le_cs_subevent_step step;
1322
1323 step.mode = net_buf_simple_pull_u8(step_data_buf);
1324 step.channel = net_buf_simple_pull_u8(step_data_buf);
1325 step.data_len = net_buf_simple_pull_u8(step_data_buf);
1326
1327 if (step.data_len == 0) {
1328 LOG_WRN("Encountered zero-length step data.");
1329 return;
1330 }
1331
1332 step.data = step_data_buf->data;
1333
1334 if (step.data_len > step_data_buf->len) {
1335 LOG_WRN("Step data appears malformed.");
1336 return;
1337 }
1338
1339 if (!func(&step, user_data)) {
1340 return;
1341 }
1342
1343 net_buf_simple_pull(step_data_buf, step.data_len);
1344 }
1345 }
1346
1347 #endif /* CONFIG_BT_CHANNEL_SOUNDING */
1348
