bluez5_utils-5.62/peripheral/gap.c

// SPDX-License-Identifier: LGPL-2.1-or-later
/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2015  Intel Corporation. All rights reserved.
 *
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdio.h>
#include <string.h>

#include "lib/bluetooth.h"
#include "lib/mgmt.h"
#include "src/shared/util.h"
#include "src/shared/mgmt.h"
#include "peripheral/gatt.h"
#include "peripheral/gap.h"

static struct mgmt *mgmt = NULL;
static uint16_t mgmt_index = MGMT_INDEX_NONE;

static bool adv_features = false;
static bool adv_instances = false;
static bool require_connectable = true;

static uint8_t static_addr[6] = { 0x90, 0x78, 0x56, 0x34, 0x12, 0xc0 };
static uint8_t dev_name[260] = { 0x00, };
static uint8_t dev_name_len = 0;

void gap_set_static_address(uint8_t addr[6])
{
	memcpy(static_addr, addr, sizeof(static_addr));

	printf("Using static address %02x:%02x:%02x:%02x:%02x:%02x\n",
			static_addr[5], static_addr[4], static_addr[3],
			static_addr[2], static_addr[1], static_addr[0]);
}

static void clear_long_term_keys(uint16_t index)
{
	struct mgmt_cp_load_long_term_keys cp;

	memset(&cp, 0, sizeof(cp));
	cp.key_count = cpu_to_le16(0);

	mgmt_send(mgmt, MGMT_OP_LOAD_LONG_TERM_KEYS, index,
					sizeof(cp), &cp, NULL, NULL, NULL);
}

static void clear_identity_resolving_keys(uint16_t index)
{
	struct mgmt_cp_load_irks cp;

	memset(&cp, 0, sizeof(cp));
	cp.irk_count = cpu_to_le16(0);

	mgmt_send(mgmt, MGMT_OP_LOAD_IRKS, index,
					sizeof(cp), &cp, NULL, NULL, NULL);
}

static void add_advertising(uint16_t index)
{
	const char ad[] = { 0x11, 0x15,
			0xd0, 0x00, 0x2d, 0x12, 0x1e, 0x4b, 0x0f, 0xa4,
			0x99, 0x4e, 0xce, 0xb5, 0x31, 0xf4, 0x05, 0x79 };
	struct mgmt_cp_add_advertising *cp;
	void *buf;

	buf = malloc(sizeof(*cp) + sizeof(ad));
	if (!buf)
		return;

	memset(buf, 0, sizeof(*cp) + sizeof(ad));
	cp = buf;
	cp->instance = 0x01;
	cp->flags = cpu_to_le32((1 << 0) | (1 << 1) | (1 << 4));
	cp->duration = cpu_to_le16(0);
	cp->timeout = cpu_to_le16(0);
	cp->adv_data_len = sizeof(ad);
	cp->scan_rsp_len = 0;
	memcpy(cp->data, ad, sizeof(ad));

	mgmt_send(mgmt, MGMT_OP_ADD_ADVERTISING, index,
			sizeof(*cp) + sizeof(ad), buf, NULL, NULL, NULL);

	free(buf);
}

static void enable_advertising(uint16_t index)
{
	uint8_t val;

	val = require_connectable ? 0x01 : 0x00;
	mgmt_send(mgmt, MGMT_OP_SET_CONNECTABLE, index, 1, &val,
						NULL, NULL, NULL);

	val = 0x01;
	mgmt_send(mgmt, MGMT_OP_SET_POWERED, index, 1, &val,
						NULL, NULL, NULL);

	if (adv_instances) {
		add_advertising(index);
		return;
	}

	val = require_connectable ? 0x01 : 0x02;
	mgmt_send(mgmt, MGMT_OP_SET_ADVERTISING, index, 1, &val,
						NULL, NULL, NULL);
}

static void new_settings_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("New settings\n");
}

static void local_name_changed_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Local name changed\n");
}

static void new_long_term_key_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("New long term key\n");
}

static void device_connected_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Device connected\n");
}

static void device_disconnected_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Device disconnected\n");
}

static void user_confirm_request_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("User confirm request\n");
}

static void user_passkey_request_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("User passkey request\n");
}

static void auth_failed_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Authentication failed\n");
}

static void device_unpaired_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Device unpaired\n");
}

static void passkey_notify_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Passkey notification\n");
}

static void new_irk_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("New identify resolving key\n");
}

static void new_csrk_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("New connection signature resolving key\n");
}

static void new_conn_param_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("New connection parameter\n");
}

static void advertising_added_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Advertising added\n");
}

static void advertising_removed_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Advertising removed\n");
}

static void read_adv_features_complete(uint8_t status, uint16_t len,
					const void *param, void *user_data)
{
	const struct mgmt_rp_read_adv_features *rp = param;
	uint16_t index = PTR_TO_UINT(user_data);
	uint32_t flags;

	flags = le32_to_cpu(rp->supported_flags);

	if (rp->max_instances > 0) {
		adv_instances = true;

		if (flags & (1 << 0))
			require_connectable = false;
	} else
		require_connectable = false;

	enable_advertising(index);
}

