1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <math.h>
8 #include <zephyr/sys/byteorder.h>
9 #include <zephyr/bluetooth/bluetooth.h>
10 #include <zephyr/bluetooth/cs.h>
11 #include <zephyr/bluetooth/att.h>
12 #include <zephyr/bluetooth/gatt.h>
13 #include "distance_estimation.h"
14 #include "common.h"
15
16 #define CS_CONFIG_ID 0
17 #define NUM_MODE_0_STEPS 1
18
19 static K_SEM_DEFINE(sem_acl_encryption_enabled, 0, 1);
20 static K_SEM_DEFINE(sem_remote_capabilities_obtained, 0, 1);
21 static K_SEM_DEFINE(sem_config_created, 0, 1);
22 static K_SEM_DEFINE(sem_cs_security_enabled, 0, 1);
23 static K_SEM_DEFINE(sem_procedure_done, 0, 1);
24 static K_SEM_DEFINE(sem_connected, 0, 1);
25 static K_SEM_DEFINE(sem_data_received, 0, 1);
26
27 static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
28 const void *buf, uint16_t len, uint16_t offset, uint8_t flags);
29 static struct bt_conn *connection;
30 static uint8_t n_ap;
31 static uint8_t latest_num_steps_reported;
32 static uint16_t latest_step_data_len;
33 static uint8_t latest_local_steps[STEP_DATA_BUF_LEN];
34 static uint8_t latest_peer_steps[STEP_DATA_BUF_LEN];
35
36 static struct bt_gatt_attr gatt_attributes[] = {
37 BT_GATT_PRIMARY_SERVICE(&step_data_svc_uuid),
38 BT_GATT_CHARACTERISTIC(&step_data_char_uuid.uuid, BT_GATT_CHRC_WRITE,
39 BT_GATT_PERM_WRITE | BT_GATT_PERM_PREPARE_WRITE, NULL,
40 on_attr_write_cb, NULL),
41 };
42 static struct bt_gatt_service step_data_gatt_service = BT_GATT_SERVICE(gatt_attributes);
43 static const char sample_str[] = "CS Sample";
44
on_attr_write_cb(struct bt_conn * conn,const struct bt_gatt_attr * attr,const void * buf,uint16_t len,uint16_t offset,uint8_t flags)45 static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
46 const void *buf, uint16_t len, uint16_t offset, uint8_t flags)
47 {
48 if (flags & BT_GATT_WRITE_FLAG_PREPARE) {
49 return 0;
50 }
51
52 if (offset) {
53 return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
54 }
55
56 if (len != sizeof(latest_local_steps)) {
57 return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
58 }
59
60 if (flags & BT_GATT_WRITE_FLAG_EXECUTE) {
61 uint8_t *data = (uint8_t *)buf;
62
63 memcpy(latest_peer_steps, &data[offset], len);
64 k_sem_give(&sem_data_received);
65 }
66
67 return len;
68 }
69
subevent_result_cb(struct bt_conn * conn,struct bt_conn_le_cs_subevent_result * result)70 static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result)
71 {
72 latest_num_steps_reported = result->header.num_steps_reported;
73 n_ap = result->header.num_antenna_paths;
74
75 if (result->step_data_buf) {
76 if (result->step_data_buf->len <= STEP_DATA_BUF_LEN) {
77 memcpy(latest_local_steps, result->step_data_buf->data,
78 result->step_data_buf->len);
79 latest_step_data_len = result->step_data_buf->len;
80 } else {
81 printk("Not enough memory to store step data. (%d > %d)\n",
82 result->step_data_buf->len, STEP_DATA_BUF_LEN);
83 latest_num_steps_reported = 0;
84 }
85 }
86
87 if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) {
88 k_sem_give(&sem_procedure_done);
89 }
90 }
91
mtu_exchange_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_exchange_params * params)92 static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
93 struct bt_gatt_exchange_params *params)
94 {
95 printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn));
96 }
97
connected_cb(struct bt_conn * conn,uint8_t err)98 static void connected_cb(struct bt_conn *conn, uint8_t err)
99 {
100 char addr[BT_ADDR_LE_STR_LEN];
101
102 (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
103 printk("Connected to %s (err 0x%02X)\n", addr, err);
104
105 __ASSERT(connection == conn, "Unexpected connected callback");
106
107 if (err) {
108 bt_conn_unref(conn);
109 connection = NULL;
110 }
111
112 static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb};
113
114 err = bt_gatt_exchange_mtu(connection, &mtu_exchange_params);
115 if (err) {
116 printk("%s: MTU exchange failed (err %d)\n", __func__, err);
117 }
118
119 k_sem_give(&sem_connected);
120 }
121
disconnected_cb(struct bt_conn * conn,uint8_t reason)122 static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
123 {
124 printk("Disconnected (reason 0x%02X)\n", reason);
125
126 bt_conn_unref(conn);
127 connection = NULL;
128 }
129
security_changed_cb(struct bt_conn * conn,bt_security_t level,enum bt_security_err err)130 static void security_changed_cb(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
131 {
132 if (err) {
133 printk("Encryption failed. (err %d)\n", err);
134 } else {
135 printk("Security changed to level %d.\n", level);
136 }
137
138 k_sem_give(&sem_acl_encryption_enabled);
139 }
140
remote_capabilities_cb(struct bt_conn * conn,struct bt_conn_le_cs_capabilities * params)141 static void remote_capabilities_cb(struct bt_conn *conn, struct bt_conn_le_cs_capabilities *params)
142 {
143 ARG_UNUSED(params);
144 printk("CS capability exchange completed.\n");
145 k_sem_give(&sem_remote_capabilities_obtained);
146 }
147
config_created_cb(struct bt_conn * conn,struct bt_conn_le_cs_config * config)148 static void config_created_cb(struct bt_conn *conn, struct bt_conn_le_cs_config *config)
149 {
150 printk("CS config creation complete. ID: %d\n", config->id);
151 k_sem_give(&sem_config_created);
152 }
153
security_enabled_cb(struct bt_conn * conn)154 static void security_enabled_cb(struct bt_conn *conn)
155 {
156 printk("CS security enabled.\n");
157 k_sem_give(&sem_cs_security_enabled);
158 }
159
procedure_enabled_cb(struct bt_conn * conn,struct bt_conn_le_cs_procedure_enable_complete * params)160 static void procedure_enabled_cb(struct bt_conn *conn,
161 struct bt_conn_le_cs_procedure_enable_complete *params)
162 {
163 if (params->state == 1) {
164 printk("CS procedures enabled.\n");
165 } else {
166 printk("CS procedures disabled.\n");
167 }
168 }
169
data_cb(struct bt_data * data,void * user_data)170 static bool data_cb(struct bt_data *data, void *user_data)
171 {
172 char *name = user_data;
173 uint8_t len;
174
175 switch (data->type) {
176 case BT_DATA_NAME_SHORTENED:
177 case BT_DATA_NAME_COMPLETE:
178 len = MIN(data->data_len, NAME_LEN - 1);
179 memcpy(name, data->data, len);
180 name[len] = '\0';
181 return false;
182 default:
183 return true;
184 }
185 }
186
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad)187 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
188 struct net_buf_simple *ad)
189 {
190 char addr_str[BT_ADDR_LE_STR_LEN];
191 char name[NAME_LEN] = {};
192 int err;
193
194 if (connection) {
195 return;
196 }
197
198 /* We're only interested in connectable events */
199 if (type != BT_GAP_ADV_TYPE_ADV_IND && type != BT_GAP_ADV_TYPE_ADV_DIRECT_IND) {
200 return;
201 }
202
203 bt_data_parse(ad, data_cb, name);
204
205 if (strcmp(name, sample_str)) {
206 return;
207 }
208
209 if (bt_le_scan_stop()) {
210 return;
211 }
212
213 printk("Found device with name %s, connecting...\n", name);
214
215 err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
216 &connection);
217 if (err) {
218 printk("Create conn to %s failed (%u)\n", addr_str, err);
219 }
220 }
221
222 BT_CONN_CB_DEFINE(conn_cb) = {
223 .connected = connected_cb,
224 .disconnected = disconnected_cb,
225 .security_changed = security_changed_cb,
226 .le_cs_remote_capabilities_available = remote_capabilities_cb,
227 .le_cs_config_created = config_created_cb,
228 .le_cs_security_enabled = security_enabled_cb,
229 .le_cs_procedure_enabled = procedure_enabled_cb,
230 .le_cs_subevent_data_available = subevent_result_cb,
231 };
232
main(void)233 int main(void)
234 {
235 int err;
236
237 printk("Starting Channel Sounding Demo\n");
238
239 /* Initialize the Bluetooth Subsystem */
240 err = bt_enable(NULL);
241 if (err) {
242 printk("Bluetooth init failed (err %d)\n", err);
243 return 0;
244 }
245
246 err = bt_gatt_service_register(&step_data_gatt_service);
247 if (err) {
248 printk("bt_gatt_service_register() returned err %d\n", err);
249 return 0;
250 }
251
252 err = bt_le_scan_start(BT_LE_SCAN_ACTIVE_CONTINUOUS, device_found);
253 if (err) {
254 printk("Scanning failed to start (err %d)\n", err);
255 return 0;
256 }
257
258 k_sem_take(&sem_connected, K_FOREVER);
259
260 const struct bt_le_cs_set_default_settings_param default_settings = {
261 .enable_initiator_role = true,
262 .enable_reflector_role = false,
263 .cs_sync_antenna_selection = BT_LE_CS_ANTENNA_SELECTION_OPT_REPETITIVE,
264 .max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER,
265 };
266
267 err = bt_le_cs_set_default_settings(connection, &default_settings);
268 if (err) {
269 printk("Failed to configure default CS settings (err %d)\n", err);
270 }
271
272 err = bt_conn_set_security(connection, BT_SECURITY_L2);
273 if (err) {
274 printk("Failed to encrypt connection (err %d)\n", err);
275 return 0;
276 }
277
278 k_sem_take(&sem_acl_encryption_enabled, K_FOREVER);
279
280 err = bt_le_cs_read_remote_supported_capabilities(connection);
281 if (err) {
282 printk("Failed to exchange CS capabilities (err %d)\n", err);
283 return 0;
284 }
285
286 k_sem_take(&sem_remote_capabilities_obtained, K_FOREVER);
287
288 struct bt_le_cs_create_config_params config_params = {
289 .id = CS_CONFIG_ID,
290 .main_mode_type = BT_CONN_LE_CS_MAIN_MODE_2,
291 .sub_mode_type = BT_CONN_LE_CS_SUB_MODE_1,
292 .min_main_mode_steps = 2,
293 .max_main_mode_steps = 10,
294 .main_mode_repetition = 0,
295 .mode_0_steps = NUM_MODE_0_STEPS,
296 .role = BT_CONN_LE_CS_ROLE_INITIATOR,
297 .rtt_type = BT_CONN_LE_CS_RTT_TYPE_AA_ONLY,
298 .cs_sync_phy = BT_CONN_LE_CS_SYNC_1M_PHY,
299 .channel_map_repetition = 1,
300 .channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B,
301 .ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT,
302 .ch3c_jump = 2,
303 };
304
305 bt_le_cs_set_valid_chmap_bits(config_params.channel_map);
306
307 err = bt_le_cs_create_config(connection, &config_params,
308 BT_LE_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE);
309 if (err) {
310 printk("Failed to create CS config (err %d)\n", err);
311 return 0;
312 }
313
314 k_sem_take(&sem_config_created, K_FOREVER);
315
316 err = bt_le_cs_security_enable(connection);
317 if (err) {
318 printk("Failed to start CS Security (err %d)\n", err);
319 return 0;
320 }
321
322 k_sem_take(&sem_cs_security_enabled, K_FOREVER);
323
324 const struct bt_le_cs_set_procedure_parameters_param procedure_params = {
325 .config_id = CS_CONFIG_ID,
326 .max_procedure_len = 12,
327 .min_procedure_interval = 100,
328 .max_procedure_interval = 100,
329 .max_procedure_count = 0,
330 .min_subevent_len = 6750,
331 .max_subevent_len = 6750,
332 .tone_antenna_config_selection = BT_LE_CS_TONE_ANTENNA_CONFIGURATION_INDEX_ONE,
333 .phy = BT_LE_CS_PROCEDURE_PHY_1M,
334 .tx_power_delta = 0x80,
335 .preferred_peer_antenna = BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_1,
336 .snr_control_initiator = BT_LE_CS_SNR_CONTROL_NOT_USED,
337 .snr_control_reflector = BT_LE_CS_SNR_CONTROL_NOT_USED,
338 };
339
340 err = bt_le_cs_set_procedure_parameters(connection, &procedure_params);
341 if (err) {
342 printk("Failed to set procedure parameters (err %d)\n", err);
343 return 0;
344 }
345
346 struct bt_le_cs_procedure_enable_param params = {
347 .config_id = CS_CONFIG_ID,
348 .enable = 1,
349 };
350
351 err = bt_le_cs_procedure_enable(connection, ¶ms);
352 if (err) {
353 printk("Failed to enable CS procedures (err %d)\n", err);
354 return 0;
355 }
356
357 while (true) {
358 k_sem_take(&sem_procedure_done, K_FOREVER);
359 k_sem_take(&sem_data_received, K_FOREVER);
360
361 estimate_distance(
362 latest_local_steps, latest_step_data_len, latest_peer_steps,
363 latest_step_data_len -
364 NUM_MODE_0_STEPS *
365 (sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator) -
366 sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector)),
367 n_ap, BT_CONN_LE_CS_ROLE_INITIATOR);
368 }
369
370 return 0;
371 }
372
