1 /*
2 * Copyright (c) 2023 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "utils.h"
8 #include "argparse.h"
9 #include "bs_pc_backchannel.h"
10
11 #include <zephyr/kernel.h>
12 #include <zephyr/bluetooth/bluetooth.h>
13 #include <zephyr/bluetooth/gatt.h>
14 #include <zephyr/sys/__assert.h>
15
16 /* Custom Service Variables */
17 static const struct bt_uuid_128 test_svc_uuid = BT_UUID_INIT_128(
18 0xf0, 0xde, 0xbc, 0x9a, 0x78, 0x56, 0x34, 0x12,
19 0x78, 0x56, 0x34, 0x12, 0x78, 0x56, 0x34, 0x12);
20
21 static const struct bt_uuid_128 test_svc_uuid_2 = BT_UUID_INIT_128(
22 0xf1, 0xdd, 0xbc, 0x9a, 0x78, 0x56, 0x34, 0x12,
23 0x78, 0x56, 0x34, 0x12, 0x78, 0x56, 0x34, 0x12);
24
25 static const struct bt_uuid_128 test_chrc_uuid = BT_UUID_INIT_128(
26 0xf2, 0xde, 0xbc, 0x9a, 0x78, 0x56, 0x34, 0x12,
27 0x78, 0x56, 0x34, 0x12, 0x78, 0x56, 0x34, 0x12);
28
29 static uint8_t test_value[] = { 'T', 'e', 's', 't', '\0' };
30
31 DEFINE_FLAG(flag_client_read);
32
read_test(struct bt_conn * conn,const struct bt_gatt_attr * attr,void * buf,uint16_t len,uint16_t offset)33 static ssize_t read_test(struct bt_conn *conn, const struct bt_gatt_attr *attr,
34 void *buf, uint16_t len, uint16_t offset)
35 {
36 const char *value = attr->user_data;
37
38 printk("Client has read from test char\n");
39 SET_FLAG(flag_client_read);
40
41 return bt_gatt_attr_read(conn, attr, buf, len, offset, value,
42 strlen(value));
43 }
44
wait_for_client_read(void)45 void wait_for_client_read(void)
46 {
47 WAIT_FOR_FLAG(flag_client_read);
48 }
49
write_test(struct bt_conn * conn,const struct bt_gatt_attr * attr,const void * buf,uint16_t len,uint16_t offset,uint8_t flags)50 static ssize_t write_test(struct bt_conn *conn, const struct bt_gatt_attr *attr,
51 const void *buf, uint16_t len, uint16_t offset,
52 uint8_t flags)
53 {
54 uint8_t *value = attr->user_data;
55
56 printk("Client has written to test char\n");
57
58 if (offset + len > sizeof(test_value)) {
59 return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
60 }
61
62 memcpy(value + offset, buf, len);
63
64 return len;
65 }
66
67 static struct bt_gatt_attr test_attrs[] = {
68 /* Vendor Primary Service Declaration */
69 BT_GATT_PRIMARY_SERVICE(&test_svc_uuid),
70
71 BT_GATT_CHARACTERISTIC(&test_chrc_uuid.uuid,
72 BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
73 BT_GATT_PERM_READ |
74 BT_GATT_PERM_WRITE,
75 read_test, write_test, test_value),
76 };
77
78 static struct bt_gatt_attr test_attrs_2[] = {
79 /* Vendor Primary Service Declaration */
80 BT_GATT_PRIMARY_SERVICE(&test_svc_uuid_2),
81
82 BT_GATT_CHARACTERISTIC(&test_chrc_uuid.uuid,
83 BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
84 BT_GATT_PERM_READ_ENCRYPT |
85 BT_GATT_PERM_WRITE_ENCRYPT,
86 read_test, write_test, test_value),
87 };
88
89 static struct bt_gatt_service test_svc = BT_GATT_SERVICE(test_attrs);
90 static struct bt_gatt_service test_svc_2 = BT_GATT_SERVICE(test_attrs_2);
91
gatt_register_service_1(void)92 void gatt_register_service_1(void)
93 {
94 int err = bt_gatt_service_register(&test_svc);
95
96 __ASSERT(!err, "Failed to register GATT service (err %d)\n", err);
97 }
98
gatt_register_service_2(void)99 void gatt_register_service_2(void)
100 {
101 /* This service is only used to trigger a GATT DB change.
102 * No reads or writes will be attempted.
103 */
104 int err = bt_gatt_service_register(&test_svc_2);
105
106 __ASSERT(!err, "Failed to register GATT service (err %d)\n", err);
107 }
108
109 /* We need to discover:
110 * - Dynamic service
111 * - Client Features (to set robust caching)
112 * - Service Changed (to sub to indications)
113 */
114 enum GATT_HANDLES {
115 CLIENT_FEATURES,
116 SERVICE_CHANGED,
117 TEST_CHAR,
118 NUM_HANDLES,
119 };
120
121 uint16_t gatt_handles[NUM_HANDLES] = {0};
122
123 DEFINE_FLAG(flag_discovered);
124
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * params)125 static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
126 struct bt_gatt_discover_params *params)
127 {
128 if (attr == NULL) {
129 for (int i = 0; i < ARRAY_SIZE(gatt_handles); i++) {
130 printk("handle[%d] = 0x%x\n", i, gatt_handles[i]);
131
132 if (gatt_handles[i] == 0) {
133 FAIL("Did not discover all characteristics\n");
134 }
135 }
136
137 (void)memset(params, 0, sizeof(*params));
138
139 SET_FLAG(flag_discovered);
140
141 return BT_GATT_ITER_STOP;
142 }
143
144 if (params->type == BT_GATT_DISCOVER_CHARACTERISTIC) {
145 const struct bt_gatt_chrc *chrc = (struct bt_gatt_chrc *)attr->user_data;
146
147 if (bt_uuid_cmp(chrc->uuid, BT_UUID_GATT_CLIENT_FEATURES) == 0) {
148 printk("Found client supported features\n");
149 gatt_handles[CLIENT_FEATURES] = chrc->value_handle;
150
151 } else if (bt_uuid_cmp(chrc->uuid, BT_UUID_GATT_SC) == 0) {
152 printk("Found service changed\n");
153 gatt_handles[SERVICE_CHANGED] = chrc->value_handle;
154
155 } else if (bt_uuid_cmp(chrc->uuid, &test_chrc_uuid.uuid) == 0) {
156 printk("Found test characteristic\n");
157 gatt_handles[TEST_CHAR] = chrc->value_handle;
158 }
159 }
160
161 return BT_GATT_ITER_CONTINUE;
162 }
163
164 DEFINE_FLAG(flag_sc_indicated);
sc_indicated(struct bt_conn * conn,struct bt_gatt_subscribe_params * params,const void * data,uint16_t length)165 static uint8_t sc_indicated(struct bt_conn *conn,
166 struct bt_gatt_subscribe_params *params,
167 const void *data, uint16_t length)
168 {
169 if (!data) {
170 params->value_handle = 0U;
171 return BT_GATT_ITER_STOP;
172 }
173
174 printk("SC received\n");
175 SET_FLAG(flag_sc_indicated);
176
177 return BT_GATT_ITER_CONTINUE;
178 }
179
wait_for_sc_indication(void)180 void wait_for_sc_indication(void)
181 {
182 WAIT_FOR_FLAG(flag_sc_indicated);
183 }
184
185 DEFINE_FLAG(flag_sc_subscribed);
sc_subscribed(struct bt_conn * conn,uint8_t err,struct bt_gatt_subscribe_params * params)186 static void sc_subscribed(struct bt_conn *conn, uint8_t err,
187 struct bt_gatt_subscribe_params *params)
188 {
189 if (params->value) {
190 printk("SC subscribed\n");
191 SET_FLAG(flag_sc_subscribed);
192 } else {
193 printk("SC unsubscribed\n");
194 UNSET_FLAG(flag_sc_subscribed);
195 }
196 }
197
198 static struct bt_gatt_discover_params disc_params;
199 static struct bt_gatt_subscribe_params subscribe_params;
gatt_subscribe_to_service_changed(bool subscribe)200 void gatt_subscribe_to_service_changed(bool subscribe)
201 {
202 int err;
203
204 subscribe_params.value_handle = gatt_handles[SERVICE_CHANGED];
205 subscribe_params.notify = sc_indicated;
206 subscribe_params.subscribe = sc_subscribed;
207
208 if (subscribe) {
209 subscribe_params.ccc_handle = BT_GATT_AUTO_DISCOVER_CCC_HANDLE;
210 subscribe_params.disc_params = &disc_params,
211 subscribe_params.value = BT_GATT_CCC_INDICATE;
212 subscribe_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
213
214 err = bt_gatt_subscribe(get_conn(), &subscribe_params);
215 WAIT_FOR_FLAG(flag_sc_subscribed);
216 } else {
217 /* Params are already set to the correct values by the previous
218 * call of this fn.
219 */
220 err = bt_gatt_unsubscribe(get_conn(), &subscribe_params);
221 WAIT_FOR_FLAG_UNSET(flag_sc_subscribed);
222 }
223
224 if (err != 0) {
225 FAIL("Subscription failed(err %d)\n", err);
226 } else {
227 printk("%subscribed %s SC indications\n",
228 subscribe ? "S" : "Uns",
229 subscribe ? "to" : "from");
230 }
231 }
232
gatt_discover(void)233 void gatt_discover(void)
234 {
235 static struct bt_gatt_discover_params discover_params;
236 int err;
237
238 printk("Discovering services and characteristics\n");
239 UNSET_FLAG(flag_discovered);
240
241 discover_params.uuid = NULL;
242 discover_params.func = discover_func;
243 discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
244 discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
245 discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
246
247 err = bt_gatt_discover(get_conn(), &discover_params);
248 if (err != 0) {
249 FAIL("Discover failed(err %d)\n", err);
250 }
251
252 WAIT_FOR_FLAG(flag_discovered);
253 printk("Discover complete\n");
254 }
255
256 DEFINE_FLAG(flag_written);
257
write_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_write_params * params)258 static void write_cb(struct bt_conn *conn, uint8_t err, struct bt_gatt_write_params *params)
259 {
260 if (err != BT_ATT_ERR_SUCCESS) {
261 FAIL("Write failed: 0x%02X\n", err);
262 }
263
264 SET_FLAG(flag_written);
265 }
266
267 #define CF_BIT_ROBUST_CACHING 0
activate_robust_caching(void)268 void activate_robust_caching(void)
269 {
270 int err;
271
272 static const uint8_t csf = BIT(CF_BIT_ROBUST_CACHING);
273 static struct bt_gatt_write_params write_params = {
274 .func = write_cb,
275 .offset = 0,
276 .data = &csf,
277 .length = sizeof(csf),
278 };
279
280 write_params.handle = gatt_handles[CLIENT_FEATURES];
281
282 UNSET_FLAG(flag_written);
283 err = bt_gatt_write(get_conn(), &write_params);
284
285 __ASSERT(!err, "Failed to enable robust caching\n");
286
287 WAIT_FOR_FLAG(flag_written);
288 printk("Robust caching enabled\n");
289 }
290
291 DEFINE_FLAG(flag_read);
292
_expect_success(struct bt_conn * conn,uint8_t err,struct bt_gatt_read_params * params,const void * data,uint16_t length)293 static uint8_t _expect_success(struct bt_conn *conn, uint8_t err,
294 struct bt_gatt_read_params *params,
295 const void *data, uint16_t length)
296 {
297 /* printk("GATT read cb: err 0x%02X\n", err); */
298 __ASSERT(err == 0, "Failed to read: err 0x%x\n", err);
299
300 SET_FLAG(flag_read);
301
302 return 0;
303 }
304
_expect_out_of_sync_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_read_params * params,const void * data,uint16_t length)305 static uint8_t _expect_out_of_sync_cb(struct bt_conn *conn, uint8_t err,
306 struct bt_gatt_read_params *params,
307 const void *data, uint16_t length)
308 {
309 /* printk("GATT read cb: err 0x%02X\n", err); */
310 __ASSERT(err == BT_ATT_ERR_DB_OUT_OF_SYNC,
311 "Didn't get expected error code: err 0x%x\n", err);
312
313 SET_FLAG(flag_read);
314
315 return 0;
316 }
317
read_char(uint16_t handle,bool expect_success)318 static void read_char(uint16_t handle, bool expect_success)
319 {
320 int err;
321
322 struct bt_gatt_read_params read_params = {
323 .handle_count = 1,
324 .single = {
325 .handle = handle,
326 .offset = 0,
327 },
328 };
329
330 if (expect_success) {
331 read_params.func = _expect_success;
332 } else {
333 read_params.func = _expect_out_of_sync_cb;
334 }
335
336 UNSET_FLAG(flag_read);
337
338 err = bt_gatt_read(get_conn(), &read_params);
339 __ASSERT(!err, "Failed to read char\n");
340
341 WAIT_FOR_FLAG(flag_read);
342 }
343
read_test_char(bool expect_success)344 void read_test_char(bool expect_success)
345 {
346 read_char(gatt_handles[TEST_CHAR], expect_success);
347 }
348
gatt_clear_flags(void)349 void gatt_clear_flags(void)
350 {
351 UNSET_FLAG(flag_client_read);
352 UNSET_FLAG(flag_discovered);
353 UNSET_FLAG(flag_sc_indicated);
354 UNSET_FLAG(flag_sc_subscribed);
355 UNSET_FLAG(flag_written);
356 UNSET_FLAG(flag_read);
357 }
358
