/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2011-2012 Intel Corporation
* Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <getopt.h>
#include "lib/bluetooth.h"
#include "lib/mgmt.h"
#include "monitor/bt.h"
#include "src/shared/mainloop.h"
#include "src/shared/util.h"
#include "src/shared/mgmt.h"
#include "src/shared/hci.h"
#include "src/shared/crypto.h"
#define PEER_ADDR_TYPE 0x00
#define PEER_ADDR "\x00\x00\x00\x00\x00\x00"
#define ADV_IRK "\x69\x30\xde\xc3\x8f\x84\x74\x14" \
"\xe1\x23\x99\xc1\xca\x9a\xc3\x31"
#define SCAN_IRK "\xfa\x73\x09\x11\x3f\x03\x37\x0f" \
"\xf4\xf9\x93\x1e\xf9\xa3\x63\xa6"
static struct mgmt *mgmt;
static uint16_t index1 = MGMT_INDEX_NONE;
static uint16_t index2 = MGMT_INDEX_NONE;
static struct bt_crypto *crypto;
static struct bt_hci *adv_dev;
static struct bt_hci *scan_dev;
static void print_rpa(const uint8_t addr[6])
{
printf(" Address: %02x:%02x:%02x:%02x:%02x:%02x\n",
addr[5], addr[4], addr[3],
addr[2], addr[1], addr[0]);
printf(" Random: %02x%02x%02x\n", addr[3], addr[4], addr[5]);
printf(" Hash: %02x%02x%02x\n", addr[0], addr[1], addr[2]);
}
static void scan_le_adv_report(const void *data, uint8_t size,
void *user_data)
{
const struct bt_hci_evt_le_adv_report *evt = data;
if (evt->addr_type == 0x01 && (evt->addr[5] & 0xc0) == 0x40) {
uint8_t hash[3], irk[16];
memcpy(irk, ADV_IRK, 16);
bt_crypto_ah(crypto, irk, evt->addr + 3, hash);
if (!memcmp(evt->addr, hash, 3)) {
printf("Received advertising report\n");
print_rpa(evt->addr);
memcpy(irk, ADV_IRK, 16);
bt_crypto_ah(crypto, irk, evt->addr + 3, hash);
printf(" -> Computed hash: %02x%02x%02x\n",
hash[0], hash[1], hash[2]);
mainloop_quit();
}
}
}
static void scan_le_meta_event(const void *data, uint8_t size,
void *user_data)
{
uint8_t evt_code = ((const uint8_t *) data)[0];
switch (evt_code) {
case BT_HCI_EVT_LE_ADV_REPORT:
scan_le_adv_report(data + 1, size - 1, user_data);
break;
}
}
static void scan_enable_callback(const void *data, uint8_t size,
void *user_data)
{
}
static void adv_enable_callback(const void *data, uint8_t size,
void *user_data)
{
struct bt_hci_cmd_le_set_scan_parameters cmd4;
struct bt_hci_cmd_le_set_scan_enable cmd5;
cmd4.type = 0x00; /* Passive scanning */
cmd4.interval = cpu_to_le16(0x0010);
cmd4.window = cpu_to_le16(0x0010);
cmd4.own_addr_type = 0x00; /* Use public address */
cmd4.filter_policy = 0x00;
bt_hci_send(scan_dev, BT_HCI_CMD_LE_SET_SCAN_PARAMETERS,
&cmd4, sizeof(cmd4), NULL, NULL, NULL);
cmd5.enable = 0x01;
cmd5.filter_dup = 0x01;
bt_hci_send(scan_dev, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
&cmd5, sizeof(cmd5),
scan_enable_callback, NULL, NULL);
}
static void adv_le_evtmask_callback(const void *data, uint8_t size,
void *user_data)
{
struct bt_hci_cmd_le_set_resolv_timeout cmd0;
struct bt_hci_cmd_le_add_to_resolv_list cmd1;
struct bt_hci_cmd_le_set_resolv_enable cmd2;
struct bt_hci_cmd_le_set_random_address cmd3;
struct bt_hci_cmd_le_set_adv_parameters cmd4;
struct bt_hci_cmd_le_set_adv_enable cmd5;
cmd0.timeout = cpu_to_le16(0x0384);
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_RESOLV_TIMEOUT,
&cmd0, sizeof(cmd0), NULL, NULL, NULL);
cmd1.addr_type = PEER_ADDR_TYPE;
memcpy(cmd1.addr, PEER_ADDR, 6);
memset(cmd1.peer_irk, 0, 16);
memcpy(cmd1.local_irk, ADV_IRK, 16);
bt_hci_send(adv_dev, BT_HCI_CMD_LE_ADD_TO_RESOLV_LIST,
&cmd1, sizeof(cmd1), NULL, NULL, NULL);
cmd2.enable = 0x01;
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_RESOLV_ENABLE,
&cmd2, sizeof(cmd2), NULL, NULL, NULL);
bt_crypto_random_bytes(crypto, cmd3.addr + 3, 3);
cmd3.addr[5] &= 0x3f; /* Clear two most significant bits */
cmd3.addr[5] |= 0x40; /* Set second most significant bit */
bt_crypto_ah(crypto, cmd1.local_irk, cmd3.addr + 3, cmd3.addr);
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
&cmd3, sizeof(cmd3), NULL, NULL, NULL);
printf("Setting advertising address\n");
print_rpa(cmd3.addr);
cmd4.min_interval = cpu_to_le16(0x0800);
cmd4.max_interval = cpu_to_le16(0x0800);
cmd4.type = 0x03; /* Non-connectable advertising */
cmd4.own_addr_type = 0x03; /* Local IRK, random address fallback */
cmd4.direct_addr_type = PEER_ADDR_TYPE;
memcpy(cmd4.direct_addr, PEER_ADDR, 6);
cmd4.channel_map = 0x07;
cmd4.filter_policy = 0x00;
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
&cmd4, sizeof(cmd4), NULL, NULL, NULL);
cmd5.enable = 0x01;
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_ADV_ENABLE,
&cmd5, sizeof(cmd5),
adv_enable_callback, NULL, NULL);
}
static void adv_le_features_callback(const void *data, uint8_t size,
void *user_data)
{
const struct bt_hci_rsp_le_read_local_features *rsp = data;
uint8_t evtmask[] = { 0xff, 0xff, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00 };
if (rsp->status) {
fprintf(stderr, "Failed to read local LE features\n");
mainloop_exit_failure();
return;
}
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_EVENT_MASK, evtmask, 8,
adv_le_evtmask_callback, NULL, NULL);
}
static void adv_features_callback(const void *data, uint8_t size,
void *user_data)
{
const struct bt_hci_rsp_read_local_features *rsp = data;
uint8_t evtmask[] = { 0x90, 0xe8, 0x04, 0x02, 0x00, 0x80, 0x00, 0x20 };
if (rsp->status) {
fprintf(stderr, "Failed to read local features\n");
mainloop_exit_failure();
return;
}
if (!(rsp->features[4] & 0x40)) {
fprintf(stderr, "Controller without Low Energy support\n");
mainloop_exit_failure();
return;
}
bt_hci_send(adv_dev, BT_HCI_CMD_SET_EVENT_MASK, evtmask, 8,
NULL, NULL, NULL);
bt_hci_send(adv_dev, BT_HCI_CMD_LE_READ_LOCAL_FEATURES, NULL, 0,
adv_le_features_callback, NULL, NULL);
}
static void scan_le_evtmask_callback(const void *data, uint8_t size,
void *user_data)
{
bt_hci_send(adv_dev, BT_HCI_CMD_RESET, NULL, 0, NULL, NULL, NULL);
bt_hci_send(adv_dev, BT_HCI_CMD_READ_LOCAL_FEATURES, NULL, 0,
adv_features_callback, NULL, NULL);
}
static void scan_le_features_callback(const void *data, uint8_t size,
void *user_data)
{
const struct bt_hci_rsp_le_read_local_features *rsp = data;
uint8_t evtmask[] = { 0xff, 0xff, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00 };
if (rsp->status) {
fprintf(stderr, "Failed to read local LE features\n");
mainloop_exit_failure();
return;
}
bt_hci_send(adv_dev, BT_HCI_CMD_LE_SET_EVENT_MASK, evtmask, 8,
scan_le_evtmask_callback, NULL, NULL);
}
static void scan_features_callback(const void *data, uint8_t size,
void *user_data)
{
const struct bt_hci_rsp_read_local_features *rsp = data;
uint8_t evtmask[] = { 0x90, 0xe8, 0x04, 0x02, 0x00, 0x80, 0x00, 0x20 };
if (rsp->status) {
fprintf(stderr, "Failed to read local features\n");
mainloop_exit_failure();
return;
}
if (!(rsp->features[4] & 0x40)) {
fprintf(stderr, "Controller without Low Energy support\n");
mainloop_exit_failure();
return;
}
bt_hci_send(scan_dev, BT_HCI_CMD_SET_EVENT_MASK, evtmask, 8,
NULL, NULL, NULL);
bt_hci_send(scan_dev, BT_HCI_CMD_LE_READ_LOCAL_FEATURES, NULL, 0,
scan_le_features_callback, NULL, NULL);
}
static void read_index_list(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));
mainloop_exit_failure();
return;
}
count = le16_to_cpu(rp->num_controllers);
if (count < 2) {
fprintf(stderr, "At least 2 controllers are required\n");
mainloop_exit_failure();
return;
}
for (i = 0; i < count; i++) {
uint16_t index = cpu_to_le16(rp->index[i]);
if (index < index1)
index1 = index;
}
for (i = 0; i < count; i++) {
uint16_t index = cpu_to_le16(rp->index[i]);
if (index < index2 && index > index1)
index2 = index;
}
printf("Selecting index %u for advertiser\n", index1);
printf("Selecting index %u for scanner\n", index2);
crypto = bt_crypto_new();
if (!crypto) {
fprintf(stderr, "Failed to open crypto interface\n");
mainloop_exit_failure();
return;
}
adv_dev = bt_hci_new_user_channel(index1);
if (!adv_dev) {
fprintf(stderr, "Failed to open HCI for advertiser\n");
mainloop_exit_failure();
return;
}
scan_dev = bt_hci_new_user_channel(index2);
if (!scan_dev) {
fprintf(stderr, "Failed to open HCI for scanner\n");
mainloop_exit_failure();
return;
}
bt_hci_register(scan_dev, BT_HCI_EVT_LE_META_EVENT,
scan_le_meta_event, NULL, NULL);
bt_hci_send(scan_dev, BT_HCI_CMD_RESET, NULL, 0, NULL, NULL, NULL);
bt_hci_send(scan_dev, BT_HCI_CMD_READ_LOCAL_FEATURES, NULL, 0,
scan_features_callback, NULL, NULL);
}
static void signal_callback(int signum, void *user_data)
{
switch (signum) {
case SIGINT:
case SIGTERM:
mainloop_quit();
break;
}
}
static void usage(void)
{
printf("advtest - Advertising testing\n"
"Usage:\n");
printf("\tadvtest [options]\n");
printf("options:\n"
"\t-h, --help Show help options\n");
}
static const struct option main_options[] = {
{ "version", no_argument, NULL, 'v' },
{ "help", no_argument, NULL, 'h' },
{ }
};
int main(int argc ,char *argv[])
{
int exit_status;
for (;;) {
int opt;
opt = getopt_long(argc, argv, "vh", main_options, NULL);
if (opt < 0)
break;
switch (opt) {
case 'v':
printf("%s\n", VERSION);
return EXIT_SUCCESS;
case 'h':
usage();
return EXIT_SUCCESS;
default:
return EXIT_FAILURE;
}
}
if (argc - optind > 0) {
fprintf(stderr, "Invalid command line parameters\n");
return EXIT_FAILURE;
}
mainloop_init();
mgmt = mgmt_new_default();
if (!mgmt) {
fprintf(stderr, "Failed to open management socket\n");
return EXIT_FAILURE;
}
if (!mgmt_send(mgmt, MGMT_OP_READ_INDEX_LIST,
MGMT_INDEX_NONE, 0, NULL,
read_index_list, NULL, NULL)) {
fprintf(stderr, "Failed to read index list\n");
exit_status = EXIT_FAILURE;
goto done;
}
exit_status = mainloop_run_with_signal(signal_callback, NULL);
bt_hci_unref(adv_dev);
bt_hci_unref(scan_dev);
bt_crypto_unref(crypto);
done:
mgmt_unref(mgmt);
return exit_status;
}