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,struct bt_conn_le_cs_capabilities * params)92 static void remote_capabilities_cb(struct bt_conn *conn, struct bt_conn_le_cs_capabilities *params)
93 {
94 ARG_UNUSED(params);
95 printk("CS capability exchange completed.\n");
96 k_sem_give(&sem_remote_capabilities_obtained);
97 }
98
config_created_cb(struct bt_conn * conn,struct bt_conn_le_cs_config * config)99 static void config_created_cb(struct bt_conn *conn, struct bt_conn_le_cs_config *config)
100 {
101 printk("CS config creation complete. ID: %d\n", config->id);
102 k_sem_give(&sem_config_created);
103 }
104
security_enabled_cb(struct bt_conn * conn)105 static void security_enabled_cb(struct bt_conn *conn)
106 {
107 printk("CS security enabled.\n");
108 k_sem_give(&sem_cs_security_enabled);
109 }
110
procedure_enabled_cb(struct bt_conn * conn,struct bt_conn_le_cs_procedure_enable_complete * params)111 static void procedure_enabled_cb(struct bt_conn *conn,
112 struct bt_conn_le_cs_procedure_enable_complete *params)
113 {
114 if (params->state == 1) {
115 printk("CS procedures enabled.\n");
116 } else {
117 printk("CS procedures disabled.\n");
118 }
119 }
120
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * params)121 static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
122 struct bt_gatt_discover_params *params)
123 {
124 struct bt_gatt_chrc *chrc;
125 char str[BT_UUID_STR_LEN];
126
127 printk("Discovery: attr %p\n", attr);
128
129 if (!attr) {
130 return BT_GATT_ITER_STOP;
131 }
132
133 chrc = (struct bt_gatt_chrc *)attr->user_data;
134
135 bt_uuid_to_str(chrc->uuid, str, sizeof(str));
136 printk("UUID %s\n", str);
137
138 if (!bt_uuid_cmp(chrc->uuid, &step_data_char_uuid.uuid)) {
139 step_data_attr_handle = chrc->value_handle;
140
141 printk("Found expected UUID\n");
142
143 k_sem_give(&sem_discovered);
144 }
145
146 return BT_GATT_ITER_STOP;
147 }
148
write_func(struct bt_conn * conn,uint8_t err,struct bt_gatt_write_params * params)149 static void write_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params)
150 {
151 if (err) {
152 printk("Write failed (err %d)\n", err);
153
154 return;
155 }
156
157 k_sem_give(&sem_written);
158 }
159
160 BT_CONN_CB_DEFINE(conn_cb) = {
161 .connected = connected_cb,
162 .disconnected = disconnected_cb,
163 .le_cs_remote_capabilities_available = remote_capabilities_cb,
164 .le_cs_config_created = config_created_cb,
165 .le_cs_security_enabled = security_enabled_cb,
166 .le_cs_procedure_enabled = procedure_enabled_cb,
167 .le_cs_subevent_data_available = subevent_result_cb,
168 };
169
main(void)170 int main(void)
171 {
172 int err;
173 struct bt_gatt_discover_params discover_params;
174 struct bt_gatt_write_params write_params;
175
176 printk("Starting Channel Sounding Demo\n");
177
178 /* Initialize the Bluetooth Subsystem */
179 err = bt_enable(NULL);
180 if (err) {
181 printk("Bluetooth init failed (err %d)\n", err);
182 return 0;
183 }
184
185 err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONN, BT_GAP_ADV_FAST_INT_MIN_1,
186 BT_GAP_ADV_FAST_INT_MAX_1, NULL),
187 ad, ARRAY_SIZE(ad), NULL, 0);
188 if (err) {
189 printk("Advertising failed to start (err %d)\n", err);
190 return 0;
191 }
192
193 k_sem_take(&sem_connected, K_FOREVER);
194
195 const struct bt_le_cs_set_default_settings_param default_settings = {
196 .enable_initiator_role = false,
197 .enable_reflector_role = true,
198 .cs_sync_antenna_selection = BT_LE_CS_ANTENNA_SELECTION_OPT_REPETITIVE,
199 .max_tx_power = BT_HCI_OP_LE_CS_MAX_MAX_TX_POWER,
200 };
201
202 err = bt_le_cs_set_default_settings(connection, &default_settings);
203 if (err) {
204 printk("Failed to configure default CS settings (err %d)\n", err);
205 }
206
207 discover_params.uuid = &step_data_char_uuid.uuid;
208 discover_params.func = discover_func;
209 discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
210 discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
211 discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
212
213 err = bt_gatt_discover(connection, &discover_params);
214 if (err) {
215 printk("Discovery failed (err %d)\n", err);
216 return 0;
217 }
218
219 err = k_sem_take(&sem_discovered, K_SECONDS(10));
220 if (err) {
221 printk("Timed out during GATT discovery\n");
222 return 0;
223 }
224
225 while (true) {
226 k_sem_take(&sem_procedure_done, K_FOREVER);
227
228 write_params.func = write_func;
229 write_params.handle = step_data_attr_handle;
230 write_params.length = STEP_DATA_BUF_LEN;
231 write_params.data = &latest_local_steps[0];
232 write_params.offset = 0;
233
234 err = bt_gatt_write(connection, &write_params);
235 if (err) {
236 printk("Write failed (err %d)\n", err);
237 return 0;
238 }
239
240 err = k_sem_take(&sem_written, K_SECONDS(10));
241 if (err) {
242 printk("Timed out during GATT write\n");
243 return 0;
244 }
245 }
246
247 return 0;
248 }
249
