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/services/ias.h>
18 #include <zephyr/bluetooth/uuid.h>
19 #include <zephyr/kernel.h>
20 #include <zephyr/settings/settings.h>
21 #include <zephyr/sys/byteorder.h>
22 #include <zephyr/sys/check.h>
23 #include <zephyr/sys/printk.h>
24 #include <zephyr/types.h>
25 
26 #include <zephyr/logging/log.h>
27 LOG_MODULE_REGISTER(peripheral, LOG_LEVEL_INF);
28 
29 #include "bstests.h"
30 #include "bs_types.h"
31 #include "bs_tracing.h"
32 #include "bstests.h"
33 #include "bs_pc_backchannel.h"
34 #include "argparse.h"
35 
36 #define TEST_ROUNDS 10
37 #define MIN_NOTIFICATIONS 50
38 
39 #define NOTIFICATION_DATA_PREFIX     "Counter:"
40 #define NOTIFICATION_DATA_PREFIX_LEN (sizeof(NOTIFICATION_DATA_PREFIX) - 1)
41 
42 #define CHARACTERISTIC_DATA_MAX_LEN 260
43 #define NOTIFICATION_DATA_LEN	    MAX(200, (CONFIG_BT_L2CAP_TX_MTU - 4))
44 BUILD_ASSERT(NOTIFICATION_DATA_LEN <= CHARACTERISTIC_DATA_MAX_LEN);
45 
46 #define CENTRAL_SERVICE_UUID_VAL                                                                   \
47 	BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdea0)
48 
49 #define CENTRAL_CHARACTERISTIC_UUID_VAL                                                            \
50 	BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdea1)
51 
52 #define CENTRAL_SERVICE_UUID	    BT_UUID_DECLARE_128(CENTRAL_SERVICE_UUID_VAL)
53 #define CENTRAL_CHARACTERISTIC_UUID BT_UUID_DECLARE_128(CENTRAL_CHARACTERISTIC_UUID_VAL)
54 
55 /* Custom Service Variables */
56 static struct bt_uuid_128 vnd_uuid =
57 	BT_UUID_INIT_128(BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdef0));
58 
59 static struct bt_uuid_128 vnd_enc_uuid =
60 	BT_UUID_INIT_128(BT_UUID_128_ENCODE(0x12345678, 0x1234, 0x5678, 0x1234, 0x56789abcdef1));
61 
62 enum {
63 	CONN_INFO_CONNECTED,
64 	CONN_INFO_SECURITY_LEVEL_UPDATED,
65 	CONN_INFO_MTU_EXCHANGED,
66 	CONN_INFO_DISCOVERING,
67 	CONN_INFO_SUBSCRIBED,
68 	/* Total number of flags - must be at the end of the enum */
69 	CONN_INFO_NUM_FLAGS,
70 };
71 
72 struct active_conn_info {
73 	ATOMIC_DEFINE(flags, CONN_INFO_NUM_FLAGS);
74 	struct bt_conn *conn_ref;
75 	atomic_t notify_counter;
76 	atomic_t tx_notify_counter;
77 #if defined(CONFIG_BT_USER_DATA_LEN_UPDATE)
78 	struct bt_conn_le_data_len_param le_data_len_param;
79 #endif
80 };
81 
82 static struct active_conn_info conn_info;
83 
84 static uint32_t rounds;
85 
86 /* This is outside the conn context since it can remain valid across
87  * connections
88  */
89 static uint8_t central_subscription;
90 static uint8_t tx_data[CHARACTERISTIC_DATA_MAX_LEN];
91 static uint32_t notification_size; /* TODO: does this _need_ to be set in args? */
92 
93 static struct bt_uuid_128 uuid = BT_UUID_INIT_128(0);
94 static struct bt_gatt_discover_params discover_params;
95 static struct bt_gatt_subscribe_params subscribe_params;
96 
97 static const struct bt_uuid_16 ccc_uuid = BT_UUID_INIT_16(BT_UUID_GATT_CCC_VAL);
98 
vnd_ccc_cfg_changed(const struct bt_gatt_attr * attr,uint16_t value)99 static void vnd_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
100 {
101 	central_subscription = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0;
102 }
103 
104 /* Vendor Primary Service Declaration */
105 BT_GATT_SERVICE_DEFINE(
106 	vnd_svc, BT_GATT_PRIMARY_SERVICE(&vnd_uuid),
107 	BT_GATT_CHARACTERISTIC(&vnd_enc_uuid.uuid,
108 			       BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE | BT_GATT_CHRC_NOTIFY,
109 			       BT_GATT_PERM_READ | BT_GATT_PERM_WRITE, NULL, NULL,
110 			       NULL),
111 	BT_GATT_CCC(vnd_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
112 );
113 
mtu_updated(struct bt_conn * conn,uint16_t tx,uint16_t rx)114 void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx)
115 {
116 	LOG_INF("Updated MTU: TX: %d RX: %d bytes", tx, rx);
117 
118 	if (tx == CONFIG_BT_L2CAP_TX_MTU && rx == CONFIG_BT_L2CAP_TX_MTU) {
119 		char addr[BT_ADDR_LE_STR_LEN];
120 
121 		bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
122 
123 		atomic_set_bit(conn_info.flags, CONN_INFO_MTU_EXCHANGED);
124 		LOG_INF("Updating MTU succeeded %s", addr);
125 	}
126 }
127 
128 static struct bt_gatt_cb gatt_callbacks = {.att_mtu_updated = mtu_updated};
129 
connected(struct bt_conn * conn,uint8_t err)130 static void connected(struct bt_conn *conn, uint8_t err)
131 {
132 	char addr[BT_ADDR_LE_STR_LEN];
133 
134 	if (err) {
135 		memset(&conn_info, 0x00, sizeof(struct active_conn_info));
136 		LOG_ERR("Connection failed (err 0x%02x)", err);
137 		return;
138 	}
139 
140 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
141 
142 	rounds++;
143 	conn_info.conn_ref = conn;
144 
145 	atomic_set(&conn_info.tx_notify_counter, 0);
146 	atomic_set(&conn_info.notify_counter, 0);
147 	atomic_set_bit(conn_info.flags, CONN_INFO_CONNECTED);
148 
149 	LOG_INF("Connection %p established : %s", conn, addr);
150 }
151 
disconnected(struct bt_conn * conn,uint8_t reason)152 static void disconnected(struct bt_conn *conn, uint8_t reason)
153 {
154 	LOG_DBG("Disconnected (reason 0x%02x)", reason);
155 
156 	/* With a lot of devices, it is possible that the central doesn't see
157 	 * the disconnect packet.
158 	 */
159 	bool valid_reason =
160 		reason == BT_HCI_ERR_LOCALHOST_TERM_CONN ||
161 		reason == BT_HCI_ERR_CONN_TIMEOUT;
162 
163 	__ASSERT(valid_reason, "Disconnected (reason 0x%02x)", reason);
164 
165 	memset(&conn_info, 0x00, sizeof(struct active_conn_info));
166 
167 	if (rounds >= TEST_ROUNDS) {
168 		LOG_INF("Number of conn/disconn cycles reached, stopping advertiser...");
169 		bt_le_adv_stop();
170 
171 		LOG_INF("Test passed");
172 
173 		extern enum bst_result_t bst_result;
174 
175 		bst_result = Passed;
176 	}
177 }
178 
le_param_req(struct bt_conn * conn,struct bt_le_conn_param * param)179 static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
180 {
181 	char addr[BT_ADDR_LE_STR_LEN];
182 
183 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
184 
185 	LOG_DBG("LE conn param req: %s int (0x%04x (~%u ms), 0x%04x (~%u ms)) lat %d to %d",
186 		addr, param->interval_min, (uint32_t)(param->interval_min * 1.25),
187 		param->interval_max, (uint32_t)(param->interval_max * 1.25), param->latency,
188 		param->timeout);
189 
190 	return true;
191 }
192 
193 #if defined(CONFIG_BT_SMP)
security_changed(struct bt_conn * conn,bt_security_t level,enum bt_security_err err)194 static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
195 {
196 	char addr[BT_ADDR_LE_STR_LEN];
197 
198 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
199 
200 	if (err) {
201 		LOG_ERR("Security for %p failed: %s level %u err %d", conn, addr, level, err);
202 		return;
203 	}
204 
205 	LOG_INF("Security for %p changed: %s level %u", conn, addr, level);
206 	atomic_set_bit(conn_info.flags, CONN_INFO_SECURITY_LEVEL_UPDATED);
207 }
208 #endif /* CONFIG_BT_SMP */
209 
210 BT_CONN_CB_DEFINE(conn_callbacks) = {
211 	.connected = connected,
212 	.disconnected = disconnected,
213 	.le_param_req = le_param_req,
214 #if defined(CONFIG_BT_SMP)
215 	.security_changed = security_changed,
216 #endif /* CONFIG_BT_SMP */
217 };
218 
rx_notification(struct bt_conn * conn,struct bt_gatt_subscribe_params * params,const void * data,uint16_t length)219 static uint8_t rx_notification(struct bt_conn *conn, struct bt_gatt_subscribe_params *params,
220 			       const void *data, uint16_t length)
221 {
222 	const char *data_ptr = (const char *)data + NOTIFICATION_DATA_PREFIX_LEN;
223 	uint32_t received_counter;
224 	char addr[BT_ADDR_LE_STR_LEN];
225 
226 	if (!data) {
227 		LOG_INF("[UNSUBSCRIBED]");
228 		params->value_handle = 0U;
229 		return BT_GATT_ITER_STOP;
230 	}
231 
232 	/* TODO: enable */
233 	/* __ASSERT_NO_MSG(atomic_test_bit(conn_info.flags, CONN_INFO_SUBSCRIBED)); */
234 
235 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
236 
237 	received_counter = strtoul(data_ptr, NULL, 0);
238 	LOG_INF("RX %d", received_counter);
239 
240 	__ASSERT(atomic_get(&conn_info.notify_counter) == received_counter,
241 		 "expected counter : %u , received counter : %u",
242 		 atomic_get(&conn_info.notify_counter), received_counter);
243 
244 	atomic_inc(&conn_info.notify_counter);
245 
246 	return BT_GATT_ITER_CONTINUE;
247 }
248 
discover_func(struct bt_conn * conn,const struct bt_gatt_attr * attr,struct bt_gatt_discover_params * params)249 static uint8_t discover_func(struct bt_conn *conn, const struct bt_gatt_attr *attr,
250 			     struct bt_gatt_discover_params *params)
251 {
252 	int err;
253 	char uuid_str[BT_UUID_STR_LEN];
254 
255 	if (!attr) {
256 		LOG_INF("Discover complete");
257 		(void)memset(params, 0, sizeof(*params));
258 		return BT_GATT_ITER_STOP;
259 	}
260 
261 	bt_uuid_to_str(params->uuid, uuid_str, sizeof(uuid_str));
262 	LOG_DBG("UUID found : %s", uuid_str);
263 
264 	LOG_DBG("[ATTRIBUTE] handle %u", attr->handle);
265 
266 	if (discover_params.type == BT_GATT_DISCOVER_PRIMARY) {
267 		LOG_DBG("Primary Service Found");
268 		memcpy(&uuid, CENTRAL_CHARACTERISTIC_UUID, sizeof(uuid));
269 		discover_params.uuid = &uuid.uuid;
270 		discover_params.start_handle = attr->handle + 1;
271 		discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
272 
273 		err = bt_gatt_discover(conn, &discover_params);
274 		if (err == -ENOMEM) {
275 			goto nomem;
276 		}
277 
278 		__ASSERT(!err, "Discover failed (err %d)", err);
279 
280 	} else if (discover_params.type == BT_GATT_DISCOVER_CHARACTERISTIC) {
281 		LOG_DBG("Service Characteristic Found");
282 
283 		params->uuid = &ccc_uuid.uuid;
284 		params->start_handle = attr->handle + 2;
285 		params->type = BT_GATT_DISCOVER_DESCRIPTOR;
286 		subscribe_params.value_handle = bt_gatt_attr_value_handle(attr);
287 
288 		err = bt_gatt_discover(conn, params);
289 		if (err == -ENOMEM) {
290 			goto nomem;
291 		}
292 
293 		__ASSERT(!err, "Discover failed (err %d)", err);
294 
295 	} else if (atomic_test_and_clear_bit(conn_info.flags, CONN_INFO_DISCOVERING)) {
296 
297 		subscribe_params.notify = rx_notification;
298 		subscribe_params.value = BT_GATT_CCC_NOTIFY;
299 		subscribe_params.ccc_handle = attr->handle;
300 
301 		LOG_ERR("subscribe");
302 		err = bt_gatt_subscribe(conn, &subscribe_params);
303 		if (err == -ENOMEM) {
304 			goto nomem;
305 		}
306 
307 		if (err != -EALREADY) {
308 			__ASSERT(!err, "Subscribe failed (err %d)", err);
309 		}
310 
311 		__ASSERT_NO_MSG(!atomic_test_bit(conn_info.flags, CONN_INFO_SUBSCRIBED));
312 		atomic_set_bit(conn_info.flags, CONN_INFO_SUBSCRIBED);
313 
314 		char addr[BT_ADDR_LE_STR_LEN];
315 
316 		bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
317 		LOG_INF("[SUBSCRIBED] addr %s", addr);
318 	}
319 
320 	return BT_GATT_ITER_STOP;
321 
322 nomem:
323 	/* if we're out of buffers or metadata contexts, restart discovery
324 	 * later.
325 	 */
326 	LOG_ERR("out of memory, retry sub later");
327 	atomic_clear_bit(conn_info.flags, CONN_INFO_DISCOVERING);
328 
329 	return BT_GATT_ITER_STOP;
330 }
331 
subscribe_to_service(struct bt_conn * conn)332 static void subscribe_to_service(struct bt_conn *conn)
333 {
334 	while (!atomic_test_and_set_bit(conn_info.flags, CONN_INFO_DISCOVERING) &&
335 	       !atomic_test_bit(conn_info.flags, CONN_INFO_SUBSCRIBED)) {
336 		int err;
337 		char addr[BT_ADDR_LE_STR_LEN];
338 
339 		bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
340 
341 		memcpy(&uuid, CENTRAL_SERVICE_UUID, sizeof(uuid));
342 		discover_params.uuid = &uuid.uuid;
343 		discover_params.func = discover_func;
344 		discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
345 		discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
346 		discover_params.type = BT_GATT_DISCOVER_PRIMARY;
347 
348 		err = bt_gatt_discover(conn, &discover_params);
349 		if (err == -ENOMEM) {
350 			LOG_DBG("out of memory, retry sub later");
351 			atomic_clear_bit(conn_info.flags, CONN_INFO_DISCOVERING);
352 		}
353 
354 		__ASSERT(!err, "Subscribe failed (err %d)", err);
355 
356 		while (atomic_test_bit(conn_info.flags, CONN_INFO_DISCOVERING) &&
357 		       !atomic_test_bit(conn_info.flags, CONN_INFO_SUBSCRIBED)) {
358 			k_sleep(K_MSEC(10));
359 		}
360 	}
361 }
362 
set_tx_payload(uint32_t count)363 void set_tx_payload(uint32_t count)
364 {
365 	memset(tx_data, 0x00, sizeof(tx_data));
366 	snprintk(tx_data, notification_size, "%s%u", NOTIFICATION_DATA_PREFIX, count);
367 }
368 
disconnect(void)369 void disconnect(void)
370 {
371 	/* we should always be the ones doing the disconnecting */
372 	__ASSERT_NO_MSG(conn_info.conn_ref);
373 
374 	int err = bt_conn_disconnect(conn_info.conn_ref,
375 				     BT_HCI_ERR_REMOTE_POWER_OFF);
376 
377 	if (err) {
378 		LOG_ERR("Terminating conn failed (err %d)", err);
379 	}
380 
381 	/* wait for disconnection callback */
382 	while (atomic_test_bit(conn_info.flags, CONN_INFO_CONNECTED)) {
383 		k_sleep(K_MSEC(10));
384 	}
385 }
386 
test_peripheral_main(void)387 void test_peripheral_main(void)
388 {
389 	struct bt_gatt_attr *vnd_attr;
390 	char name[10];
391 	int err;
392 
393 	err = bt_enable(NULL);
394 	if (err) {
395 		LOG_ERR("Bluetooth init failed (err %d)", err);
396 		return;
397 	}
398 	LOG_DBG("Bluetooth initialized");
399 
400 	sprintf(name, "per-%d", get_device_nbr());
401 	bt_set_name(name);
402 
403 	err = bt_le_adv_start(BT_LE_ADV_CONN_FAST_1, NULL, 0, NULL, 0);
404 	if (err) {
405 		LOG_ERR("Advertising failed to start (err %d)", err);
406 		__ASSERT_NO_MSG(err);
407 	}
408 	LOG_INF("Started advertising");
409 
410 	bt_gatt_cb_register(&gatt_callbacks);
411 
412 	vnd_attr = bt_gatt_find_by_uuid(vnd_svc.attrs, vnd_svc.attr_count, &vnd_enc_uuid.uuid);
413 
414 	while (true) {
415 		LOG_DBG("Waiting for connection from central..");
416 		while (!atomic_test_bit(conn_info.flags, CONN_INFO_CONNECTED)) {
417 			k_sleep(K_MSEC(10));
418 		}
419 
420 		LOG_DBG("Subscribing to central..");
421 		subscribe_to_service(conn_info.conn_ref);
422 
423 		LOG_DBG("Waiting until central subscribes..");
424 		while (!central_subscription) {
425 			k_sleep(K_MSEC(10));
426 		}
427 
428 		while (!atomic_test_bit(conn_info.flags, CONN_INFO_MTU_EXCHANGED)) {
429 			k_sleep(K_MSEC(10));
430 		}
431 
432 		LOG_INF("Begin sending notifications to central..");
433 		while (central_subscription &&
434 		       atomic_test_bit(conn_info.flags, CONN_INFO_CONNECTED)) {
435 
436 			set_tx_payload(atomic_get(&conn_info.tx_notify_counter));
437 			err = bt_gatt_notify(NULL, vnd_attr, tx_data, notification_size);
438 			if (err) {
439 				if (atomic_get(&conn_info.tx_notify_counter) > 0) {
440 					atomic_dec(&conn_info.tx_notify_counter);
441 				}
442 				LOG_DBG("Couldn't send GATT notification");
443 				k_msleep(10);
444 			} else {
445 				atomic_inc(&conn_info.tx_notify_counter);
446 				LOG_INF("TX %d", atomic_get(&conn_info.tx_notify_counter));
447 			}
448 
449 			if (atomic_get(&conn_info.tx_notify_counter) > MIN_NOTIFICATIONS &&
450 			    atomic_get(&conn_info.notify_counter) > MIN_NOTIFICATIONS) {
451 				LOG_INF("Disconnecting..");
452 				disconnect();
453 			}
454 		}
455 	}
456 }
457 
test_init(void)458 void test_init(void)
459 {
460 	extern enum bst_result_t bst_result;
461 
462 	LOG_INF("Initializing Test");
463 	bst_result = Failed;
464 }
465 
test_args(int argc,char ** argv)466 static void test_args(int argc, char **argv)
467 {
468 	notification_size = NOTIFICATION_DATA_LEN;
469 
470 	if (argc >= 1) {
471 		char const *ptr;
472 
473 		ptr = strstr(argv[0], "notify_size=");
474 		if (ptr != NULL) {
475 			ptr += strlen("notify_size=");
476 			notification_size = atol(ptr);
477 			notification_size = MIN(NOTIFICATION_DATA_LEN, notification_size);
478 		}
479 	}
480 
481 	bs_trace_raw(0, "Notification data size : %d\n", notification_size);
482 }
483 
484 static const struct bst_test_instance test_def[] = {
485 	{
486 		.test_id = "peripheral",
487 		.test_descr = "Peripheral Connection Stress",
488 		.test_args_f = test_args,
489 		.test_pre_init_f = test_init,
490 		.test_main_f = test_peripheral_main
491 	},
492 	BSTEST_END_MARKER
493 };
494 
test_main_conn_stress_install(struct bst_test_list * tests)495 struct bst_test_list *test_main_conn_stress_install(struct bst_test_list *tests)
496 {
497 	return bst_add_tests(tests, test_def);
498 }
499 
500 extern struct bst_test_list *test_main_conn_stress_install(struct bst_test_list *tests);
501 
502 bst_test_install_t test_installers[] = {
503 	test_main_conn_stress_install,
504 	NULL
505 };
506 
main(void)507 int main(void)
508 {
509 	bst_main();
510 
511 	return 0;
512 }
513