From 5bf4196468fe39d7631c3fcc3b8cfab1637dfe47 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Mon, 13 Jan 2014 22:57:14 -0800 Subject: [PATCH] shared: Add address and features configuration support --- tools/bluemoon.c | 290 +++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 279 insertions(+), 11 deletions(-) diff --git a/tools/bluemoon.c b/tools/bluemoon.c index f23df8a35..b9e4dce77 100644 --- a/tools/bluemoon.c +++ b/tools/bluemoon.c @@ -36,8 +36,8 @@ #include "src/shared/util.h" #include "src/shared/hci.h" -#define CMD_BLUEMOON_READ_VERSION 0xfc05 -struct rsp_bluemoon_read_version { +#define CMD_READ_VERSION 0xfc05 +struct rsp_read_version { uint8_t status; uint8_t hw_platform; uint8_t hw_variant; @@ -50,12 +50,247 @@ struct rsp_bluemoon_read_version { uint8_t fw_patch; } __attribute__ ((packed)); -static struct bt_hci *hci_dev; +#define CMD_MANUFACTURER_MODE 0xfc11 +struct cmd_manufacturer_mode { + uint8_t mode_switch; + uint8_t reset; +} __attribute__ ((packed)); -static void bluemoon_read_version_complete(const void *data, uint8_t size, +#define CMD_WRITE_BD_DATA 0xfc2f +struct cmd_write_bd_data { + uint8_t bdaddr[6]; + uint8_t reserved1[6]; + uint8_t features[8]; + uint8_t le_features; + uint8_t reserved2[32]; + uint8_t lmp_version; + uint8_t reserved3[26]; +} __attribute__ ((packed)); + +#define CMD_READ_BD_DATA 0xfc30 +struct rsp_read_bd_data { + uint8_t status; + uint8_t bdaddr[6]; + uint8_t reserved1[6]; + uint8_t features[8]; + uint8_t le_features; + uint8_t reserved2[32]; + uint8_t lmp_version; + uint8_t reserved3[26]; +} __attribute__ ((packed)); + +#define CMD_WRITE_BD_ADDRESS 0xfc31 +struct cmd_write_bd_address { + uint8_t bdaddr[6]; +} __attribute__ ((packed)); + +static struct bt_hci *hci_dev; +static uint16_t hci_index = 0; + +static bool set_bdaddr = false; +static const char *set_bdaddr_value = NULL; + +static bool reset_on_exit = false; +static bool use_manufacturer_mode = false; +static bool get_bddata = false; + +static void reset_complete(const void *data, uint8_t size, void *user_data) +{ + uint8_t status = *((uint8_t *) data); + + if (status) { + fprintf(stderr, "Failed to reset (0x%02x)\n", status); + mainloop_quit(); + return; + } + + mainloop_quit(); +} + +static void leave_manufacturer_mode_complete(const void *data, uint8_t size, void *user_data) { - const struct rsp_bluemoon_read_version *rsp = data; + uint8_t status = *((uint8_t *) data); + + if (status) { + fprintf(stderr, "Failed to leave manufacturer mode (0x%02x)\n", + status); + mainloop_quit(); + return; + } + + if (reset_on_exit) { + bt_hci_send(hci_dev, BT_HCI_CMD_RESET, NULL, 0, + reset_complete, NULL, NULL); + return; + } + + mainloop_quit(); +} + +static void shutdown_device(void) +{ + bt_hci_flush(hci_dev); + + if (use_manufacturer_mode) { + struct cmd_manufacturer_mode cmd; + + cmd.mode_switch = 0x00; + cmd.reset = 0x00; + + bt_hci_send(hci_dev, CMD_MANUFACTURER_MODE, &cmd, sizeof(cmd), + leave_manufacturer_mode_complete, NULL, NULL); + return; + } + + if (reset_on_exit) { + bt_hci_send(hci_dev, BT_HCI_CMD_RESET, NULL, 0, + reset_complete, NULL, NULL); + return; + } + + mainloop_quit(); +} + +static void write_bd_address_complete(const void *data, uint8_t size, + void *user_data) +{ + uint8_t status = *((uint8_t *) data); + + if (status) { + fprintf(stderr, "Failed to write address (0x%02x)\n", status); + mainloop_quit(); + return; + } + + shutdown_device(); +} + +static void read_bd_addr_complete(const void *data, uint8_t size, + void *user_data) +{ + const struct bt_hci_rsp_read_bd_addr *rsp = data; + struct cmd_write_bd_address cmd; + + if (rsp->status) { + fprintf(stderr, "Failed to read address (0x%02x)\n", + rsp->status); + mainloop_quit(); + shutdown_device(); + return; + } + + if (set_bdaddr_value) { + fprintf(stderr, "Setting address is not supported\n"); + mainloop_quit(); + return; + } + + printf("Controller Address\n"); + printf("\tOld BD_ADDR: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n", + rsp->bdaddr[5], rsp->bdaddr[4], + rsp->bdaddr[3], rsp->bdaddr[2], + rsp->bdaddr[1], rsp->bdaddr[0]); + + memcpy(cmd.bdaddr, rsp->bdaddr, 6); + cmd.bdaddr[0] = (hci_index & 0xff); + + printf("\tNew BD_ADDR: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n", + cmd.bdaddr[5], cmd.bdaddr[4], + cmd.bdaddr[3], cmd.bdaddr[2], + cmd.bdaddr[1], cmd.bdaddr[0]); + + bt_hci_send(hci_dev, CMD_WRITE_BD_ADDRESS, &cmd, sizeof(cmd), + write_bd_address_complete, NULL, NULL); +} + +static void write_bd_data_complete(const void *data, uint8_t size, + void *user_data) +{ + uint8_t status = *((uint8_t *) data); + + if (status) { + fprintf(stderr, "Failed to write data (0x%02x)\n", status); + shutdown_device(); + return; + } + + shutdown_device(); +} + +static void read_bd_data_complete(const void *data, uint8_t size, + void *user_data) +{ + const struct rsp_read_bd_data *rsp = data; + + if (rsp->status) { + fprintf(stderr, "Failed to read data (0x%02x)\n", rsp->status); + shutdown_device(); + return; + } + + printf("Controller Data\n"); + printf("\tBD_ADDR: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n", + rsp->bdaddr[5], rsp->bdaddr[4], + rsp->bdaddr[3], rsp->bdaddr[2], + rsp->bdaddr[1], rsp->bdaddr[0]); + + printf("\tLMP Version: %u\n", rsp->lmp_version); + printf("\tLMP Features: 0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x" + " 0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x\n", + rsp->features[0], rsp->features[1], + rsp->features[2], rsp->features[3], + rsp->features[4], rsp->features[5], + rsp->features[6], rsp->features[7]); + printf("\tLE Features: 0x%2.2x\n", rsp->le_features); + + if (set_bdaddr) { + struct cmd_write_bd_data cmd; + + memcpy(cmd.bdaddr, rsp->bdaddr, 6); + cmd.bdaddr[0] = (hci_index & 0xff); + cmd.lmp_version = 0x07; + memcpy(cmd.features, rsp->features, 8); + cmd.features[0] &= ~(0x01 | 0x02); + cmd.le_features = rsp->le_features; + cmd.le_features |= 0x1e; + memcpy(cmd.reserved1, rsp->reserved1, sizeof(cmd.reserved1)); + memcpy(cmd.reserved2, rsp->reserved2, sizeof(cmd.reserved2)); + memcpy(cmd.reserved3, rsp->reserved3, sizeof(cmd.reserved3)); + + bt_hci_send(hci_dev, CMD_WRITE_BD_DATA, &cmd, sizeof(cmd), + write_bd_data_complete, NULL, NULL); + return; + } + + shutdown_device(); +} + +static void enter_manufacturer_mode_complete(const void *data, uint8_t size, + void *user_data) +{ + uint8_t status = *((uint8_t *) data); + + if (status) { + fprintf(stderr, "Failed to enter manufacturer mode (0x%02x)\n", + status); + mainloop_quit(); + return; + } + + if (get_bddata || set_bdaddr) { + bt_hci_send(hci_dev, CMD_READ_BD_DATA, NULL, 0, + read_bd_data_complete, NULL, NULL); + return; + } + + shutdown_device(); +} + +static void read_version_complete(const void *data, uint8_t size, + void *user_data) +{ + const struct rsp_read_version *rsp = data; const char *str; if (rsp->status) { @@ -65,6 +300,23 @@ static void bluemoon_read_version_complete(const void *data, uint8_t size, return; } + if (use_manufacturer_mode) { + struct cmd_manufacturer_mode cmd; + + cmd.mode_switch = 0x01; + cmd.reset = 0x00; + + bt_hci_send(hci_dev, CMD_MANUFACTURER_MODE, &cmd, sizeof(cmd), + enter_manufacturer_mode_complete, NULL, NULL); + return; + } + + if (set_bdaddr) { + bt_hci_send(hci_dev, BT_HCI_CMD_READ_BD_ADDR, NULL, 0, + read_bd_addr_complete, NULL, NULL); + return; + } + printf("Controller Version Information\n"); printf("\tHardware Platform:\t%u\n", rsp->hw_platform); @@ -125,8 +377,8 @@ static void read_local_version_complete(const void *data, uint8_t size, return; } - bt_hci_send(hci_dev, CMD_BLUEMOON_READ_VERSION, NULL, 0, - bluemoon_read_version_complete, NULL, NULL); + bt_hci_send(hci_dev, CMD_READ_VERSION, NULL, 0, + read_version_complete, NULL, NULL); } static void signal_callback(int signum, void *user_data) @@ -145,11 +397,16 @@ static void usage(void) "Usage:\n"); printf("\tbluemoon [options]\n"); printf("Options:\n" + "\t-B, --bdaddr [addr] Set Bluetooth address\n" + "\t-R, --reset Reset controller\n" "\t-i, --index Use specified controller\n" "\t-h, --help Show help options\n"); } static const struct option main_options[] = { + { "bdaddr", optional_argument, NULL, 'A' }, + { "bddata", no_argument, NULL, 'D' }, + { "reset", no_argument, NULL, 'R' }, { "index", required_argument, NULL, 'i' }, { "version", no_argument, NULL, 'v' }, { "help", no_argument, NULL, 'h' }, @@ -158,7 +415,6 @@ static const struct option main_options[] = { int main(int argc, char *argv[]) { - uint16_t index = 0; const char *str; sigset_t mask; int exit_status; @@ -166,11 +422,23 @@ int main(int argc, char *argv[]) for (;;) { int opt; - opt = getopt_long(argc, argv, "i:vh", main_options, NULL); + opt = getopt_long(argc, argv, "A::DRi:vh", main_options, NULL); if (opt < 0) break; switch (opt) { + case 'A': + if (optarg) + set_bdaddr_value = optarg; + set_bdaddr = true; + break; + case 'D': + use_manufacturer_mode = true; + get_bddata = true; + break; + case 'R': + reset_on_exit = true; + break; case 'i': if (strlen(optarg) > 3 && !strncmp(optarg, "hci", 3)) str = optarg + 3; @@ -180,7 +448,7 @@ int main(int argc, char *argv[]) usage(); return EXIT_FAILURE; } - index = atoi(str); + hci_index = atoi(str); break; case 'v': printf("%s\n", VERSION); @@ -208,7 +476,7 @@ int main(int argc, char *argv[]) printf("Bluemoon configuration utility ver %s\n", VERSION); - hci_dev = bt_hci_new_user_channel(index); + hci_dev = bt_hci_new_user_channel(hci_index); if (!hci_dev) { fprintf(stderr, "Failed to open HCI user channel\n"); return EXIT_FAILURE;