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_discovered, 0, 1);
23 static K_SEM_DEFINE(sem_written, 0, 1);
24
25 static uint16_t step_data_attr_handle;
26 static struct bt_conn *connection;
27 static uint8_t latest_num_steps_reported;
28 static uint8_t latest_local_steps[STEP_DATA_BUF_LEN];
29
30 static const char sample_str[] = "CS Test Sample";
31 static const struct bt_data ad[] = {
32 BT_DATA(BT_DATA_NAME_COMPLETE, "CS Test Sample", sizeof(sample_str) - 1),
33 };
34
subevent_result_cb(struct bt_conn_le_cs_subevent_result * result)35 static void subevent_result_cb(struct bt_conn_le_cs_subevent_result *result)
36 {
37 latest_num_steps_reported = result->header.num_steps_reported;
38
39 if (result->step_data_buf) {
40 if (result->step_data_buf->len <= STEP_DATA_BUF_LEN) {
41 memcpy(latest_local_steps, result->step_data_buf->data,
42 result->step_data_buf->len);
43 } else {
44 printk("Not enough memory to store step data. (%d > %d)\n",
45 result->step_data_buf->len, STEP_DATA_BUF_LEN);
46 latest_num_steps_reported = 0;
47 }
48 }
49
50 if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE ||
51 result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_ABORTED) {
52 k_sem_give(&sem_results_available);
53 }
54 }
55
end_cb(void)56 static void end_cb(void)
57 {
58 k_sem_give(&sem_test_complete);
59 }
60
mtu_exchange_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_exchange_params * params)61 static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
62 struct bt_gatt_exchange_params *params)
63 {
64 printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn));
65 }
66
connected_cb(struct bt_conn * conn,uint8_t err)67 static void connected_cb(struct bt_conn *conn, uint8_t err)
68 {
69 char addr[BT_ADDR_LE_STR_LEN];
70
71 (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
72 printk("Connected to %s (err 0x%02X)\n", addr, err);
73
74 __ASSERT(connection == conn, "Unexpected connected callback");
75
76 if (err) {
77 bt_conn_unref(conn);
78 connection = NULL;
79 }
80
81 connection = bt_conn_ref(conn);
82
83 static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb};
84
85 err = bt_gatt_exchange_mtu(connection, &mtu_exchange_params);
86 if (err) {
87 printk("%s: MTU exchange failed (err %d)\n", __func__, err);
88 }
89
90 k_sem_give(&sem_connected);
91 }
92
disconnected_cb(struct bt_conn * conn,uint8_t reason)93 static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
94 {
95 printk("Disconnected (reason 0x%02X)\n", reason);
96
97 bt_conn_unref(conn);
98 connection = NULL;
99
100 k_sem_give(&sem_disconnected);
101 }
102
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * params)103 static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
104 struct bt_gatt_discover_params *params)
105 {
106 struct bt_gatt_chrc *chrc;
107 char str[BT_UUID_STR_LEN];
108
109 printk("Discovery: attr %p\n", attr);
110
111 if (!attr) {
112 return BT_GATT_ITER_STOP;
113 }
114
115 chrc = (struct bt_gatt_chrc *)attr->user_data;
116
117 bt_uuid_to_str(chrc->uuid, str, sizeof(str));
118 printk("UUID %s\n", str);
119
120 if (!bt_uuid_cmp(chrc->uuid, &step_data_char_uuid.uuid)) {
121 step_data_attr_handle = chrc->value_handle;
122
123 printk("Found expected UUID\n");
124
125 k_sem_give(&sem_discovered);
126 }
127
128 return BT_GATT_ITER_STOP;
129 }
130
write_func(struct bt_conn * conn,uint8_t err,struct bt_gatt_write_params * params)131 static void write_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params)
132 {
133 if (err) {
134 printk("Write failed (err %d)\n", err);
135
136 return;
137 }
138
139 k_sem_give(&sem_written);
140 }
141
142 BT_CONN_CB_DEFINE(conn_cb) = {
143 .connected = connected_cb,
144 .disconnected = disconnected_cb,
145 };
146
main(void)147 int main(void)
148 {
149 int err;
150 struct bt_le_cs_test_param test_params;
151 struct bt_gatt_discover_params discover_params;
152 struct bt_gatt_write_params write_params;
153
154 printk("Starting Channel Sounding Demo\n");
155
156 /* Initialize the Bluetooth Subsystem */
157 err = bt_enable(NULL);
158 if (err) {
159 printk("Bluetooth init failed (err %d)\n", err);
160 return 0;
161 }
162
163 struct bt_le_cs_test_cb cs_test_cb = {
164 .le_cs_test_subevent_data_available = subevent_result_cb,
165 .le_cs_test_end_complete = end_cb,
166 };
167
168 err = bt_le_cs_test_cb_register(cs_test_cb);
169 if (err) {
170 printk("Failed to register callbacks (err %d)\n", err);
171 return 0;
172 }
173
174 while (true) {
175 while (true) {
176 k_sleep(K_SECONDS(2));
177
178 test_params = test_params_get(BT_CONN_LE_CS_ROLE_REFLECTOR);
179
180 err = bt_le_cs_start_test(&test_params);
181 if (err) {
182 printk("Failed to start CS test (err %d)\n", err);
183 return 0;
184 }
185
186 k_sem_take(&sem_results_available, K_SECONDS(5));
187
188 err = bt_le_cs_stop_test();
189 if (err) {
190 printk("Failed to stop CS test (err %d)\n", err);
191 return 0;
192 }
193
194 k_sem_take(&sem_test_complete, K_FOREVER);
195
196 if (latest_num_steps_reported > NUM_MODE_0_STEPS) {
197 break;
198 }
199 }
200
201 err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1,
202 BT_GAP_ADV_FAST_INT_MAX_1, NULL),
203 ad, ARRAY_SIZE(ad), NULL, 0);
204 if (err) {
205 printk("Advertising failed to start (err %d)\n", err);
206 return 0;
207 }
208
209 k_sem_take(&sem_connected, K_FOREVER);
210
211 discover_params.uuid = &step_data_char_uuid.uuid;
212 discover_params.func = discover_func;
213 discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
214 discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
215 discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
216
217 err = bt_gatt_discover(connection, &discover_params);
218 if (err) {
219 printk("Discovery failed (err %d)\n", err);
220 return 0;
221 }
222
223 err = k_sem_take(&sem_discovered, K_SECONDS(10));
224 if (err) {
225 printk("Timed out during GATT discovery\n");
226 return 0;
227 }
228
229 write_params.func = write_func;
230 write_params.handle = step_data_attr_handle;
231 write_params.length = STEP_DATA_BUF_LEN;
232 write_params.data = latest_local_steps;
233 write_params.offset = 0;
234
235 err = bt_gatt_write(connection, &write_params);
236 if (err) {
237 printk("Write failed (err %d)\n", err);
238 return 0;
239 }
240
241 k_sem_take(&sem_disconnected, K_FOREVER);
242
243 printk("Re-running CS test...\n");
244 }
245
246 return 0;
247 }
248
