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,uint8_t status,struct bt_conn_le_cs_capabilities * params)141 static void remote_capabilities_cb(struct bt_conn *conn,
142 				   uint8_t status,
143 				   struct bt_conn_le_cs_capabilities *params)
144 {
145 	ARG_UNUSED(params);
146 
147 	if (status == BT_HCI_ERR_SUCCESS) {
148 		printk("CS capability exchange completed.\n");
149 		k_sem_give(&sem_remote_capabilities_obtained);
150 	} else {
151 		printk("CS capability exchange failed. (HCI status 0x%02x)\n", status);
152 	}
153 }
154 
config_create_cb(struct bt_conn * conn,uint8_t status,struct bt_conn_le_cs_config * config)155 static void config_create_cb(struct bt_conn *conn,
156 			     uint8_t status,
157 			     struct bt_conn_le_cs_config *config)
158 {
159 	if (status == BT_HCI_ERR_SUCCESS) {
160 		printk("CS config creation complete. ID: %d\n", config->id);
161 		k_sem_give(&sem_config_created);
162 	} else {
163 		printk("CS config creation failed. (HCI status 0x%02x)\n", status);
164 	}
165 }
166 
security_enable_cb(struct bt_conn * conn,uint8_t status)167 static void security_enable_cb(struct bt_conn *conn, uint8_t status)
168 {
169 	if (status == BT_HCI_ERR_SUCCESS) {
170 		printk("CS security enabled.\n");
171 		k_sem_give(&sem_cs_security_enabled);
172 	} else {
173 		printk("CS security enable failed. (HCI status 0x%02x)\n", status);
174 	}
175 }
176 
procedure_enable_cb(struct bt_conn * conn,uint8_t status,struct bt_conn_le_cs_procedure_enable_complete * params)177 static void procedure_enable_cb(struct bt_conn *conn,
178 				 uint8_t status,
179 				 struct bt_conn_le_cs_procedure_enable_complete *params)
180 {
181 	if (status == BT_HCI_ERR_SUCCESS) {
182 		if (params->state == 1) {
183 			printk("CS procedures enabled.\n");
184 		} else {
185 			printk("CS procedures disabled.\n");
186 		}
187 	} else {
188 		printk("CS procedures enable failed. (HCI status 0x%02x)\n", status);
189 	}
190 }
191 
data_cb(struct bt_data * data,void * user_data)192 static bool data_cb(struct bt_data *data, void *user_data)
193 {
194 	char *name = user_data;
195 	uint8_t len;
196 
197 	switch (data->type) {
198 	case BT_DATA_NAME_SHORTENED:
199 	case BT_DATA_NAME_COMPLETE:
200 		len = MIN(data->data_len, NAME_LEN - 1);
201 		memcpy(name, data->data, len);
202 		name[len] = '\0';
203 		return false;
204 	default:
205 		return true;
206 	}
207 }
208 
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad)209 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
210 			 struct net_buf_simple *ad)
211 {
212 	char addr_str[BT_ADDR_LE_STR_LEN];
213 	char name[NAME_LEN] = {};
214 	int err;
215 
216 	if (connection) {
217 		return;
218 	}
219 
220 	/* We're only interested in connectable events */
221 	if (type != BT_GAP_ADV_TYPE_ADV_IND && type != BT_GAP_ADV_TYPE_ADV_DIRECT_IND) {
222 		return;
223 	}
224 
225 	bt_data_parse(ad, data_cb, name);
226 
227 	if (strcmp(name, sample_str)) {
228 		return;
229 	}
230 
231 	if (bt_le_scan_stop()) {
232 		return;
233 	}
234 
235 	printk("Found device with name %s, connecting...\n", name);
236 
237 	err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
238 				&connection);
239 	if (err) {
240 		printk("Create conn to %s failed (%u)\n", addr_str, err);
241 	}
242 }
243 
244 BT_CONN_CB_DEFINE(conn_cb) = {
245 	.connected = connected_cb,
246 	.disconnected = disconnected_cb,
247 	.security_changed = security_changed_cb,
248 	.le_cs_read_remote_capabilities_complete = remote_capabilities_cb,
249 	.le_cs_config_complete = config_create_cb,
250 	.le_cs_security_enable_complete = security_enable_cb,
251 	.le_cs_procedure_enable_complete = procedure_enable_cb,
252 	.le_cs_subevent_data_available = subevent_result_cb,
253 };
254 
main(void)255 int main(void)
256 {
257 	int err;
258 
259 	printk("Starting Channel Sounding Demo\n");
260 
261 	/* Initialize the Bluetooth Subsystem */
262 	err = bt_enable(NULL);
263 	if (err) {
264 		printk("Bluetooth init failed (err %d)\n", err);
265 		return 0;
266 	}
267 
268 	err = bt_gatt_service_register(&step_data_gatt_service);
269 	if (err) {
270 		printk("bt_gatt_service_register() returned err %d\n", err);
271 		return 0;
272 	}
273 
274 	err = bt_le_scan_start(BT_LE_SCAN_ACTIVE_CONTINUOUS, device_found);
275 	if (err) {
276 		printk("Scanning failed to start (err %d)\n", err);
277 		return 0;
278 	}
279 
280 	k_sem_take(&sem_connected, K_FOREVER);
281 
282 	const struct bt_le_cs_set_default_settings_param default_settings = {
283 		.enable_initiator_role = true,
284 		.enable_reflector_role = false,
285 		.cs_sync_antenna_selection = BT_LE_CS_ANTENNA_SELECTION_OPT_REPETITIVE,
286 		.max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER,
287 	};
288 
289 	err = bt_le_cs_set_default_settings(connection, &default_settings);
290 	if (err) {
291 		printk("Failed to configure default CS settings (err %d)\n", err);
292 	}
293 
294 	err = bt_conn_set_security(connection, BT_SECURITY_L2);
295 	if (err) {
296 		printk("Failed to encrypt connection (err %d)\n", err);
297 		return 0;
298 	}
299 
300 	k_sem_take(&sem_acl_encryption_enabled, K_FOREVER);
301 
302 	err = bt_le_cs_read_remote_supported_capabilities(connection);
303 	if (err) {
304 		printk("Failed to exchange CS capabilities (err %d)\n", err);
305 		return 0;
306 	}
307 
308 	k_sem_take(&sem_remote_capabilities_obtained, K_FOREVER);
309 
310 	struct bt_le_cs_create_config_params config_params = {
311 		.id = CS_CONFIG_ID,
312 		.mode = BT_CONN_LE_CS_MAIN_MODE_2_SUB_MODE_1,
313 		.min_main_mode_steps = 2,
314 		.max_main_mode_steps = 10,
315 		.main_mode_repetition = 0,
316 		.mode_0_steps = NUM_MODE_0_STEPS,
317 		.role = BT_CONN_LE_CS_ROLE_INITIATOR,
318 		.rtt_type = BT_CONN_LE_CS_RTT_TYPE_AA_ONLY,
319 		.cs_sync_phy = BT_CONN_LE_CS_SYNC_1M_PHY,
320 		.channel_map_repetition = 1,
321 		.channel_selection_type = BT_CONN_LE_CS_CHSEL_TYPE_3B,
322 		.ch3c_shape = BT_CONN_LE_CS_CH3C_SHAPE_HAT,
323 		.ch3c_jump = 2,
324 	};
325 
326 	bt_le_cs_set_valid_chmap_bits(config_params.channel_map);
327 
328 	err = bt_le_cs_create_config(connection, &config_params,
329 				     BT_LE_CS_CREATE_CONFIG_CONTEXT_LOCAL_AND_REMOTE);
330 	if (err) {
331 		printk("Failed to create CS config (err %d)\n", err);
332 		return 0;
333 	}
334 
335 	k_sem_take(&sem_config_created, K_FOREVER);
336 
337 	err = bt_le_cs_security_enable(connection);
338 	if (err) {
339 		printk("Failed to start CS Security (err %d)\n", err);
340 		return 0;
341 	}
342 
343 	k_sem_take(&sem_cs_security_enabled, K_FOREVER);
344 
345 	const struct bt_le_cs_set_procedure_parameters_param procedure_params = {
346 		.config_id = CS_CONFIG_ID,
347 		.max_procedure_len = 12,
348 		.min_procedure_interval = 100,
349 		.max_procedure_interval = 100,
350 		.max_procedure_count = 0,
351 		.min_subevent_len = 6750,
352 		.max_subevent_len = 6750,
353 		.tone_antenna_config_selection = BT_LE_CS_TONE_ANTENNA_CONFIGURATION_A1_B1,
354 		.phy = BT_LE_CS_PROCEDURE_PHY_1M,
355 		.tx_power_delta = 0x80,
356 		.preferred_peer_antenna = BT_LE_CS_PROCEDURE_PREFERRED_PEER_ANTENNA_1,
357 		.snr_control_initiator = BT_LE_CS_SNR_CONTROL_NOT_USED,
358 		.snr_control_reflector = BT_LE_CS_SNR_CONTROL_NOT_USED,
359 	};
360 
361 	err = bt_le_cs_set_procedure_parameters(connection, &procedure_params);
362 	if (err) {
363 		printk("Failed to set procedure parameters (err %d)\n", err);
364 		return 0;
365 	}
366 
367 	struct bt_le_cs_procedure_enable_param params = {
368 		.config_id = CS_CONFIG_ID,
369 		.enable = 1,
370 	};
371 
372 	err = bt_le_cs_procedure_enable(connection, &params);
373 	if (err) {
374 		printk("Failed to enable CS procedures (err %d)\n", err);
375 		return 0;
376 	}
377 
378 	while (true) {
379 		k_sem_take(&sem_procedure_done, K_FOREVER);
380 		k_sem_take(&sem_data_received, K_FOREVER);
381 
382 		estimate_distance(
383 			latest_local_steps, latest_step_data_len, latest_peer_steps,
384 			latest_step_data_len -
385 				NUM_MODE_0_STEPS *
386 					(sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator) -
387 					 sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector)),
388 			n_ap, BT_CONN_LE_CS_ROLE_INITIATOR);
389 	}
390 
391 	return 0;
392 }
393