1 /*
2 * Copyright (c) 2023 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <errno.h>
8 #include <stddef.h>
9 #include <stdio.h>
10 #include <stdlib.h>
11 #include <string.h>
12
13 #include <zephyr/bluetooth/bluetooth.h>
14 #include <zephyr/bluetooth/conn.h>
15 #include <zephyr/bluetooth/gatt.h>
16 #include <zephyr/bluetooth/hci.h>
17 #include <zephyr/bluetooth/uuid.h>
18 #include <zephyr/kernel.h>
19 #include <zephyr/sys/byteorder.h>
20 #include <zephyr/sys/check.h>
21 #include <zephyr/sys/printk.h>
22 #include <zephyr/types.h>
23
24 #include <zephyr/logging/log.h>
25 LOG_MODULE_REGISTER(central, LOG_LEVEL_INF);
26
27 #include "bstests.h"
28 #include "bs_types.h"
29 #include "bs_tracing.h"
30 #include "bstests.h"
31 #include "bs_pc_backchannel.h"
32
33 #define DEFAULT_CONN_INTERVAL 20
34 #define PERIPHERAL_DEVICE_NAME "Zephyr Peripheral"
35 #define PERIPHERAL_DEVICE_NAME_LEN (sizeof(PERIPHERAL_DEVICE_NAME) - 1)
36
37 #define NOTIFICATION_DATA_PREFIX "Counter:"
38 #define NOTIFICATION_DATA_PREFIX_LEN (sizeof(NOTIFICATION_DATA_PREFIX) - 1)
39
40 #define CHARACTERISTIC_DATA_MAX_LEN 260
41 #define NOTIFICATION_DATA_LEN MAX(200, (CONFIG_BT_L2CAP_TX_MTU - 4))
42 BUILD_ASSERT(NOTIFICATION_DATA_LEN <= CHARACTERISTIC_DATA_MAX_LEN);
43
44 #define PERIPHERAL_SERVICE_UUID_VAL \
45 BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdef0)
46
47 #define PERIPHERAL_CHARACTERISTIC_UUID_VAL \
48 BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdef1)
49
50 #define PERIPHERAL_SERVICE_UUID BT_UUID_DECLARE_128(PERIPHERAL_SERVICE_UUID_VAL)
51 #define PERIPHERAL_CHARACTERISTIC_UUID BT_UUID_DECLARE_128(PERIPHERAL_CHARACTERISTIC_UUID_VAL)
52
53 static struct bt_uuid_128 vnd_uuid =
54 BT_UUID_INIT_128(BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdea0));
55
56 static struct bt_uuid_128 vnd_enc_uuid =
57 BT_UUID_INIT_128(BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdea1));
58
59 enum {
60 DEVICE_IS_SCANNING,
61 DEVICE_IS_CONNECTING,
62
63 /* Total number of flags - must be at the end of the enum */
64 DEVICE_NUM_FLAGS,
65 };
66
67 enum {
68 CONN_INFO_SENT_MTU_EXCHANGE,
69 CONN_INFO_MTU_EXCHANGED,
70 CONN_INFO_DISCOVERING,
71 CONN_INFO_DISCOVER_PAUSED,
72 CONN_INFO_SUBSCRIBED,
73
74 /* Total number of flags - must be at the end of the enum */
75 CONN_INFO_NUM_FLAGS,
76 };
77
78 ATOMIC_DEFINE(status_flags, DEVICE_NUM_FLAGS);
79 static atomic_t conn_count;
80 static struct bt_conn *conn_connecting;
81 static struct bt_gatt_exchange_params mtu_exchange_params;
82
83 struct conn_info {
84 ATOMIC_DEFINE(flags, CONN_INFO_NUM_FLAGS);
85 struct bt_conn *conn_ref;
86 uint32_t notify_counter;
87 uint32_t tx_notify_counter;
88 struct bt_uuid_128 uuid;
89 struct bt_gatt_discover_params discover_params;
90 struct bt_gatt_subscribe_params subscribe_params;
91 bt_addr_le_t addr;
92 };
93
94 static struct conn_info conn_infos[CONFIG_BT_MAX_CONN] = {0};
95
96 static uint32_t conn_interval_max, notification_size;
97 static uint8_t vnd_value[CHARACTERISTIC_DATA_MAX_LEN];
98
99 static const struct bt_uuid_16 ccc_uuid = BT_UUID_INIT_16(BT_UUID_GATT_CCC_VAL);
100
clear_info(struct conn_info * info)101 void clear_info(struct conn_info *info)
102 {
103 /* clear everything except the address + sub params + uuid (lifetime > connection) */
104 memset(&info->flags, 0, sizeof(info->flags));
105 memset(&info->conn_ref, 0, sizeof(info->conn_ref));
106 memset(&info->notify_counter, 0, sizeof(info->notify_counter));
107 memset(&info->tx_notify_counter, 0, sizeof(info->tx_notify_counter));
108 }
109
ccc_cfg_changed(const struct bt_gatt_attr * attr,uint16_t value)110 static void ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
111 {
112 /* TODO: add peer subscription check? */
113 LOG_INF("CCC changed: %u", value);
114 }
115
116 /* Vendor Primary Service Declaration */
117 BT_GATT_SERVICE_DEFINE(
118 vnd_svc, BT_GATT_PRIMARY_SERVICE(&vnd_uuid),
119 BT_GATT_CHARACTERISTIC(&vnd_enc_uuid.uuid,
120 BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE | BT_GATT_CHRC_NOTIFY,
121 BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, NULL, NULL,
122 NULL),
123 BT_GATT_CCC(ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
124 );
125
get_new_conn_info_ref(const bt_addr_le_t * addr)126 static struct conn_info *get_new_conn_info_ref(const bt_addr_le_t *addr)
127 {
128 /* try to find per-addr first */
129 for (size_t i = 0; i < ARRAY_SIZE(conn_infos); i++) {
130 if (bt_addr_le_eq(&conn_infos[i].addr, addr)) {
131 return &conn_infos[i];
132 }
133 }
134
135 /* try to allocate if addr not found */
136 for (size_t i = 0; i < ARRAY_SIZE(conn_infos); i++) {
137 if (conn_infos[i].conn_ref == NULL) {
138 bt_addr_le_copy(&conn_infos[i].addr, addr);
139
140 return &conn_infos[i];
141 }
142 }
143
144 __ASSERT(0, "ran out of contexts");
145 }
146
get_conn_info_ref(struct bt_conn * conn_ref)147 static struct conn_info *get_conn_info_ref(struct bt_conn *conn_ref)
148 {
149 for (size_t i = 0; i < ARRAY_SIZE(conn_infos); i++) {
150 if (conn_ref == conn_infos[i].conn_ref) {
151 return &conn_infos[i];
152 }
153 }
154
155 return NULL;
156 }
157
is_connected(struct bt_conn * conn)158 static bool is_connected(struct bt_conn *conn)
159 {
160 struct bt_conn_info info;
161 int err = bt_conn_get_info(conn, &info);
162
163 __ASSERT(!err, "Couldn't get conn info %d", err);
164
165 return info.state == BT_CONN_STATE_CONNECTED;
166 }
167
get_connected_conn_info_ref(struct bt_conn * conn)168 static struct conn_info *get_connected_conn_info_ref(struct bt_conn *conn)
169 {
170 if (is_connected(conn)) {
171 return get_conn_info_ref(conn);
172 }
173
174 return NULL;
175 }
176
notify_func(struct bt_conn * conn,struct bt_gatt_subscribe_params * params,const void * data,uint16_t length)177 static uint8_t notify_func(struct bt_conn *conn, struct bt_gatt_subscribe_params *params,
178 const void *data, uint16_t length)
179 {
180 const char *data_ptr = (const char *)data + NOTIFICATION_DATA_PREFIX_LEN;
181 uint32_t received_counter;
182 struct conn_info *conn_info_ref;
183 char addr[BT_ADDR_LE_STR_LEN];
184
185 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
186
187 if (!data) {
188 LOG_INF("[UNSUBSCRIBED] addr %s", addr);
189 params->value_handle = 0U;
190 return BT_GATT_ITER_STOP;
191 }
192
193 conn_info_ref = get_conn_info_ref(conn);
194 __ASSERT_NO_MSG(conn_info_ref);
195
196 received_counter = strtoul(data_ptr, NULL, 0);
197
198 LOG_DBG("[NOTIFICATION] addr %s data %s length %u cnt %u",
199 addr, data, length, received_counter);
200
201 LOG_HEXDUMP_DBG(data, length, "RX");
202
203 __ASSERT(conn_info_ref->notify_counter == received_counter,
204 "addr %s expected counter : %u , received counter : %u",
205 addr, conn_info_ref->notify_counter, received_counter);
206 conn_info_ref->notify_counter++;
207
208 return BT_GATT_ITER_CONTINUE;
209 }
210
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * params)211 static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
212 struct bt_gatt_discover_params *params)
213 {
214 int err;
215 char uuid_str[BT_UUID_STR_LEN];
216 struct conn_info *conn_info_ref;
217
218 if (!attr) {
219 /* We might be called from the ATT disconnection callback if we
220 * have an ongoing procedure. That is ok.
221 */
222 __ASSERT_NO_MSG(!is_connected(conn));
223 return BT_GATT_ITER_STOP;
224 }
225 __ASSERT(attr, "Did not find requested UUID");
226
227 bt_uuid_to_str(params->uuid, uuid_str, sizeof(uuid_str));
228 LOG_DBG("UUID found : %s", uuid_str);
229
230 LOG_INF("[ATTRIBUTE] handle %u", attr->handle);
231
232 conn_info_ref = get_connected_conn_info_ref(conn);
233 __ASSERT_NO_MSG(conn_info_ref);
234
235 atomic_clear_bit(conn_info_ref->flags, CONN_INFO_DISCOVER_PAUSED);
236
237 if (conn_info_ref->discover_params.type == BT_GATT_DISCOVER_PRIMARY) {
238 LOG_DBG("Primary Service Found");
239 memcpy(&conn_info_ref->uuid,
240 PERIPHERAL_CHARACTERISTIC_UUID,
241 sizeof(conn_info_ref->uuid));
242 conn_info_ref->discover_params.uuid = &conn_info_ref->uuid.uuid;
243 conn_info_ref->discover_params.start_handle = attr->handle + 1;
244 conn_info_ref->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
245
246 err = bt_gatt_discover(conn, &conn_info_ref->discover_params);
247 if (err == -ENOMEM || err == -ENOTCONN) {
248 goto retry;
249 }
250
251 __ASSERT(!err, "Discover failed (err %d)", err);
252
253 } else if (conn_info_ref->discover_params.type == BT_GATT_DISCOVER_CHARACTERISTIC) {
254 LOG_DBG("Service Characteristic Found");
255
256 conn_info_ref->discover_params.uuid = &ccc_uuid.uuid;
257 conn_info_ref->discover_params.start_handle = attr->handle + 2;
258 conn_info_ref->discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
259 conn_info_ref->subscribe_params.value_handle = bt_gatt_attr_value_handle(attr);
260
261 err = bt_gatt_discover(conn, &conn_info_ref->discover_params);
262 if (err == -ENOMEM || err == -ENOTCONN) {
263 goto retry;
264 }
265
266 __ASSERT(!err, "Discover failed (err %d)", err);
267
268 } else {
269 conn_info_ref->subscribe_params.notify = notify_func;
270 conn_info_ref->subscribe_params.value = BT_GATT_CCC_NOTIFY;
271 conn_info_ref->subscribe_params.ccc_handle = attr->handle;
272
273 err = bt_gatt_subscribe(conn, &conn_info_ref->subscribe_params);
274 if (err == -ENOMEM || err == -ENOTCONN) {
275 goto retry;
276 }
277
278 if (err != -EALREADY) {
279 __ASSERT(!err, "Subscribe failed (err %d)", err);
280 }
281
282 __ASSERT_NO_MSG(atomic_test_bit(conn_info_ref->flags, CONN_INFO_DISCOVERING));
283 __ASSERT_NO_MSG(!atomic_test_bit(conn_info_ref->flags, CONN_INFO_SUBSCRIBED));
284 atomic_set_bit(conn_info_ref->flags, CONN_INFO_SUBSCRIBED);
285
286 char addr[BT_ADDR_LE_STR_LEN];
287
288 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
289 LOG_INF("[SUBSCRIBED] addr %s", addr);
290 }
291
292 return BT_GATT_ITER_STOP;
293
294 retry:
295 /* if we're out of buffers or metadata contexts, continue discovery
296 * later.
297 */
298 LOG_INF("out of memory/not connected, continuing sub later");
299 atomic_set_bit(conn_info_ref->flags, CONN_INFO_DISCOVER_PAUSED);
300
301 return BT_GATT_ITER_STOP;
302 }
303
stop_scan(void)304 static void stop_scan(void)
305 {
306 int err;
307
308 __ASSERT(atomic_test_bit(status_flags, DEVICE_IS_SCANNING),
309 "No scanning procedure is ongoing");
310 atomic_clear_bit(status_flags, DEVICE_IS_SCANNING);
311
312 err = bt_le_scan_stop();
313 __ASSERT(!err, "Stop LE scan failed (err %d)", err);
314
315 LOG_INF("Stopped scanning");
316 }
317
check_if_peer_connected(const bt_addr_le_t * addr)318 static bool check_if_peer_connected(const bt_addr_le_t *addr)
319 {
320 for (size_t i = 0; i < ARRAY_SIZE(conn_infos); i++) {
321 if (conn_infos[i].conn_ref != NULL) {
322 if (!bt_addr_le_cmp(bt_conn_get_dst(conn_infos[i].conn_ref), addr)) {
323 return true;
324 }
325 }
326 }
327
328 return false;
329 }
330
parse_ad(struct bt_data * data,void * user_data)331 static bool parse_ad(struct bt_data *data, void *user_data)
332 {
333 bt_addr_le_t *addr = user_data;
334
335 LOG_DBG("[AD]: %u data_len %u", data->type, data->data_len);
336
337 switch (data->type) {
338 case BT_DATA_NAME_SHORTENED:
339 case BT_DATA_NAME_COMPLETE:
340 LOG_INF("------------------------------------------------------");
341 LOG_INF("Device name : %.*s", data->data_len, data->data);
342
343 if (check_if_peer_connected(addr)) {
344 LOG_ERR("Peer is already connected or in disconnecting state");
345 break;
346 }
347
348 __ASSERT(!atomic_test_bit(status_flags, DEVICE_IS_CONNECTING),
349 "A connection procedure is ongoing");
350 atomic_set_bit(status_flags, DEVICE_IS_CONNECTING);
351
352 stop_scan();
353
354 char addr_str[BT_ADDR_LE_STR_LEN];
355
356 bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
357 LOG_INF("Connecting to %s", addr_str);
358
359 struct bt_le_conn_param *param = BT_LE_CONN_PARAM_DEFAULT;
360 int err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, param,
361 &conn_connecting);
362
363 __ASSERT(!err, "Create conn failed (err %d)", err);
364
365 return false;
366
367 break;
368 }
369
370 return true;
371 }
372
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad)373 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
374 struct net_buf_simple *ad)
375 {
376 bt_data_parse(ad, parse_ad, (void *)addr);
377 }
378
start_scan(void)379 static void start_scan(void)
380 {
381 int err;
382 struct bt_le_scan_param scan_param = {
383 .type = BT_LE_SCAN_TYPE_ACTIVE,
384 .options = BT_LE_SCAN_OPT_NONE,
385 .interval = BT_GAP_SCAN_FAST_INTERVAL,
386 .window = BT_GAP_SCAN_FAST_WINDOW,
387 };
388
389 atomic_set_bit(status_flags, DEVICE_IS_SCANNING);
390
391 err = bt_le_scan_start(&scan_param, device_found);
392 __ASSERT(!err, "Scanning failed to start (err %d)", err);
393
394 LOG_INF("Started scanning");
395 }
396
connected_cb(struct bt_conn * conn,uint8_t conn_err)397 static void connected_cb(struct bt_conn *conn, uint8_t conn_err)
398 {
399 struct conn_info *conn_info_ref;
400 char addr[BT_ADDR_LE_STR_LEN];
401
402 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
403
404 __ASSERT(conn_err == BT_HCI_ERR_SUCCESS, "Failed to connect to %s (%u)", addr, conn_err);
405
406 LOG_INF("Connection %p established : %s", conn, addr);
407
408 atomic_inc(&conn_count);
409 LOG_DBG("connected to %u devices", atomic_get(&conn_count));
410
411 conn_info_ref = get_new_conn_info_ref(bt_conn_get_dst(conn));
412 __ASSERT_NO_MSG(conn_info_ref->conn_ref == NULL);
413
414 conn_info_ref->conn_ref = conn_connecting;
415
416 #if defined(CONFIG_BT_SMP)
417 int err = bt_conn_set_security(conn, BT_SECURITY_L2);
418
419 if (!err) {
420 LOG_INF("Security level is set to : %u", BT_SECURITY_L2);
421 } else {
422 LOG_ERR("Failed to set security (%d).", err);
423 }
424 #endif /* CONFIG_BT_SMP */
425
426 __ASSERT_NO_MSG(conn == conn_connecting);
427 if (conn == conn_connecting) {
428 conn_connecting = NULL;
429 atomic_clear_bit(status_flags, DEVICE_IS_CONNECTING);
430 }
431 }
432
disconnected(struct bt_conn * conn,uint8_t reason)433 static void disconnected(struct bt_conn *conn, uint8_t reason)
434 {
435 struct conn_info *conn_info_ref;
436 char addr[BT_ADDR_LE_STR_LEN];
437
438 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
439
440 LOG_INF("Disconnected: %s (reason 0x%02x)", addr, reason);
441
442 conn_info_ref = get_conn_info_ref(conn);
443 __ASSERT_NO_MSG(conn_info_ref->conn_ref != NULL);
444
445 bool valid_reason =
446 reason == BT_HCI_ERR_REMOTE_POWER_OFF ||
447 reason == BT_HCI_ERR_LOCALHOST_TERM_CONN;
448
449 __ASSERT(valid_reason, "Disconnected (reason 0x%02x)", reason);
450
451 bt_conn_unref(conn);
452 clear_info(conn_info_ref);
453 atomic_dec(&conn_count);
454 }
455
456 #if defined(CONFIG_BT_SMP)
security_changed(struct bt_conn * conn,bt_security_t level,enum bt_security_err err)457 static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
458 {
459 char addr[BT_ADDR_LE_STR_LEN];
460
461 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
462
463 __ASSERT(!err, "Security for %s failed", addr);
464 LOG_INF("Security for %s changed: level %u", addr, level);
465
466 if (err) {
467 LOG_ERR("Security failed, disconnecting");
468 bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_POWER_OFF);
469 }
470 }
471 #endif /* CONFIG_BT_SMP */
472
identity_resolved(struct bt_conn * conn,const bt_addr_le_t * rpa,const bt_addr_le_t * identity)473 static void identity_resolved(struct bt_conn *conn, const bt_addr_le_t *rpa,
474 const bt_addr_le_t *identity)
475 {
476 char addr_identity[BT_ADDR_LE_STR_LEN];
477 char addr_rpa[BT_ADDR_LE_STR_LEN];
478 struct conn_info *conn_info_ref;
479
480 bt_addr_le_to_str(identity, addr_identity, sizeof(addr_identity));
481 bt_addr_le_to_str(rpa, addr_rpa, sizeof(addr_rpa));
482
483 LOG_ERR("Identity resolved %s -> %s", addr_rpa, addr_identity);
484
485 /* overwrite RPA */
486 conn_info_ref = get_conn_info_ref(conn);
487 bt_addr_le_copy(&conn_info_ref->addr, identity);
488 }
489
490 BT_CONN_CB_DEFINE(conn_callbacks) = {
491 .connected = connected_cb,
492 .disconnected = disconnected,
493 #if defined(CONFIG_BT_SMP)
494 .security_changed = security_changed,
495 #endif /* CONFIG_BT_SMP */
496 .identity_resolved = identity_resolved,
497 };
498
mtu_updated(struct bt_conn * conn,uint16_t tx,uint16_t rx)499 void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx)
500 {
501 LOG_INF("Updated MTU: TX: %d RX: %d bytes", tx, rx);
502 }
503
504 static struct bt_gatt_cb gatt_callbacks = {.att_mtu_updated = mtu_updated};
505
mtu_exchange_cb(struct bt_conn * conn,uint8_t err,struct bt_gatt_exchange_params * params)506 static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
507 struct bt_gatt_exchange_params *params)
508 {
509 struct conn_info *conn_info_ref;
510 char addr[BT_ADDR_LE_STR_LEN];
511
512 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
513
514 conn_info_ref = get_conn_info_ref(conn);
515 __ASSERT_NO_MSG(conn_info_ref);
516
517 LOG_DBG("MTU exchange addr %s conn %s", addr,
518 err == 0U ? "successful" : "failed");
519
520 atomic_set_bit(conn_info_ref->flags, CONN_INFO_MTU_EXCHANGED);
521 }
522
exchange_mtu(struct bt_conn * conn,void * data)523 static void exchange_mtu(struct bt_conn *conn, void *data)
524 {
525 struct conn_info *conn_info_ref;
526 char addr[BT_ADDR_LE_STR_LEN];
527 int err;
528
529 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
530
531 conn_info_ref = get_connected_conn_info_ref(conn);
532 if (conn_info_ref == NULL) {
533 LOG_DBG("not connected: %s", addr);
534 return;
535 }
536
537 if (!atomic_test_bit(conn_info_ref->flags, CONN_INFO_MTU_EXCHANGED) &&
538 !atomic_test_and_set_bit(conn_info_ref->flags, CONN_INFO_SENT_MTU_EXCHANGE)) {
539 LOG_DBG("Updating MTU for %s to %u", addr, bt_gatt_get_mtu(conn));
540
541 mtu_exchange_params.func = mtu_exchange_cb;
542 err = bt_gatt_exchange_mtu(conn, &mtu_exchange_params);
543 if (err) {
544 LOG_ERR("MTU exchange failed (err %d)", err);
545 atomic_clear_bit(conn_info_ref->flags, CONN_INFO_SENT_MTU_EXCHANGE);
546 } else {
547 LOG_INF("MTU Exchange pending...");
548 }
549 }
550 }
551
subscribe_to_service(struct bt_conn * conn,void * data)552 static void subscribe_to_service(struct bt_conn *conn, void *data)
553 {
554 struct conn_info *conn_info_ref;
555 int err;
556 int *p_err = (int *)data;
557 char addr[BT_ADDR_LE_STR_LEN];
558
559 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
560
561 conn_info_ref = get_connected_conn_info_ref(conn);
562 if (conn_info_ref == NULL) {
563 LOG_DBG("not connected: %s", addr);
564 return;
565 }
566
567 /* start subscription procedure if:
568 * - we haven't started it yet for this conn
569 * - it was suspended due to a lack of resources
570 */
571 if (!atomic_test_bit(conn_info_ref->flags, CONN_INFO_SUBSCRIBED) &&
572 (!atomic_test_bit(conn_info_ref->flags, CONN_INFO_DISCOVERING) ||
573 atomic_test_bit(conn_info_ref->flags, CONN_INFO_DISCOVER_PAUSED))) {
574 /* If discovery hasn't started yet, load params. If it was
575 * already started, then not touching the params will resume
576 * discovery at the attribute it was stopped at.
577 */
578 if (!atomic_test_and_set_bit(conn_info_ref->flags, CONN_INFO_DISCOVERING)) {
579 memcpy(&conn_info_ref->uuid, PERIPHERAL_SERVICE_UUID,
580 sizeof(conn_info_ref->uuid));
581 memset(&conn_info_ref->discover_params, 0,
582 sizeof(conn_info_ref->discover_params));
583
584 conn_info_ref->discover_params.uuid = &conn_info_ref->uuid.uuid;
585 conn_info_ref->discover_params.func = discover_func;
586 conn_info_ref->discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
587 conn_info_ref->discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
588 conn_info_ref->discover_params.type = BT_GATT_DISCOVER_PRIMARY;
589 LOG_INF("start discovery of %s", addr);
590 } else {
591 LOG_INF("resume discovery of %s", addr);
592 }
593
594 err = bt_gatt_discover(conn, &conn_info_ref->discover_params);
595 if (*p_err == 0) {
596 /* Don't overwrite `err` if it was previously set. it is
597 * cleared by the caller.
598 */
599 *p_err = err;
600 }
601
602 if (err != -ENOMEM && err != -ENOTCONN) {
603 __ASSERT(!err, "Subscribe failed (err %d)", err);
604 }
605 }
606 }
607
notify_peers(struct bt_conn * conn,void * data)608 static void notify_peers(struct bt_conn *conn, void *data)
609 {
610 int err;
611 struct bt_gatt_attr *vnd_attr = (struct bt_gatt_attr *)data;
612 struct conn_info *conn_info_ref;
613 char addr[BT_ADDR_LE_STR_LEN];
614
615 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
616
617 conn_info_ref = get_connected_conn_info_ref(conn);
618 if (conn_info_ref == NULL) {
619 LOG_DBG("not connected: %s", addr);
620 return;
621 }
622
623 if (!atomic_test_bit(conn_info_ref->flags, CONN_INFO_MTU_EXCHANGED)) {
624 LOG_DBG("can't notify: MTU not yet exchanged");
625 /* sleep a bit to allow the exchange to take place */
626 k_msleep(100);
627 return;
628 }
629
630 memset(vnd_value, 0x00, sizeof(vnd_value));
631 snprintk(vnd_value, notification_size, "%s%u", NOTIFICATION_DATA_PREFIX,
632 conn_info_ref->tx_notify_counter);
633 LOG_INF("notify: %s", addr);
634 err = bt_gatt_notify(conn, vnd_attr, vnd_value, notification_size);
635 if (err) {
636 LOG_ERR("Couldn't send GATT notification");
637 return;
638 }
639
640 LOG_DBG("central notified: %s %d", addr, conn_info_ref->tx_notify_counter);
641
642 conn_info_ref->tx_notify_counter++;
643 }
644
test_central_main(void)645 void test_central_main(void)
646 {
647 int err;
648 struct bt_gatt_attr *vnd_attr;
649 char str[BT_UUID_STR_LEN];
650 char addr[BT_ADDR_LE_STR_LEN];
651
652 memset(&conn_infos, 0x00, sizeof(conn_infos));
653
654 err = bt_enable(NULL);
655
656 if (err) {
657 LOG_ERR("Bluetooth init failed (err %d)", err);
658 return;
659 }
660
661 LOG_DBG("Bluetooth initialized");
662
663 bt_gatt_cb_register(&gatt_callbacks);
664
665 vnd_attr = bt_gatt_find_by_uuid(vnd_svc.attrs, vnd_svc.attr_count, &vnd_enc_uuid.uuid);
666
667 bt_uuid_to_str(&vnd_enc_uuid.uuid, str, sizeof(str));
668 LOG_DBG("Indicate VND attr %p (UUID %s)", vnd_attr, str);
669
670 start_scan();
671
672 while (true) {
673 /* reconnect peripherals when they drop out */
674 if (atomic_get(&conn_count) < CONFIG_BT_MAX_CONN &&
675 !atomic_test_bit(status_flags, DEVICE_IS_SCANNING) &&
676 !atomic_test_bit(status_flags, DEVICE_IS_CONNECTING)) {
677 start_scan();
678 } else {
679 if (atomic_test_bit(status_flags, DEVICE_IS_CONNECTING)) {
680 bt_addr_le_to_str(bt_conn_get_dst(conn_connecting),
681 addr, sizeof(addr));
682 LOG_INF("already connecting to: %s", addr);
683 }
684 }
685
686 bt_conn_foreach(BT_CONN_TYPE_LE, exchange_mtu, NULL);
687
688 err = 0;
689 bt_conn_foreach(BT_CONN_TYPE_LE, subscribe_to_service, &err);
690 if (!err) {
691 bt_conn_foreach(BT_CONN_TYPE_LE, notify_peers, vnd_attr);
692 } else {
693 /* Allow the sub procedure to complete. Else the
694 * notifications use up all the buffers and it can never
695 * complete in time.
696 */
697 LOG_ERR("subscription failed: %d, not notifying", err);
698 }
699 k_msleep(10);
700 }
701 }
702
test_init(void)703 void test_init(void)
704 {
705 extern enum bst_result_t bst_result;
706
707 LOG_INF("Initializing Test");
708 /* The peripherals determines whether the test passed. */
709 bst_result = Passed;
710 }
711
test_args(int argc,char ** argv)712 static void test_args(int argc, char **argv)
713 {
714 conn_interval_max = DEFAULT_CONN_INTERVAL;
715 notification_size = NOTIFICATION_DATA_LEN;
716
717 if (argc >= 1) {
718 char const *ptr;
719
720 ptr = strstr(argv[0], "notify_size=");
721 if (ptr != NULL) {
722 ptr += strlen("notify_size=");
723 notification_size = atol(ptr);
724 notification_size = MIN(NOTIFICATION_DATA_LEN, notification_size);
725 }
726 }
727
728 if (argc == 2) {
729 char const *ptr;
730
731 ptr = strstr(argv[1], "conn_interval=");
732 if (ptr != NULL) {
733 ptr += strlen("conn_interval=");
734 conn_interval_max = atol(ptr);
735 }
736 }
737
738 bs_trace_raw(0, "Connection interval max : %d\n", conn_interval_max);
739 bs_trace_raw(0, "Notification data size : %d\n", notification_size);
740 }
741
742 static const struct bst_test_instance test_def[] = {
743 {
744 .test_id = "central",
745 .test_descr = "Central Connection Stress",
746 .test_args_f = test_args,
747 .test_pre_init_f = test_init,
748 .test_main_f = test_central_main
749 },
750 BSTEST_END_MARKER
751 };
752
test_main_conn_stress_install(struct bst_test_list * tests)753 struct bst_test_list *test_main_conn_stress_install(struct bst_test_list *tests)
754 {
755 return bst_add_tests(tests, test_def);
756 }
757
758 extern struct bst_test_list *test_main_conn_stress_install(struct bst_test_list *tests);
759
760 bst_test_install_t test_installers[] = {
761 test_main_conn_stress_install,
762 NULL
763 };
764
main(void)765 int main(void)
766 {
767 bst_main();
768
769 return 0;
770 }
771
