samples/bluetooth/encrypted_advertising/central/src/central_ead.c - third_party/github/zephyrproject-rtos/zephyr - Git at Google

 /*
  * Copyright (c) 2023 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */

 #include <stdint.h>
 #include <string.h>

 #include <zephyr/kernel.h>

 #include <zephyr/net/buf.h>
 #include <zephyr/sys/printk.h>
 #include <zephyr/logging/log.h>

 #include <zephyr/bluetooth/ead.h>
 #include <zephyr/bluetooth/att.h>
 #include <zephyr/bluetooth/gatt.h>
 #include <zephyr/bluetooth/addr.h>
 #include <zephyr/bluetooth/conn.h>
 #include <zephyr/bluetooth/uuid.h>
 #include <zephyr/bluetooth/bluetooth.h>

 #include "common/bt_str.h"

 #include "common.h"

 LOG_MODULE_REGISTER(ead_central_sample, CONFIG_BT_EAD_LOG_LEVEL);

 static uint8_t *received_data;
 static size_t received_data_size;
 static struct key_material keymat;

 static bt_addr_le_t peer_addr;
 static struct bt_conn *default_conn;

 static struct bt_conn_cb central_cb;
 static struct bt_conn_auth_cb central_auth_cb;

 static struct k_poll_signal conn_signal;
 static struct k_poll_signal sec_update_signal;
 static struct k_poll_signal passkey_enter_signal;
 static struct k_poll_signal device_found_cb_completed;

 /* GATT Discover data */
 static uint8_t gatt_disc_err;
 static uint16_t gatt_disc_end_handle;
 static uint16_t gatt_disc_start_handle;
 static struct k_poll_signal gatt_disc_signal;

 /* GATT Read data */
 static uint8_t gatt_read_err;
 static uint8_t *gatt_read_res;
 static uint16_t gatt_read_len;
 static uint16_t gatt_read_handle;
 static struct k_poll_signal gatt_read_signal;

 static bool data_parse_cb(struct bt_data *data, void *user_data)
 {
 	size_t *parsed_data_size = (size_t *)user_data;

 	if (data->type == BT_DATA_ENCRYPTED_AD_DATA) {
 		int err;
 		struct net_buf_simple decrypted_buf;
 		size_t decrypted_data_size = BT_EAD_DECRYPTED_PAYLOAD_SIZE(data->data_len);
 		uint8_t decrypted_data[decrypted_data_size];

 		err = bt_ead_decrypt(keymat.session_key, keymat.iv, data->data, data->data_len,
 				     decrypted_data);
 		if (err < 0) {
 			LOG_ERR("Error during decryption (err %d)", err);
 		}

 		net_buf_simple_init_with_data(&decrypted_buf, &decrypted_data[0],
 					      decrypted_data_size);

 		bt_data_parse(&decrypted_buf, &data_parse_cb, user_data);
 	} else {
 		LOG_INF("len : %u", data->data_len);
 		LOG_INF("type: 0x%02x", data->type);
 		LOG_HEXDUMP_INF(data->data, data->data_len, "data:");

 		/* Copy the data out if we are running in a test context */
 		if (received_data != NULL) {
 			if (bt_data_get_len(data, 1) <=
 			    (received_data_size - (*parsed_data_size))) {
 				*parsed_data_size +=
 					bt_data_serialize(data, &received_data[*parsed_data_size]);
 			}
 		} else {
 			*parsed_data_size += bt_data_get_len(data, 1);
 		}
 	}

 	return true;
 }

 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
 			 struct net_buf_simple *ad)
 {
 	int err;
 	size_t parsed_data_size;
 	char addr_str[BT_ADDR_LE_STR_LEN];

 	if (default_conn) {
 		return;
 	}

 	bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));

 	/* We are only interested in the previously connected device. */
 	if (!bt_addr_le_eq(addr, &peer_addr)) {
 		return;
 	}

 	LOG_DBG("Peer found.");

 	parsed_data_size = 0;
 	LOG_INF("Received data size: %zu", ad->len);
 	bt_data_parse(ad, data_parse_cb, &parsed_data_size);

 	LOG_DBG("All data parsed. (total size: %zu)", parsed_data_size);

 	err = bt_le_scan_stop();
 	if (err) {
 		LOG_DBG("Failed to stop scanner (err %d)", err);
 		return;
 	}

 	k_poll_signal_raise(&device_found_cb_completed, 0);
 }

 static void connect_device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
 				 struct net_buf_simple *ad)
 {
 	int err;
 	char addr_str[BT_ADDR_LE_STR_LEN];

 	if (default_conn) {
 		return;
 	}

 	/* Connect only to devices in close range */
 	if (rssi < -70) {
 		return;
 	}

 	bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));

 	LOG_DBG("Device found: %s (RSSI %d)", addr_str, rssi);

 	err = bt_le_scan_stop();
 	if (err) {
 		LOG_DBG("Failed to stop scanner (err %d)", err);
 		return;
 	}

 	err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
 				&default_conn);
 	if (err) {
 		LOG_DBG("Failed to connect to %s (err %d)", addr_str, err);
 		return;
 	}

 	k_poll_signal_raise(&device_found_cb_completed, 0);
 }

 static int start_scan(bool connect)
 {
 	int err;

 	k_poll_signal_reset(&conn_signal);
 	k_poll_signal_reset(&device_found_cb_completed);

 	err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, connect ? connect_device_found : device_found);
 	if (err) {
 		LOG_DBG("Scanning failed to start (err %d)", err);
 		return -1;
 	}

 	LOG_DBG("Scanning started.");

 	if (connect) {
 		LOG_DBG("Waiting for connection");
 		await_signal(&conn_signal);
 	}

 	await_signal(&device_found_cb_completed);

 	return 0;
 }

 static uint8_t gatt_read_cb(struct bt_conn *conn, uint8_t att_err,
 			    struct bt_gatt_read_params *params, const void *data, uint16_t read_len)
 {
 	gatt_read_err = att_err;
 	gatt_read_len = read_len;
 	gatt_read_handle = params->by_uuid.start_handle;

 	if (!att_err) {
 		memcpy(gatt_read_res, data, read_len);
 		k_poll_signal_raise(&gatt_read_signal, 0);
 	} else {
 		LOG_ERR("ATT error (err %d)", att_err);
 	}

 	return BT_GATT_ITER_STOP;
 }

 static int gatt_read(struct bt_conn *conn, const struct bt_uuid *uuid, size_t read_size,
 		     uint16_t start_handle, uint16_t end_handle, uint8_t *buf)
 {
 	int err;
 	size_t offset;
 	uint16_t handle;
 	struct bt_gatt_read_params params;

 	gatt_read_res = &buf[0];

 	params.handle_count = 0;
 	params.by_uuid.start_handle = start_handle;
 	params.by_uuid.end_handle = end_handle;
 	params.by_uuid.uuid = uuid;
 	params.func = gatt_read_cb;

 	k_poll_signal_reset(&gatt_read_signal);

 	err = bt_gatt_read(conn, &params);
 	if (err) {
 		LOG_DBG("GATT read failed (err %d)", err);
 		return -1;
 	}

 	await_signal(&gatt_read_signal);

 	offset = gatt_read_len;
 	handle = gatt_read_handle;

 	while (offset < read_size) {
 		gatt_read_res = &buf[offset];

 		params.handle_count = 1;
 		params.single.handle = handle;
 		params.single.offset = offset;

 		k_poll_signal_reset(&gatt_read_signal);

 		err = bt_gatt_read(conn, &params);
 		if (err) {
 			LOG_DBG("GATT read failed (err %d)", err);
 			return -1;
 		}

 		await_signal(&gatt_read_signal);

 		offset += gatt_read_len;
 	}

 	return 0;
 }

 static uint8_t gatt_discover_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
 				struct bt_gatt_discover_params *params)
 {
 	gatt_disc_err = attr ? 0 : BT_ATT_ERR_ATTRIBUTE_NOT_FOUND;

 	if (attr) {
 		gatt_disc_start_handle = attr->handle;
 		gatt_disc_end_handle = ((struct bt_gatt_service_val *)attr->user_data)->end_handle;
 	}

 	k_poll_signal_raise(&gatt_disc_signal, 0);

 	return BT_GATT_ITER_STOP;
 }

 static int gatt_discover_primary_service(struct bt_conn *conn, const struct bt_uuid *service_type,
 					 uint16_t *start_handle, uint16_t *end_handle)
 {
 	int err;
 	struct bt_gatt_discover_params params;

 	params.type = BT_GATT_DISCOVER_PRIMARY;
 	params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
 	params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
 	params.uuid = service_type;
 	params.func = gatt_discover_cb;

 	k_poll_signal_reset(&gatt_disc_signal);

 	err = bt_gatt_discover(conn, &params);
 	if (err) {
 		LOG_DBG("Primary service discover failed (err %d)", err);
 		return -1;
 	}

 	await_signal(&gatt_disc_signal);

 	*start_handle = gatt_disc_start_handle;
 	*end_handle = gatt_disc_end_handle;

 	return gatt_disc_err;
 }

 static void connected(struct bt_conn *conn, uint8_t conn_err)
 {
 	char addr[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

 	if (conn_err) {
 		LOG_DBG("Failed to connect to %s (err %u)", addr, conn_err);

 		bt_conn_unref(default_conn);
 		default_conn = NULL;

 		(void)start_scan(true);

 		return;
 	}

 	LOG_DBG("Connected to: %s", addr);

 	k_poll_signal_raise(&conn_signal, 0);
 }

 static void disconnected(struct bt_conn *conn, uint8_t reason)
 {
 	char addr[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

 	LOG_DBG("Disconnected: %s (reason 0x%02x)", addr, reason);

 	if (default_conn != conn) {
 		return;
 	}

 	bt_conn_unref(default_conn);
 	default_conn = NULL;
 }

 static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
 {
 	char addr[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

 	if (!err) {
 		LOG_DBG("Security changed: %s level %u", addr, level);
 		k_poll_signal_raise(&sec_update_signal, 0);
 	} else {
 		LOG_DBG("Security failed: %s level %u err %d", addr, level, err);
 	}
 }

 static void identity_resolved(struct bt_conn *conn, const bt_addr_le_t *rpa,
 			      const bt_addr_le_t *identity)
 {
 	char addr_identity[BT_ADDR_LE_STR_LEN];
 	char addr_rpa[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(identity, addr_identity, sizeof(addr_identity));
 	bt_addr_le_to_str(rpa, addr_rpa, sizeof(addr_rpa));

 	LOG_DBG("Identity resolved %s -> %s", addr_rpa, addr_identity);

 	bt_addr_le_copy(&peer_addr, identity);
 }

 static void auth_passkey_confirm(struct bt_conn *conn, unsigned int passkey)
 {
 	char passkey_str[7];
 	char addr[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

 	snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);

 	printk("Passkey for %s: %s\n", addr, passkey_str);

 	k_poll_signal_raise(&passkey_enter_signal, 0);
 }

 static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
 {
 	char passkey_str[7];
 	char addr[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

 	snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);

 	LOG_DBG("Passkey for %s: %s", addr, passkey_str);
 }

 static void auth_cancel(struct bt_conn *conn)
 {
 	char addr[BT_ADDR_LE_STR_LEN];

 	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

 	LOG_DBG("Pairing cancelled: %s", addr);
 }

 static int init_bt(void)
 {
 	int err;

 	default_conn = NULL;

 	k_poll_signal_init(&conn_signal);
 	k_poll_signal_init(&passkey_enter_signal);
 	k_poll_signal_init(&sec_update_signal);
 	k_poll_signal_init(&gatt_disc_signal);
 	k_poll_signal_init(&gatt_read_signal);
 	k_poll_signal_init(&device_found_cb_completed);

 	err = bt_enable(NULL);
 	if (err) {
 		LOG_ERR("Bluetooth init failed (err %d)", err);
 		return -1;
 	}

 	LOG_DBG("Bluetooth initialized");

 	err = bt_unpair(BT_ID_DEFAULT, BT_ADDR_LE_ANY);
 	if (err) {
 		LOG_ERR("Unpairing failed (err %d)", err);
 	}

 	central_cb.connected = connected;
 	central_cb.disconnected = disconnected;
 	central_cb.security_changed = security_changed;
 	central_cb.identity_resolved = identity_resolved;

 	bt_conn_cb_register(&central_cb);

 	central_auth_cb.pairing_confirm = NULL;
 	central_auth_cb.passkey_confirm = auth_passkey_confirm;
 	central_auth_cb.passkey_display = auth_passkey_display;
 	central_auth_cb.passkey_entry = NULL;
 	central_auth_cb.oob_data_request = NULL;
 	central_auth_cb.cancel = auth_cancel;

 	err = bt_conn_auth_cb_register(&central_auth_cb);
 	if (err) {
 		return -1;
 	}

 	return 0;
 }

 int run_central_sample(int get_passkey_confirmation(struct bt_conn *conn),
 		       uint8_t *test_received_data, size_t test_received_data_size,
 		       struct key_material *test_received_keymat)
 {
 	int err;
 	bool connect;
 	uint16_t end_handle;
 	uint16_t start_handle;

 	if (test_received_data != NULL) {
 		received_data = test_received_data;
 		received_data_size = test_received_data_size;
 	}

 	/* Initialize Bluetooth and callbacks */
 	err = init_bt();
 	if (err) {
 		return -1;
 	}

 	/* Start scan and connect to our peripheral */
 	connect = true;
 	err = start_scan(connect);
 	if (err) {
 		return -2;
 	}

 	/* Update connection security level */
 	err = bt_conn_set_security(default_conn, BT_SECURITY_L4);
 	if (err) {
 		LOG_ERR("Failed to set security (err %d)", err);
 		return -3;
 	}

 	await_signal(&passkey_enter_signal);

 	err = get_passkey_confirmation(default_conn);
 	if (err) {
 		LOG_ERR("Security update failed");
 		return -4;
 	}

 	/* Locate the primary service */
 	err = gatt_discover_primary_service(default_conn, BT_UUID_CUSTOM_SERVICE, &start_handle,
 					    &end_handle);
 	if (err) {
 		LOG_ERR("Service not found (err %d)", err);
 		return -5;
 	}

 	/* Read the Key Material characteristic */
 	err = gatt_read(default_conn, BT_UUID_GATT_EDKM, sizeof(keymat), start_handle, end_handle,
 			(uint8_t *)&keymat);
 	if (err) {
 		LOG_ERR("GATT read failed (err %d)", err);
 		return -6;
 	}

 	LOG_HEXDUMP_DBG(keymat.session_key, BT_EAD_KEY_SIZE, "Session Key");
 	LOG_HEXDUMP_DBG(keymat.iv, BT_EAD_IV_SIZE, "IV");

 	if (test_received_keymat != NULL) {
 		memcpy(test_received_keymat, &keymat, sizeof(keymat));
 	}

 	/* Start a new scan to get and decrypt the Advertising Data */
 	err = bt_conn_disconnect(default_conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
 	if (err) {
 		LOG_ERR("Failed to disconnect.");
 		return -7;
 	}

 	connect = false;
 	err = start_scan(connect);
 	if (err) {
 		return -2;
 	}

 	return 0;
 }
	/*
	* Copyright (c) 2023 Nordic Semiconductor ASA
	*
	* SPDX-License-Identifier: Apache-2.0
	*/

	#include <stdint.h>
	#include <string.h>

	#include <zephyr/kernel.h>

	#include <zephyr/net/buf.h>
	#include <zephyr/sys/printk.h>
	#include <zephyr/logging/log.h>

	#include <zephyr/bluetooth/ead.h>
	#include <zephyr/bluetooth/att.h>
	#include <zephyr/bluetooth/gatt.h>
	#include <zephyr/bluetooth/addr.h>
	#include <zephyr/bluetooth/conn.h>
	#include <zephyr/bluetooth/uuid.h>
	#include <zephyr/bluetooth/bluetooth.h>

	#include "common/bt_str.h"

	#include "common.h"

	LOG_MODULE_REGISTER(ead_central_sample, CONFIG_BT_EAD_LOG_LEVEL);

	static uint8_t *received_data;
	static size_t received_data_size;
	static struct key_material keymat;

	static bt_addr_le_t peer_addr;
	static struct bt_conn *default_conn;

	static struct bt_conn_cb central_cb;
	static struct bt_conn_auth_cb central_auth_cb;

	static struct k_poll_signal conn_signal;
	static struct k_poll_signal sec_update_signal;
	static struct k_poll_signal passkey_enter_signal;
	static struct k_poll_signal device_found_cb_completed;

	/* GATT Discover data */
	static uint8_t gatt_disc_err;
	static uint16_t gatt_disc_end_handle;
	static uint16_t gatt_disc_start_handle;
	static struct k_poll_signal gatt_disc_signal;

	/* GATT Read data */
	static uint8_t gatt_read_err;
	static uint8_t *gatt_read_res;
	static uint16_t gatt_read_len;
	static uint16_t gatt_read_handle;
	static struct k_poll_signal gatt_read_signal;

	static bool data_parse_cb(struct bt_data data, void user_data)
	{
	size_t parsed_data_size = (size_t )user_data;

	if (data->type == BT_DATA_ENCRYPTED_AD_DATA) {
	int err;
	struct net_buf_simple decrypted_buf;
	size_t decrypted_data_size = BT_EAD_DECRYPTED_PAYLOAD_SIZE(data->data_len);
	uint8_t decrypted_data[decrypted_data_size];

	err = bt_ead_decrypt(keymat.session_key, keymat.iv, data->data, data->data_len,
	decrypted_data);
	if (err < 0) {
	LOG_ERR("Error during decryption (err %d)", err);
	}

	net_buf_simple_init_with_data(&decrypted_buf, &decrypted_data[0],
	decrypted_data_size);

	bt_data_parse(&decrypted_buf, &data_parse_cb, user_data);
	} else {
	LOG_INF("len : %u", data->data_len);
	LOG_INF("type: 0x%02x", data->type);
	LOG_HEXDUMP_INF(data->data, data->data_len, "data:");

	/* Copy the data out if we are running in a test context */
	if (received_data != NULL) {
	if (bt_data_get_len(data, 1) <=
	(received_data_size - (*parsed_data_size))) {
	*parsed_data_size +=
	bt_data_serialize(data, &received_data[*parsed_data_size]);
	}
	} else {
	*parsed_data_size += bt_data_get_len(data, 1);
	}
	}

	return true;
	}

	static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
	struct net_buf_simple *ad)
	{
	int err;
	size_t parsed_data_size;
	char addr_str[BT_ADDR_LE_STR_LEN];

	if (default_conn) {
	return;
	}

	bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));

	/* We are only interested in the previously connected device. */
	if (!bt_addr_le_eq(addr, &peer_addr)) {
	return;
	}

	LOG_DBG("Peer found.");

	parsed_data_size = 0;
	LOG_INF("Received data size: %zu", ad->len);
	bt_data_parse(ad, data_parse_cb, &parsed_data_size);

	LOG_DBG("All data parsed. (total size: %zu)", parsed_data_size);

	err = bt_le_scan_stop();
	if (err) {
	LOG_DBG("Failed to stop scanner (err %d)", err);
	return;
	}

	k_poll_signal_raise(&device_found_cb_completed, 0);
	}

	static void connect_device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
	struct net_buf_simple *ad)
	{
	int err;
	char addr_str[BT_ADDR_LE_STR_LEN];

	if (default_conn) {
	return;
	}

	/* Connect only to devices in close range */
	if (rssi < -70) {
	return;
	}

	bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));

	LOG_DBG("Device found: %s (RSSI %d)", addr_str, rssi);

	err = bt_le_scan_stop();
	if (err) {
	LOG_DBG("Failed to stop scanner (err %d)", err);
	return;
	}

	err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
	&default_conn);
	if (err) {
	LOG_DBG("Failed to connect to %s (err %d)", addr_str, err);
	return;
	}

	k_poll_signal_raise(&device_found_cb_completed, 0);
	}

	static int start_scan(bool connect)
	{
	int err;

	k_poll_signal_reset(&conn_signal);
	k_poll_signal_reset(&device_found_cb_completed);

	err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, connect ? connect_device_found : device_found);
	if (err) {
	LOG_DBG("Scanning failed to start (err %d)", err);
	return -1;
	}

	LOG_DBG("Scanning started.");

	if (connect) {
	LOG_DBG("Waiting for connection");
	await_signal(&conn_signal);
	}

	await_signal(&device_found_cb_completed);

	return 0;
	}

	static uint8_t gatt_read_cb(struct bt_conn *conn, uint8_t att_err,
	struct bt_gatt_read_params params, const void data, uint16_t read_len)
	{
	gatt_read_err = att_err;
	gatt_read_len = read_len;
	gatt_read_handle = params->by_uuid.start_handle;

	if (!att_err) {
	memcpy(gatt_read_res, data, read_len);
	k_poll_signal_raise(&gatt_read_signal, 0);
	} else {
	LOG_ERR("ATT error (err %d)", att_err);
	}

	return BT_GATT_ITER_STOP;
	}

	static int gatt_read(struct bt_conn conn, const struct bt_uuid uuid, size_t read_size,
	uint16_t start_handle, uint16_t end_handle, uint8_t *buf)
	{
	int err;
	size_t offset;
	uint16_t handle;
	struct bt_gatt_read_params params;

	gatt_read_res = &buf[0];

	params.handle_count = 0;
	params.by_uuid.start_handle = start_handle;
	params.by_uuid.end_handle = end_handle;
	params.by_uuid.uuid = uuid;
	params.func = gatt_read_cb;

	k_poll_signal_reset(&gatt_read_signal);

	err = bt_gatt_read(conn, &params);
	if (err) {
	LOG_DBG("GATT read failed (err %d)", err);
	return -1;
	}

	await_signal(&gatt_read_signal);

	offset = gatt_read_len;
	handle = gatt_read_handle;

	while (offset < read_size) {
	gatt_read_res = &buf[offset];

	params.handle_count = 1;
	params.single.handle = handle;
	params.single.offset = offset;

	k_poll_signal_reset(&gatt_read_signal);

	err = bt_gatt_read(conn, &params);
	if (err) {
	LOG_DBG("GATT read failed (err %d)", err);
	return -1;
	}

	await_signal(&gatt_read_signal);

	offset += gatt_read_len;
	}

	return 0;
	}

	static uint8_t gatt_discover_cb(struct bt_conn conn, const struct bt_gatt_attr attr,
	struct bt_gatt_discover_params *params)
	{
	gatt_disc_err = attr ? 0 : BT_ATT_ERR_ATTRIBUTE_NOT_FOUND;

	if (attr) {
	gatt_disc_start_handle = attr->handle;
	gatt_disc_end_handle = ((struct bt_gatt_service_val *)attr->user_data)->end_handle;
	}

	k_poll_signal_raise(&gatt_disc_signal, 0);

	return BT_GATT_ITER_STOP;
	}

	static int gatt_discover_primary_service(struct bt_conn conn, const struct bt_uuid service_type,
	uint16_t start_handle, uint16_t end_handle)
	{
	int err;
	struct bt_gatt_discover_params params;

	params.type = BT_GATT_DISCOVER_PRIMARY;
	params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
	params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
	params.uuid = service_type;
	params.func = gatt_discover_cb;

	k_poll_signal_reset(&gatt_disc_signal);

	err = bt_gatt_discover(conn, &params);
	if (err) {
	LOG_DBG("Primary service discover failed (err %d)", err);
	return -1;
	}

	await_signal(&gatt_disc_signal);

	*start_handle = gatt_disc_start_handle;
	*end_handle = gatt_disc_end_handle;

	return gatt_disc_err;
	}

	static void connected(struct bt_conn *conn, uint8_t conn_err)
	{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	if (conn_err) {
	LOG_DBG("Failed to connect to %s (err %u)", addr, conn_err);

	bt_conn_unref(default_conn);
	default_conn = NULL;

	(void)start_scan(true);

	return;
	}

	LOG_DBG("Connected to: %s", addr);

	k_poll_signal_raise(&conn_signal, 0);
	}

	static void disconnected(struct bt_conn *conn, uint8_t reason)
	{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_DBG("Disconnected: %s (reason 0x%02x)", addr, reason);

	if (default_conn != conn) {
	return;
	}

	bt_conn_unref(default_conn);
	default_conn = NULL;
	}

	static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err)
	{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	if (!err) {
	LOG_DBG("Security changed: %s level %u", addr, level);
	k_poll_signal_raise(&sec_update_signal, 0);
	} else {
	LOG_DBG("Security failed: %s level %u err %d", addr, level, err);
	}
	}

	static void identity_resolved(struct bt_conn conn, const bt_addr_le_t rpa,
	const bt_addr_le_t *identity)
	{
	char addr_identity[BT_ADDR_LE_STR_LEN];
	char addr_rpa[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(identity, addr_identity, sizeof(addr_identity));
	bt_addr_le_to_str(rpa, addr_rpa, sizeof(addr_rpa));

	LOG_DBG("Identity resolved %s -> %s", addr_rpa, addr_identity);

	bt_addr_le_copy(&peer_addr, identity);
	}

	static void auth_passkey_confirm(struct bt_conn *conn, unsigned int passkey)
	{
	char passkey_str[7];
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);

	printk("Passkey for %s: %s\n", addr, passkey_str);

	k_poll_signal_raise(&passkey_enter_signal, 0);
	}

	static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
	{
	char passkey_str[7];
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);

	LOG_DBG("Passkey for %s: %s", addr, passkey_str);
	}

	static void auth_cancel(struct bt_conn *conn)
	{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_DBG("Pairing cancelled: %s", addr);
	}

	static int init_bt(void)
	{
	int err;

	default_conn = NULL;

	k_poll_signal_init(&conn_signal);
	k_poll_signal_init(&passkey_enter_signal);
	k_poll_signal_init(&sec_update_signal);
	k_poll_signal_init(&gatt_disc_signal);
	k_poll_signal_init(&gatt_read_signal);
	k_poll_signal_init(&device_found_cb_completed);

	err = bt_enable(NULL);
	if (err) {
	LOG_ERR("Bluetooth init failed (err %d)", err);
	return -1;
	}

	LOG_DBG("Bluetooth initialized");

	err = bt_unpair(BT_ID_DEFAULT, BT_ADDR_LE_ANY);
	if (err) {
	LOG_ERR("Unpairing failed (err %d)", err);
	}

	central_cb.connected = connected;
	central_cb.disconnected = disconnected;
	central_cb.security_changed = security_changed;
	central_cb.identity_resolved = identity_resolved;

	bt_conn_cb_register(&central_cb);

	central_auth_cb.pairing_confirm = NULL;
	central_auth_cb.passkey_confirm = auth_passkey_confirm;
	central_auth_cb.passkey_display = auth_passkey_display;
	central_auth_cb.passkey_entry = NULL;
	central_auth_cb.oob_data_request = NULL;
	central_auth_cb.cancel = auth_cancel;

	err = bt_conn_auth_cb_register(&central_auth_cb);
	if (err) {
	return -1;
	}

	return 0;
	}

	int run_central_sample(int get_passkey_confirmation(struct bt_conn *conn),
	uint8_t *test_received_data, size_t test_received_data_size,
	struct key_material *test_received_keymat)
	{
	int err;
	bool connect;
	uint16_t end_handle;
	uint16_t start_handle;

	if (test_received_data != NULL) {
	received_data = test_received_data;
	received_data_size = test_received_data_size;
	}

	/* Initialize Bluetooth and callbacks */
	err = init_bt();
	if (err) {
	return -1;
	}

	/* Start scan and connect to our peripheral */
	connect = true;
	err = start_scan(connect);
	if (err) {
	return -2;
	}

	/* Update connection security level */
	err = bt_conn_set_security(default_conn, BT_SECURITY_L4);
	if (err) {
	LOG_ERR("Failed to set security (err %d)", err);
	return -3;
	}

	await_signal(&passkey_enter_signal);

	err = get_passkey_confirmation(default_conn);
	if (err) {
	LOG_ERR("Security update failed");
	return -4;
	}

	/* Locate the primary service */
	err = gatt_discover_primary_service(default_conn, BT_UUID_CUSTOM_SERVICE, &start_handle,
	&end_handle);
	if (err) {
	LOG_ERR("Service not found (err %d)", err);
	return -5;
	}

	/* Read the Key Material characteristic */
	err = gatt_read(default_conn, BT_UUID_GATT_EDKM, sizeof(keymat), start_handle, end_handle,
	(uint8_t *)&keymat);
	if (err) {
	LOG_ERR("GATT read failed (err %d)", err);
	return -6;
	}

	LOG_HEXDUMP_DBG(keymat.session_key, BT_EAD_KEY_SIZE, "Session Key");
	LOG_HEXDUMP_DBG(keymat.iv, BT_EAD_IV_SIZE, "IV");

	if (test_received_keymat != NULL) {
	memcpy(test_received_keymat, &keymat, sizeof(keymat));
	}

	/* Start a new scan to get and decrypt the Advertising Data */
	err = bt_conn_disconnect(default_conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
	if (err) {
	LOG_ERR("Failed to disconnect.");
	return -7;
	}

	connect = false;
	err = start_scan(connect);
	if (err) {
	return -2;
	}

	return 0;
	}