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