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/console/console.h>
9 #include <zephyr/sys/byteorder.h>
10 #include <zephyr/bluetooth/bluetooth.h>
11 #include <zephyr/bluetooth/cs.h>
12 #include <zephyr/bluetooth/att.h>
13 #include <zephyr/bluetooth/gatt.h>
14 #include "distance_estimation.h"
15 #include "common.h"
16 #include "cs_test_params.h"
17
18 static K_SEM_DEFINE(sem_results_available, 0, 1);
19 static K_SEM_DEFINE(sem_test_complete, 0, 1);
20 static K_SEM_DEFINE(sem_connected, 0, 1);
21 static K_SEM_DEFINE(sem_disconnected, 0, 1);
22 static K_SEM_DEFINE(sem_data_received, 0, 1);
23
24 static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
25 const void *buf, uint16_t len, uint16_t offset, uint8_t flags);
26 static struct bt_conn *connection;
27 static uint8_t n_ap;
28 static uint8_t latest_num_steps_reported;
29 static uint16_t latest_step_data_len;
30 static uint8_t latest_local_steps[STEP_DATA_BUF_LEN];
31 static uint8_t latest_peer_steps[STEP_DATA_BUF_LEN];
32
33 static struct bt_gatt_attr gatt_attributes[] = {
34 BT_GATT_PRIMARY_SERVICE(&step_data_svc_uuid),
35 BT_GATT_CHARACTERISTIC(&step_data_char_uuid.uuid, BT_GATT_CHRC_WRITE,
36 BT_GATT_PERM_WRITE | BT_GATT_PERM_READ, NULL, on_attr_write_cb,
37 NULL),
38 };
39 static struct bt_gatt_service step_data_gatt_service = BT_GATT_SERVICE(gatt_attributes);
40 static const char sample_str[] = "CS Test Sample";
41
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)42 static ssize_t on_attr_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
43 const void *buf, uint16_t len, uint16_t offset, uint8_t flags)
44 {
45 if (flags & BT_GATT_WRITE_FLAG_PREPARE) {
46 return 0;
47 }
48
49 if (offset) {
50 return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
51 }
52
53 if (len != sizeof(latest_local_steps)) {
54 return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
55 }
56
57 if (flags & BT_GATT_WRITE_FLAG_EXECUTE) {
58 uint8_t *data = (uint8_t *)buf;
59
60 memcpy(latest_peer_steps, &data[offset], len);
61 k_sem_give(&sem_data_received);
62 }
63
64 return len;
65 }
66
subevent_result_cb(struct bt_conn_le_cs_subevent_result * result)67 static void subevent_result_cb(struct bt_conn_le_cs_subevent_result *result)
68 {
69 latest_num_steps_reported = result->header.num_steps_reported;
70 n_ap = result->header.num_antenna_paths;
71
72 if (result->step_data_buf) {
73 if (result->step_data_buf->len <= STEP_DATA_BUF_LEN) {
74 memcpy(latest_local_steps, result->step_data_buf->data,
75 result->step_data_buf->len);
76 latest_step_data_len = result->step_data_buf->len;
77 } else {
78 printk("Not enough memory to store step data. (%d > %d)\n",
79 result->step_data_buf->len, STEP_DATA_BUF_LEN);
80 latest_num_steps_reported = 0;
81 }
82 }
83
84 if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE ||
85 result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) {
86 k_sem_give(&sem_results_available);
87 }
88 }
89
end_cb(void)90 static void end_cb(void)
91 {
92 k_sem_give(&sem_test_complete);
93 }
94
mtu_exchange_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_exchange_params * params)95 static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
96 struct bt_gatt_exchange_params *params)
97 {
98 printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn));
99 }
100
connected_cb(struct bt_conn * conn,uint8_t err)101 static void connected_cb(struct bt_conn *conn, uint8_t err)
102 {
103 char addr[BT_ADDR_LE_STR_LEN];
104
105 (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
106 printk("Connected to %s (err 0x%02X)\n", addr, err);
107
108 __ASSERT(connection == conn, "Unexpected connected callback");
109
110 if (err) {
111 bt_conn_unref(conn);
112 connection = NULL;
113 }
114
115 static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb};
116
117 err = bt_gatt_exchange_mtu(connection, &mtu_exchange_params);
118 if (err) {
119 printk("%s: MTU exchange failed (err %d)\n", __func__, err);
120 }
121
122 k_sem_give(&sem_connected);
123 }
124
disconnected_cb(struct bt_conn * conn,uint8_t reason)125 static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
126 {
127 printk("Disconnected (reason 0x%02X)\n", reason);
128
129 bt_conn_unref(conn);
130 connection = NULL;
131
132 k_sem_give(&sem_disconnected);
133 }
134
data_cb(struct bt_data * data,void * user_data)135 static bool data_cb(struct bt_data *data, void *user_data)
136 {
137 char *name = user_data;
138 uint8_t len;
139
140 switch (data->type) {
141 case BT_DATA_NAME_SHORTENED:
142 case BT_DATA_NAME_COMPLETE:
143 len = MIN(data->data_len, NAME_LEN - 1);
144 memcpy(name, data->data, len);
145 name[len] = '\0';
146 return false;
147 default:
148 return true;
149 }
150 }
151
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad)152 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
153 struct net_buf_simple *ad)
154 {
155 char addr_str[BT_ADDR_LE_STR_LEN];
156 char name[NAME_LEN] = {};
157 int err;
158
159 if (connection) {
160 return;
161 }
162
163 /* We're only interested in connectable events */
164 if (type != BT_GAP_ADV_TYPE_ADV_IND && type != BT_GAP_ADV_TYPE_ADV_DIRECT_IND) {
165 return;
166 }
167
168 bt_data_parse(ad, data_cb, name);
169
170 if (strcmp(name, sample_str)) {
171 return;
172 }
173
174 if (bt_le_scan_stop()) {
175 return;
176 }
177
178 printk("Found device with name %s, connecting...\n", name);
179
180 err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
181 &connection);
182 if (err) {
183 printk("Create conn to %s failed (%u)\n", addr_str, err);
184 }
185 }
186
187 BT_CONN_CB_DEFINE(conn_cb) = {
188 .connected = connected_cb,
189 .disconnected = disconnected_cb,
190 };
191
main(void)192 int main(void)
193 {
194 int err;
195 struct bt_le_cs_test_param test_params;
196
197 printk("Starting Channel Sounding Demo\n");
198
199 /* Initialize the Bluetooth Subsystem */
200 err = bt_enable(NULL);
201 if (err) {
202 printk("Bluetooth init failed (err %d)\n", err);
203 return 0;
204 }
205
206 struct bt_le_cs_test_cb cs_test_cb = {
207 .le_cs_test_subevent_data_available = subevent_result_cb,
208 .le_cs_test_end_complete = end_cb,
209 };
210
211 err = bt_le_cs_test_cb_register(cs_test_cb);
212 if (err) {
213 printk("Failed to register callbacks (err %d)\n", err);
214 return 0;
215 }
216
217 err = bt_gatt_service_register(&step_data_gatt_service);
218 if (err) {
219 printk("bt_gatt_service_register() returned err %d\n", err);
220 return 0;
221 }
222
223 while (true) {
224 while (true) {
225 k_sleep(K_SECONDS(2));
226
227 test_params = test_params_get(BT_CONN_LE_CS_ROLE_INITIATOR);
228
229 err = bt_le_cs_start_test(&test_params);
230 if (err) {
231 printk("Failed to start CS test (err %d)\n", err);
232 return 0;
233 }
234
235 k_sem_take(&sem_results_available, K_SECONDS(5));
236
237 err = bt_le_cs_stop_test();
238 if (err) {
239 printk("Failed to stop CS test (err %d)\n", err);
240 return 0;
241 }
242
243 k_sem_take(&sem_test_complete, K_FOREVER);
244
245 if (latest_num_steps_reported > NUM_MODE_0_STEPS) {
246 break;
247 }
248 }
249
250 err = bt_le_scan_start(BT_LE_SCAN_ACTIVE_CONTINUOUS, device_found);
251 if (err) {
252 printk("Scanning failed to start (err %d)\n", err);
253 return 0;
254 }
255
256 k_sem_take(&sem_connected, K_FOREVER);
257
258 k_sem_take(&sem_data_received, K_FOREVER);
259
260 estimate_distance(
261 latest_local_steps, latest_step_data_len, latest_peer_steps,
262 latest_step_data_len -
263 NUM_MODE_0_STEPS *
264 (sizeof(struct bt_hci_le_cs_step_data_mode_0_initiator) -
265 sizeof(struct bt_hci_le_cs_step_data_mode_0_reflector)),
266 n_ap, BT_CONN_LE_CS_ROLE_INITIATOR);
267
268 bt_conn_disconnect(connection, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
269
270 k_sem_take(&sem_disconnected, K_FOREVER);
271
272 printk("Re-running CS test...\n");
273 }
274
275 return 0;
276 }
277