static void read_info_complete(uint8_t status, uint16_t len,
					const void *param, void *user_data)
{
	const struct mgmt_rp_read_info *rp = param;
	uint16_t index = PTR_TO_UINT(user_data);
	uint32_t required_settings = MGMT_SETTING_LE |
					MGMT_SETTING_STATIC_ADDRESS;
	uint32_t supported_settings, current_settings;
	uint8_t val;

	required_settings = MGMT_SETTING_LE;

	if (status) {
		fprintf(stderr, "Reading info for index %u failed: %s\n",
						index, mgmt_errstr(status));
		return;
	}

	if (mgmt_index != MGMT_INDEX_NONE)
		return;

	supported_settings = le32_to_cpu(rp->supported_settings);
	current_settings = le32_to_cpu(rp->current_settings);

	if ((supported_settings & required_settings) != required_settings)
		return;

	printf("Selecting index %u\n", index);
	mgmt_index = index;

	mgmt_register(mgmt, MGMT_EV_NEW_SETTINGS, index,
					new_settings_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_LOCAL_NAME_CHANGED, index,
					local_name_changed_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_NEW_LONG_TERM_KEY, index,
					new_long_term_key_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_DEVICE_CONNECTED, index,
					device_connected_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_DEVICE_DISCONNECTED, index,
					device_disconnected_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_USER_CONFIRM_REQUEST, index,
					user_confirm_request_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_USER_PASSKEY_REQUEST, index,
					user_passkey_request_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_AUTH_FAILED, index,
					auth_failed_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_DEVICE_UNPAIRED, index,
					device_unpaired_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_PASSKEY_NOTIFY, index,
					passkey_notify_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_NEW_IRK, index,
					new_irk_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_NEW_CSRK, index,
					new_csrk_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_NEW_CONN_PARAM, index,
					new_conn_param_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_ADVERTISING_ADDED, index,
					advertising_added_event, NULL, NULL);
	mgmt_register(mgmt, MGMT_EV_ADVERTISING_REMOVED, index,
					advertising_removed_event, NULL, NULL);

	dev_name_len = snprintf((char *) dev_name, 26, "BlueZ Peripheral");

	if (current_settings & MGMT_SETTING_POWERED) {
		val = 0x00;
		mgmt_send(mgmt, MGMT_OP_SET_POWERED, index, 1, &val,
							NULL, NULL, NULL);
	}

	if (!(current_settings & MGMT_SETTING_LE)) {
		val = 0x01;
		mgmt_send(mgmt, MGMT_OP_SET_LE, index, 1, &val,
							NULL, NULL, NULL);
	}

	if (current_settings & MGMT_SETTING_BREDR) {
		val = 0x00;
		mgmt_send(mgmt, MGMT_OP_SET_BREDR, index, 1, &val,
							NULL, NULL, NULL);
	}

	if ((supported_settings & MGMT_SETTING_SECURE_CONN) &&
			!(current_settings & MGMT_SETTING_SECURE_CONN)) {
		val = 0x01;
		mgmt_send(mgmt, MGMT_OP_SET_SECURE_CONN, index, 1, &val,
							NULL, NULL, NULL);
	}

	if (current_settings & MGMT_SETTING_DEBUG_KEYS) {
		val = 0x00;
		mgmt_send(mgmt, MGMT_OP_SET_DEBUG_KEYS, index, 1, &val,
							NULL, NULL, NULL);
	}

	if (!(current_settings & MGMT_SETTING_BONDABLE)) {
		val = 0x01;
		mgmt_send(mgmt, MGMT_OP_SET_BONDABLE, index, 1, &val,
							NULL, NULL, NULL);
	}

	clear_long_term_keys(mgmt_index);
	clear_identity_resolving_keys(mgmt_index);

	mgmt_send(mgmt, MGMT_OP_SET_STATIC_ADDRESS, index,
					6, static_addr, NULL, NULL, NULL);

	mgmt_send(mgmt, MGMT_OP_SET_LOCAL_NAME, index,
					260, dev_name, NULL, NULL, NULL);

	gatt_set_static_address(static_addr);
	gatt_set_device_name(dev_name, dev_name_len);
	gatt_server_start();

	if (adv_features)
		mgmt_send(mgmt, MGMT_OP_READ_ADV_FEATURES, index, 0, NULL,
						read_adv_features_complete,
						UINT_TO_PTR(index), NULL);
	else
		enable_advertising(index);
}

