From 3743bf0203ddab7051be5c5452242831f0711db9 Mon Sep 17 00:00:00 2001 From: takawata Date: Sat, 18 Apr 2015 04:53:17 +0000 Subject: Add LE related HCI control command to hccontrol(1). --- usr.sbin/bluetooth/hccontrol/Makefile | 2 +- usr.sbin/bluetooth/hccontrol/hccontrol.c | 6 + usr.sbin/bluetooth/hccontrol/hccontrol.h | 1 + .../bluetooth/hccontrol/host_controller_baseband.c | 83 +++++ usr.sbin/bluetooth/hccontrol/le.c | 352 +++++++++++++++++++++ 5 files changed, 443 insertions(+), 1 deletion(-) create mode 100644 usr.sbin/bluetooth/hccontrol/le.c (limited to 'usr.sbin/bluetooth/hccontrol') diff --git a/usr.sbin/bluetooth/hccontrol/Makefile b/usr.sbin/bluetooth/hccontrol/Makefile index 61206df..a81fda4 100644 --- a/usr.sbin/bluetooth/hccontrol/Makefile +++ b/usr.sbin/bluetooth/hccontrol/Makefile @@ -3,7 +3,7 @@ PROG= hccontrol MAN= hccontrol.8 -SRCS= send_recv.c link_policy.c link_control.c \ +SRCS= send_recv.c link_policy.c link_control.c le.c\ host_controller_baseband.c info.c status.c node.c hccontrol.c \ util.c WARNS?= 2 diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.c b/usr.sbin/bluetooth/hccontrol/hccontrol.c index a921773..b72854f 100644 --- a/usr.sbin/bluetooth/hccontrol/hccontrol.c +++ b/usr.sbin/bluetooth/hccontrol/hccontrol.c @@ -183,6 +183,7 @@ do_hci_command(char const *node, int argc, char **argv) print_hci_command(host_controller_baseband_commands); print_hci_command(info_commands); print_hci_command(status_commands); + print_hci_command(le_commands); print_hci_command(node_commands); fprintf(stdout, "\nFor more information use " \ "'help command'\n"); @@ -214,6 +215,11 @@ do_hci_command(char const *node, int argc, char **argv) if (c != NULL) goto execute; + c = find_hci_command(cmd, le_commands); + if (c != NULL) + goto execute; + + c = find_hci_command(cmd, node_commands); if (c == NULL) { fprintf(stdout, "Unknown command: \"%s\"\n", cmd); diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.h b/usr.sbin/bluetooth/hccontrol/hccontrol.h index cd56ebf..c96aab0 100644 --- a/usr.sbin/bluetooth/hccontrol/hccontrol.h +++ b/usr.sbin/bluetooth/hccontrol/hccontrol.h @@ -53,6 +53,7 @@ extern struct hci_command host_controller_baseband_commands[]; extern struct hci_command info_commands[]; extern struct hci_command status_commands[]; extern struct hci_command node_commands[]; +extern struct hci_command le_commands[]; int hci_request (int, int, char const *, int, char *, int *); int hci_simple_request (int, int, char *, int *); diff --git a/usr.sbin/bluetooth/hccontrol/host_controller_baseband.c b/usr.sbin/bluetooth/hccontrol/host_controller_baseband.c index 213b017..c110b29 100644 --- a/usr.sbin/bluetooth/hccontrol/host_controller_baseband.c +++ b/usr.sbin/bluetooth/hccontrol/host_controller_baseband.c @@ -1488,6 +1488,78 @@ hci_write_page_scan_mode(int s, int argc, char **argv) return (OK); } /* hci_write_page_scan_mode */ +static int +hci_read_le_host_supported_command(int s, int argc, char **argv) +{ + ng_hci_read_le_host_supported_rp rp; + int n; + n = sizeof(rp); + if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, + NG_HCI_OCF_READ_LE_HOST_SUPPORTED), + (char *) &rp, &n) == ERROR) + return (ERROR); + + if (rp.status != 0x00) { + fprintf(stdout, "Status: %s [%#02x]\n", + hci_status2str(rp.status), rp.status); + return (FAILED); + } + + fprintf(stdout, "LE Host support: %#02x\n", rp.le_supported_host); + fprintf(stdout, "Simulateneouse LE Host : %#02x\n", rp.simultaneous_le_host); + + return (OK); + +} +static int +hci_write_le_host_supported_command(int s, int argc, char **argv) +{ + ng_hci_write_le_host_supported_cp cp; + ng_hci_write_le_host_supported_rp rp; + + int n; + + cp.le_supported_host = 0; + cp.simultaneous_le_host = 0; + switch (argc) { + case 2: + if (sscanf(argv[1], "%d", &n) != 1 || (n != 0 && n != 1)){ + printf("ARGC2: %d\n", n); + return (USAGE); + } + cp.simultaneous_le_host = (n &1); + + case 1: + if (sscanf(argv[0], "%d", &n) != 1 || (n != 0 && n != 1)){ + printf("ARGC1: %d\n", n); + return (USAGE); + } + + cp.le_supported_host = (n &1); + break; + + default: + return (USAGE); + } + + + /* send command */ + n = sizeof(rp); + if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, + NG_HCI_OCF_WRITE_LE_HOST_SUPPORTED), + (char const *) &cp, sizeof(cp), + (char *) &rp, &n) == ERROR) + return (ERROR); + + if (rp.status != 0x00) { + fprintf(stdout, "Status: %s [%#02x]\n", + hci_status2str(rp.status), rp.status); + return (FAILED); + } + + return (OK); +} + struct hci_command host_controller_baseband_commands[] = { { "reset", @@ -1873,6 +1945,17 @@ struct hci_command host_controller_baseband_commands[] = { "\t0x03 - Optional Page Scan Mode III", &hci_write_page_scan_mode }, +{ +"read_le_host_supported_command", \ +"Read if this host is in le supported mode and stimulatenouse le supported mode", +&hci_read_le_host_supported_command, +}, +{ +"write_le_host_supported_command", \ +"write_le_host_supported_command le_host[0|1] stimultajeous_le[0|1]", +&hci_write_le_host_supported_command, +}, + { NULL, } }; diff --git a/usr.sbin/bluetooth/hccontrol/le.c b/usr.sbin/bluetooth/hccontrol/le.c new file mode 100644 index 0000000..dd041d2 --- /dev/null +++ b/usr.sbin/bluetooth/hccontrol/le.c @@ -0,0 +1,352 @@ +/* + * le.c + * + * Copyright (c) 2015 Takanori Watanabe + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $Id: hccontrol.c,v 1.5 2003/09/05 00:38:24 max Exp $ + * $FreeBSD$ + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define L2CAP_SOCKET_CHECKED +#include +#include "hccontrol.h" +static int le_set_scan_param(int s, int argc, char *argv[]); +static int le_set_scan_enable(int s, int argc, char *argv[]); +static int parse_param(int argc, char *argv[], char *buf, int *len); +static int le_set_scan_response(int s, int argc, char *argv[]); +static int le_read_supported_status(int s, int argc, char *argv[]); +static int le_read_local_supported_features(int s, int argc ,char *argv[]); +static int set_le_event_mask(int s, uint64_t mask); +static int set_event_mask(int s, uint64_t mask); +static int le_enable(int s, int argc, char *argv[]); + +static int le_set_scan_param(int s, int argc, char *argv[]) +{ + int type; + int interval; + int window; + int adrtype; + int policy; + + ng_hci_le_set_scan_parameters_cp cp; + ng_hci_le_set_scan_parameters_rp rp; + int e,n; + + if(argc != 5){ + return USAGE; + } + + if(strcmp(argv[0], "active")==0){ + type = 1; + }else if (strcmp(argv[0], "passive") == 0){ + type = 0; + }else{ + return USAGE; + } + + interval = (int)(atof(argv[1])/0.625); + interval = (interval < 4)? 4: interval; + window = (int)(atof(argv[2])/0.625); + window = (window < 4) ? 4 : interval; + + if(strcmp(argv[3], "public")==0){ + adrtype = 0; + }else if (strcmp(argv[0], "random") == 0){ + adrtype = 1; + }else{ + return USAGE; + } + + if(strcmp(argv[4], "all")==0){ + policy = 0; + }else if (strcmp(argv[4], "whitelist") == 0){ + policy = 1; + }else{ + return USAGE; + } + + + cp.le_scan_type = type; + cp.le_scan_interval = interval; + cp.own_address_type = adrtype; + cp.le_scan_window = window; + cp.scanning_filter_policy = policy; + n = sizeof(rp); + e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), + (void *)&cp, sizeof(cp), (void *)&rp, &n); + + + return 0; + +} + + +static int le_set_scan_enable(int s, int argc, char *argv[]) +{ + ng_hci_le_set_scan_enable_cp cp; + ng_hci_le_set_scan_enable_rp rp; + int e,n,enable = 0; + + if(argc != 1) + return USAGE; + + if(strcmp(argv[0], "enable") == 0){ + enable = 1; + }else if(strcmp(argv[0], "disable")!= 0){ + return USAGE; + } + n = sizeof(rp); + cp.le_scan_enable = enable; + cp.filter_duplicates = 0; + e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_SET_SCAN_ENABLE), + (void *)&cp, sizeof(cp), (void *)&rp, &n); + + if(e != 0 || rp.status != 0){ + return ERROR; + } + return OK; + +} +static int parse_param(int argc, char *argv[], char *buf, int *len) +{ + char *buflast = buf + (*len); + char *curbuf = buf; + char *token,*lenpos; + int ch; + int datalen; + uint16_t value; + optreset = 1; + optind = 0; + while((ch = getopt(argc, argv , "n:f:u:")) != -1){ + switch(ch){ + case 'n': + datalen = strlen(optarg); + if( (curbuf + datalen + 2)>= buflast){ + goto done; + } + curbuf[0] = datalen + 1; + curbuf[1] = 8; + curbuf += 2; + memcpy(curbuf, optarg, datalen); + curbuf += datalen; + break; + case 'f': + if(curbuf+3 >buflast){ + goto done; + } + curbuf[0] = 2; + curbuf[1] = 1; + curbuf[2] = atoi(optarg); + curbuf += 3; + break; + case 'u': + lenpos = buf; + if((buf+2)>= buflast) + goto done; + + curbuf[1] = 2; + *lenpos = 1; + curbuf += 2; + while((token = strsep(&optarg, ",")) != NULL){ + value = strtol(token, NULL, 16); + if((curbuf+2)>= buflast) + break; + curbuf[0] = value &0xff; + curbuf[1] = (value>>8)&0xff; + curbuf += 2; + } + + } + } +done: + *len = curbuf - buf; + + return OK; +} + +static int le_set_scan_response(int s, int argc, char *argv[]) +{ + ng_hci_le_set_scan_response_data_cp cp; + ng_hci_le_set_scan_response_data_rp rp; + int n; + int e; + int len; + char buf[NG_HCI_ADVERTISING_DATA_SIZE]; + len = sizeof(buf); + parse_param(argc, argv, buf, &len); + memset(cp.scan_response_data, 0, sizeof(cp.scan_response_data)); + cp.scan_response_data_length = len; + memcpy(cp.scan_response_data, buf, len); + n = sizeof(rp); + e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA), + (void *)&cp, sizeof(cp), (void *)&rp, &n); + + printf("SEt SCAN RESPONSE %d %d %d\n", e, rp.status, n); + + return OK; +} + +static int le_read_local_supported_features(int s, int argc ,char *argv[]) +{ + ng_hci_le_read_local_supported_features_rp rp; + int e; + int n = sizeof(rp); + e = hci_simple_request(s, + NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), + (void *)&rp, &n); + printf("LOCAL SUPPOREDED:%d %d %lu\n", e, rp.status, rp.le_features); + + return 0; + +} +static int le_read_supported_status(int s, int argc, char *argv[]) +{ + ng_hci_le_read_supported_status_rp rp; + int e; + int n = sizeof(rp); + e = hci_simple_request(s, + NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_READ_SUPPORTED_STATUS), + (void *)&rp, &n); + printf("LE_STATUS:%d %d %lx\n", e, rp.status, rp.le_status); + + return 0; + +} + + +static int set_le_event_mask(int s, uint64_t mask) +{ + ng_hci_le_set_event_mask_cp semc; + ng_hci_le_set_event_mask_rp rp; + int i, n ,e; + + n = sizeof(rp); + + for(i=0; i< NG_HCI_LE_EVENT_MASK_SIZE;i++){ + semc.event_mask[i] = mask&0xff; + mask>>= 8; + } + e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_EVENT_MASK), (void *)&semc, sizeof(semc), (void *)&rp, &n); + + return 0; +} + + +static int set_event_mask(int s, uint64_t mask) +{ + ng_hci_set_event_mask_cp semc; + ng_hci_set_event_mask_rp rp; + int i,n,e; + + n = sizeof(rp); + + for(i=0; i< NG_HCI_EVENT_MASK_SIZE;i++){ + semc.event_mask[i] = mask&0xff; + mask>>= 8; + } + e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, NG_HCI_OCF_SET_EVENT_MASK), (void *)&semc, sizeof(semc), (void *)&rp, &n); + + return 0; +} + + +static int le_enable(int s, int argc, char *argv[]) +{ + if(argc != 1){ + return USAGE; + } + + if(strcasecmp(argv[0], "enable")==0){ + set_event_mask(s,NG_HCI_EVENT_MASK_DEFAULT | + NG_HCI_EVENT_MASK_LE); + set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); + }else if (strcasecmp(argv[0], "disble")==0){ + set_event_mask(s,NG_HCI_EVENT_MASK_DEFAULT); + }else{ + return USAGE; + } + + return OK; +} + +struct hci_command le_commands[] = { + { + "le_enable", + "le_enable [enable|disable] \n" + "Enable LE event ", + &le_enable, + }, + { + "le_read_local_supported_features", + "le_read_local_supported_features\n" + "read local supported features mask", + &le_read_local_supported_features, + }, + { + "le_read_supported_status", + "le_read_supported_status\n" + "read supported status" + , + &le_read_supported_status, + }, + { + "le_set_scan_response", + "le_set_scan_response -n $name -f $flag -u $uuid16,$uuid16 \n" + "set LE scan response data" + , + &le_set_scan_response, + }, + { + "le_set_scan_enable", + "le_set_scan_enable [enable|disable] \n" + "enable or disable LE device scan", + &le_set_scan_enable + }, + { + "le_set_scan_param", + "le_set_scan_param [active|passive] interval(ms) window(ms) [public|random] [all|whitelist] \n" + "set LE device scan parameter", + &le_set_scan_param + }, +}; -- cgit v1.1