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 "common.h"
14
15 #define CS_CONFIG_ID 0
16 #define NUM_MODE_0_STEPS 1
17
18 static K_SEM_DEFINE(sem_remote_capabilities_obtained, 0, 1);
19 static K_SEM_DEFINE(sem_config_created, 0, 1);
20 static K_SEM_DEFINE(sem_cs_security_enabled, 0, 1);
21 static K_SEM_DEFINE(sem_procedure_done, 0, 1);
22 static K_SEM_DEFINE(sem_connected, 0, 1);
23 static K_SEM_DEFINE(sem_discovered, 0, 1);
24 static K_SEM_DEFINE(sem_written, 0, 1);
25
26 static uint16_t step_data_attr_handle;
27 static struct bt_conn *connection;
28 static uint8_t latest_local_steps[STEP_DATA_BUF_LEN];
29
30 static const char sample_str[] = "CS Sample";
31 static const struct bt_data ad[] = {
32 BT_DATA(BT_DATA_NAME_COMPLETE, "CS Sample", sizeof(sample_str) - 1),
33 };
34
subevent_result_cb(struct bt_conn * conn,struct bt_conn_le_cs_subevent_result * result)35 static void subevent_result_cb(struct bt_conn *conn, struct bt_conn_le_cs_subevent_result *result)
36 {
37 if (result->step_data_buf) {
38 if (result->step_data_buf->len <= STEP_DATA_BUF_LEN) {
39 memcpy(latest_local_steps, result->step_data_buf->data,
40 result->step_data_buf->len);
41 } else {
42 printk("Not enough memory to store step data. (%d > %d)\n",
43 result->step_data_buf->len, STEP_DATA_BUF_LEN);
44 }
45 }
46
47 if (result->header.procedure_done_status == BT_CONN_LE_CS_PROCEDURE_COMPLETE) {
48 k_sem_give(&sem_procedure_done);
49 }
50 }
51
mtu_exchange_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_exchange_params * params)52 static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
53 struct bt_gatt_exchange_params *params)
54 {
55 printk("MTU exchange %s (%u)\n", err == 0U ? "success" : "failed", bt_gatt_get_mtu(conn));
56 }
57
connected_cb(struct bt_conn * conn,uint8_t err)58 static void connected_cb(struct bt_conn *conn, uint8_t err)
59 {
60 char addr[BT_ADDR_LE_STR_LEN];
61
62 (void)bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
63 printk("Connected to %s (err 0x%02X)\n", addr, err);
64
65 __ASSERT(connection == conn, "Unexpected connected callback");
66
67 if (err) {
68 bt_conn_unref(conn);
69 connection = NULL;
70 }
71
72 connection = bt_conn_ref(conn);
73
74 static struct bt_gatt_exchange_params mtu_exchange_params = {.func = mtu_exchange_cb};
75
76 err = bt_gatt_exchange_mtu(connection, &mtu_exchange_params);
77 if (err) {
78 printk("%s: MTU exchange failed (err %d)\n", __func__, err);
79 }
80
81 k_sem_give(&sem_connected);
82 }
83
disconnected_cb(struct bt_conn * conn,uint8_t reason)84 static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
85 {
86 printk("Disconnected (reason 0x%02X)\n", reason);
87
88 bt_conn_unref(conn);
89 connection = NULL;
90 }
91
remote_capabilities_cb(struct bt_conn * conn,uint8_t status,struct bt_conn_le_cs_capabilities * params)92 static void remote_capabilities_cb(struct bt_conn *conn,
93 uint8_t status,
94 struct bt_conn_le_cs_capabilities *params)
95 {
96 ARG_UNUSED(params);
97
98 if (status == BT_HCI_ERR_SUCCESS) {
99 printk("CS capability exchange completed.\n");
100 k_sem_give(&sem_remote_capabilities_obtained);
101 } else {
102 printk("CS capability exchange failed. (HCI status 0x%02x)\n", status);
103 }
104 }
105
config_create_cb(struct bt_conn * conn,uint8_t status,struct bt_conn_le_cs_config * config)106 static void config_create_cb(struct bt_conn *conn,
107 uint8_t status,
108 struct bt_conn_le_cs_config *config)
109 {
110 if (status == BT_HCI_ERR_SUCCESS) {
111 printk("CS config creation complete. ID: %d\n", config->id);
112 k_sem_give(&sem_config_created);
113 } else {
114 printk("CS config creation failed. (HCI status 0x%02x)\n", status);
115 }
116 }
117
security_enable_cb(struct bt_conn * conn,uint8_t status)118 static void security_enable_cb(struct bt_conn *conn, uint8_t status)
119 {
120 if (status == BT_HCI_ERR_SUCCESS) {
121 printk("CS security enabled.\n");
122 k_sem_give(&sem_cs_security_enabled);
123 } else {
124 printk("CS security enable failed. (HCI status 0x%02x)\n", status);
125 }
126 }
127
procedure_enable_cb(struct bt_conn * conn,uint8_t status,struct bt_conn_le_cs_procedure_enable_complete * params)128 static void procedure_enable_cb(struct bt_conn *conn,
129 uint8_t status,
130 struct bt_conn_le_cs_procedure_enable_complete *params)
131 {
132 if (status == BT_HCI_ERR_SUCCESS) {
133 if (params->state == 1) {
134 printk("CS procedures enabled.\n");
135 } else {
136 printk("CS procedures disabled.\n");
137 }
138 } else {
139 printk("CS procedures enable failed. (HCI status 0x%02x)\n", status);
140 }
141 }
142
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * params)143 static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
144 struct bt_gatt_discover_params *params)
145 {
146 struct bt_gatt_chrc *chrc;
147 char str[BT_UUID_STR_LEN];
148
149 printk("Discovery: attr %p\n", attr);
150
151 if (!attr) {
152 return BT_GATT_ITER_STOP;
153 }
154
155 chrc = (struct bt_gatt_chrc *)attr->user_data;
156
157 bt_uuid_to_str(chrc->uuid, str, sizeof(str));
158 printk("UUID %s\n", str);
159
160 if (!bt_uuid_cmp(chrc->uuid, &step_data_char_uuid.uuid)) {
161 step_data_attr_handle = chrc->value_handle;
162
163 printk("Found expected UUID\n");
164
165 k_sem_give(&sem_discovered);
166 }
167
168 return BT_GATT_ITER_STOP;
169 }
170
write_func(struct bt_conn * conn,uint8_t err,struct bt_gatt_write_params * params)171 static void write_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params)
172 {
173 if (err) {
174 printk("Write failed (err %d)\n", err);
175
176 return;
177 }
178
179 k_sem_give(&sem_written);
180 }
181
182 BT_CONN_CB_DEFINE(conn_cb) = {
183 .connected = connected_cb,
184 .disconnected = disconnected_cb,
185 .le_cs_read_remote_capabilities_complete = remote_capabilities_cb,
186 .le_cs_config_complete = config_create_cb,
187 .le_cs_security_enable_complete = security_enable_cb,
188 .le_cs_procedure_enable_complete = procedure_enable_cb,
189 .le_cs_subevent_data_available = subevent_result_cb,
190 };
191
main(void)192 int main(void)
193 {
194 int err;
195 struct bt_gatt_discover_params discover_params;
196 struct bt_gatt_write_params write_params;
197
198 printk("Starting Channel Sounding Demo\n");
199
200 /* Initialize the Bluetooth Subsystem */
201 err = bt_enable(NULL);
202 if (err) {
203 printk("Bluetooth init failed (err %d)\n", err);
204 return 0;
205 }
206
207 err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1,
208 BT_GAP_ADV_FAST_INT_MAX_1, NULL),
209 ad, ARRAY_SIZE(ad), NULL, 0);
210 if (err) {
211 printk("Advertising failed to start (err %d)\n", err);
212 return 0;
213 }
214
215 k_sem_take(&sem_connected, K_FOREVER);
216
217 const struct bt_le_cs_set_default_settings_param default_settings = {
218 .enable_initiator_role = false,
219 .enable_reflector_role = true,
220 .cs_sync_antenna_selection = BT_LE_CS_ANTENNA_SELECTION_OPT_REPETITIVE,
221 .max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER,
222 };
223
224 err = bt_le_cs_set_default_settings(connection, &default_settings);
225 if (err) {
226 printk("Failed to configure default CS settings (err %d)\n", err);
227 }
228
229 discover_params.uuid = &step_data_char_uuid.uuid;
230 discover_params.func = discover_func;
231 discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
232 discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
233 discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
234
235 err = bt_gatt_discover(connection, &discover_params);
236 if (err) {
237 printk("Discovery failed (err %d)\n", err);
238 return 0;
239 }
240
241 err = k_sem_take(&sem_discovered, K_SECONDS(10));
242 if (err) {
243 printk("Timed out during GATT discovery\n");
244 return 0;
245 }
246
247 while (true) {
248 k_sem_take(&sem_procedure_done, K_FOREVER);
249
250 write_params.func = write_func;
251 write_params.handle = step_data_attr_handle;
252 write_params.length = STEP_DATA_BUF_LEN;
253 write_params.data = &latest_local_steps[0];
254 write_params.offset = 0;
255
256 err = bt_gatt_write(connection, &write_params);
257 if (err) {
258 printk("Write failed (err %d)\n", err);
259 return 0;
260 }
261
262 err = k_sem_take(&sem_written, K_SECONDS(10));
263 if (err) {
264 printk("Timed out during GATT write\n");
265 return 0;
266 }
267 }
268
269 return 0;
270 }
271