static void read_index_list_complete(uint8_t status, uint16_t len,
					const void *param, void *user_data)
{
	const struct mgmt_rp_read_index_list *rp = param;
	uint16_t count;
	int i;

	if (status) {
		fprintf(stderr, "Reading index list failed: %s\n",
						mgmt_errstr(status));
		return;
	}

	count = le16_to_cpu(rp->num_controllers);

	printf("Index list: %u\n", count);

	for (i = 0; i < count; i++) {
		uint16_t index = cpu_to_le16(rp->index[i]);

		mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
				read_info_complete, UINT_TO_PTR(index), NULL);
	}
}

static void index_added_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Index added\n");

	if (mgmt_index != MGMT_INDEX_NONE)
		return;

	mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
				read_info_complete, UINT_TO_PTR(index), NULL);
}

static void index_removed_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	printf("Index removed\n");

	if (mgmt_index != index)
		return;

	mgmt_index = MGMT_INDEX_NONE;
}

static void read_ext_index_list_complete(uint8_t status, uint16_t len,
					const void *param, void *user_data)
{
	const struct mgmt_rp_read_ext_index_list *rp = param;
	uint16_t count;
	int i;

	if (status) {
		fprintf(stderr, "Reading extended index list failed: %s\n",
						mgmt_errstr(status));
		return;
	}

	count = le16_to_cpu(rp->num_controllers);

	printf("Extended index list: %u\n", count);

	for (i = 0; i < count; i++) {
		uint16_t index = cpu_to_le16(rp->entry[i].index);

		if (rp->entry[i].type != 0x00)
			continue;

		mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
				read_info_complete, UINT_TO_PTR(index), NULL);
	}
}

static void ext_index_added_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	const struct mgmt_ev_ext_index_added *ev = param;

	printf("Extended index added: %u\n", ev->type);

	if (mgmt_index != MGMT_INDEX_NONE)
		return;

	if (ev->type != 0x00)
		return;

	mgmt_send(mgmt, MGMT_OP_READ_INFO, index, 0, NULL,
				read_info_complete, UINT_TO_PTR(index), NULL);
}

static void ext_index_removed_event(uint16_t index, uint16_t length,
					const void *param, void *user_data)
{
	const struct mgmt_ev_ext_index_added *ev = param;

	printf("Extended index removed: %u\n", ev->type);

	if (mgmt_index != index)
		return;

	if (ev->type != 0x00)
		return;

	mgmt_index = MGMT_INDEX_NONE;
}

static void read_commands_complete(uint8_t status, uint16_t len,
					const void *param, void *user_data)
{
	const struct mgmt_rp_read_commands *rp = param;
	uint16_t num_commands;
	bool ext_index_list = false;
	int i;

	if (status) {
		fprintf(stderr, "Reading index list failed: %s\n",
						mgmt_errstr(status));
		return;
	}

	num_commands = le16_to_cpu(rp->num_commands);

	for (i = 0; i < num_commands; i++) {
		uint16_t op = get_le16(rp->opcodes + 1);

		if (op == MGMT_OP_READ_EXT_INDEX_LIST)
			ext_index_list = true;
		else if (op == MGMT_OP_READ_ADV_FEATURES)
			adv_features = true;
	}

	if (ext_index_list) {
		mgmt_register(mgmt, MGMT_EV_EXT_INDEX_ADDED, MGMT_INDEX_NONE,
					ext_index_added_event, NULL, NULL);
		mgmt_register(mgmt, MGMT_EV_EXT_INDEX_REMOVED, MGMT_INDEX_NONE,
					ext_index_removed_event, NULL, NULL);

		if (!mgmt_send(mgmt, MGMT_OP_READ_EXT_INDEX_LIST,
				MGMT_INDEX_NONE, 0, NULL,
				read_ext_index_list_complete, NULL, NULL)) {
			fprintf(stderr, "Failed to read extended index list\n");
			return;
		}
	} else {
		mgmt_register(mgmt, MGMT_EV_INDEX_ADDED, MGMT_INDEX_NONE,
					index_added_event, NULL, NULL);
		mgmt_register(mgmt, MGMT_EV_INDEX_REMOVED, MGMT_INDEX_NONE,
					index_removed_event, NULL, NULL);

		if (!mgmt_send(mgmt, MGMT_OP_READ_INDEX_LIST,
				MGMT_INDEX_NONE, 0, NULL,
				read_index_list_complete, NULL, NULL)) {
			fprintf(stderr, "Failed to read index list\n");
			return;
		}
	}
}

void gap_start(void)
{
	mgmt = mgmt_new_default();
	if (!mgmt) {
		fprintf(stderr, "Failed to open management socket\n");
		return;
	}

	if (!mgmt_send(mgmt, MGMT_OP_READ_COMMANDS,
				MGMT_INDEX_NONE, 0, NULL,
				read_commands_complete, NULL, NULL)) {
		fprintf(stderr, "Failed to read supported commands\n");
		return;
	}
}

void gap_stop(void)
{
	if (!mgmt)
		return;

	gatt_server_stop();

        mgmt_unref(mgmt);
	mgmt = NULL;

	mgmt_index = MGMT_INDEX_NONE;
}