/* * 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 $ */ #include #include #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_states(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_advertising_enable(int s, int argc, char *argv[]); static int le_set_advertising_param(int s, int argc, char *argv[]); static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]); static int le_scan(int s, int argc, char *argv[]); static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose); static int le_read_white_list_size(int s, int argc, char *argv[]); static int le_clear_white_list(int s, int argc, char *argv[]); static int le_add_device_to_white_list(int s, int argc, char *argv[]); static int le_remove_device_from_white_list(int s, int argc, char *argv[]); static int le_connect(int s, int argc, char *argv[]); static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose); static int le_read_channel_map(int s, int argc, char *argv[]); static void handle_le_remote_features_event(ng_hci_event_pkt_t* e); static int le_rand(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; int n; ng_hci_le_set_scan_parameters_cp cp; ng_hci_le_set_scan_parameters_rp rp; 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[3], "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); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), (void *)&cp, sizeof(cp), (void *)&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); } 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 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; if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_SCAN_ENABLE), (void *)&cp, sizeof(cp), (void *)&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 Scan: %s\n", enable? "Enabled" : "Disabled"); 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] = (uint8_t)strtol(optarg, NULL, 16); curbuf += 3; break; case 'u': if ((buf+2) >= buflast) goto done; lenpos = curbuf; 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; *lenpos += 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 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); if (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) == ERROR) return (ERROR); if (rp.status != 0x00) { fprintf(stdout, "Status: %s [%#02x]\n", hci_status2str(rp.status), rp.status); return (FAILED); } 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 n = sizeof(rp); union { uint64_t raw; uint8_t octets[8]; } le_features; char buffer[2048]; if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES), (void *)&rp, &n) == ERROR) return (ERROR); if (rp.status != 0x00) { fprintf(stdout, "Status: %s [%#02x]\n", hci_status2str(rp.status), rp.status); return (FAILED); } le_features.raw = rp.le_features; fprintf(stdout, "LE Features: "); for(int i = 0; i < 8; i++) fprintf(stdout, " %#02x", le_features.octets[i]); fprintf(stdout, "\n%s\n", hci_le_features2str(le_features.octets, buffer, sizeof(buffer))); fprintf(stdout, "\n"); return (OK); } static int le_read_supported_states(int s, int argc, char *argv[]) { ng_hci_le_read_supported_states_rp rp; int n = sizeof(rp); if (hci_simple_request(s, NG_HCI_OPCODE( NG_HCI_OGF_LE, NG_HCI_OCF_LE_READ_SUPPORTED_STATES), (void *)&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 States: %jx\n", rp.le_states); return (OK); } 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; n = sizeof(rp); for (i=0; i < NG_HCI_LE_EVENT_MASK_SIZE; i++) { semc.event_mask[i] = mask&0xff; mask >>= 8; } if(hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_EVENT_MASK), (void *)&semc, sizeof(semc), (void *)&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); } 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; n = sizeof(rp); for (i=0; i < NG_HCI_EVENT_MASK_SIZE; i++) { semc.event_mask[i] = mask&0xff; mask >>= 8; } if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, NG_HCI_OCF_SET_EVENT_MASK), (void *)&semc, sizeof(semc), (void *)&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); } static int le_enable(int s, int argc, char *argv[]) { int result; if (argc != 1) return (USAGE); if (strcasecmp(argv[0], "enable") == 0) { result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT | NG_HCI_EVENT_MASK_LE); if (result != OK) return result; result = set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL); if (result == OK) { fprintf(stdout, "LE enabled\n"); return (OK); } else return result; } else if (strcasecmp(argv[0], "disable") == 0) { result = set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT); if (result == OK) { fprintf(stdout, "LE disabled\n"); return (OK); } else return result; } else return (USAGE); } static int le_set_advertising_enable(int s, int argc, char *argv[]) { ng_hci_le_set_advertise_enable_cp cp; ng_hci_le_set_advertise_enable_rp rp; int 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.advertising_enable = enable; if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE), (void *)&cp, sizeof(cp), (void *)&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 Advertising %s\n", (enable ? "enabled" : "disabled")); return (OK); } static int le_set_advertising_param(int s, int argc, char *argv[]) { ng_hci_le_set_advertising_parameters_cp cp; ng_hci_le_set_advertising_parameters_rp rp; int n, ch; cp.advertising_interval_min = 0x800; cp.advertising_interval_max = 0x800; cp.advertising_type = 0; cp.own_address_type = 0; cp.direct_address_type = 0; cp.advertising_channel_map = 7; cp.advertising_filter_policy = 0; optreset = 1; optind = 0; while ((ch = getopt(argc, argv , "m:M:t:o:p:a:c:f:")) != -1) { switch(ch) { case 'm': cp.advertising_interval_min = (uint16_t)(strtod(optarg, NULL)/0.625); break; case 'M': cp.advertising_interval_max = (uint16_t)(strtod(optarg, NULL)/0.625); break; case 't': cp.advertising_type = (uint8_t)strtod(optarg, NULL); break; case 'o': cp.own_address_type = (uint8_t)strtod(optarg, NULL); break; case 'p': cp.direct_address_type = (uint8_t)strtod(optarg, NULL); break; case 'a': if (!bt_aton(optarg, &cp.direct_address)) { struct hostent *he = NULL; if ((he = bt_gethostbyname(optarg)) == NULL) return (USAGE); memcpy(&cp.direct_address, he->h_addr, sizeof(cp.direct_address)); } break; case 'c': cp.advertising_channel_map = (uint8_t)strtod(optarg, NULL); break; case 'f': cp.advertising_filter_policy = (uint8_t)strtod(optarg, NULL); break; } } n = sizeof(rp); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS), (void *)&cp, sizeof(cp), (void *)&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); } static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]) { ng_hci_le_read_advertising_channel_tx_power_rp rp; int n; n = sizeof(rp); if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER), (void *)&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, "Advertising transmit power level: %d dBm\n", (int8_t)rp.transmit_power_level); return (OK); } static int le_set_advertising_data(int s, int argc, char *argv[]) { ng_hci_le_set_advertising_data_cp cp; ng_hci_le_set_advertising_data_rp rp; int n, len; n = sizeof(rp); char buf[NG_HCI_ADVERTISING_DATA_SIZE]; len = sizeof(buf); parse_param(argc, argv, buf, &len); memset(cp.advertising_data, 0, sizeof(cp.advertising_data)); cp.advertising_data_length = len; memcpy(cp.advertising_data, buf, len); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_ADVERTISING_DATA), (void *)&cp, sizeof(cp), (void *)&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); } static int le_read_buffer_size(int s, int argc, char *argv[]) { union { ng_hci_le_read_buffer_size_rp v1; ng_hci_le_read_buffer_size_rp_v2 v2; } rp; int n, ch; uint8_t v; uint16_t cmd; optreset = 1; optind = 0; /* Default to version 1*/ v = 1; cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE; while ((ch = getopt(argc, argv , "v:")) != -1) { switch(ch) { case 'v': v = (uint8_t)strtol(optarg, NULL, 16); if (v == 2) cmd = NG_HCI_OCF_LE_READ_BUFFER_SIZE_V2; else if (v > 2) return (USAGE); break; default: v = 1; } } n = sizeof(rp); if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, cmd), (void *)&rp, &n) == ERROR) return (ERROR); if (rp.v1.status != 0x00) { fprintf(stdout, "Status: %s [%#02x]\n", hci_status2str(rp.v1.status), rp.v1.status); return (FAILED); } fprintf(stdout, "ACL data packet length: %d\n", rp.v1.hc_le_data_packet_length); fprintf(stdout, "Number of ACL data packets: %d\n", rp.v1.hc_total_num_le_data_packets); if (v == 2) { fprintf(stdout, "ISO data packet length: %d\n", rp.v2.hc_iso_data_packet_length); fprintf(stdout, "Number of ISO data packets: %d\n", rp.v2.hc_total_num_iso_data_packets); } return (OK); } static int le_scan(int s, int argc, char *argv[]) { int n, bufsize, scancount, numscans; bool verbose; uint8_t active = 0; char ch; char b[512]; ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; ng_hci_le_set_scan_parameters_cp scan_param_cp; ng_hci_le_set_scan_parameters_rp scan_param_rp; ng_hci_le_set_scan_enable_cp scan_enable_cp; ng_hci_le_set_scan_enable_rp scan_enable_rp; optreset = 1; optind = 0; verbose = false; numscans = 1; while ((ch = getopt(argc, argv , "an:v")) != -1) { switch(ch) { case 'a': active = 1; break; case 'n': numscans = (uint8_t)strtol(optarg, NULL, 10); break; case 'v': verbose = true; break; } } scan_param_cp.le_scan_type = active; scan_param_cp.le_scan_interval = (uint16_t)(100/0.625); scan_param_cp.le_scan_window = (uint16_t)(50/0.625); /* Address type public */ scan_param_cp.own_address_type = 0; /* 'All' filter policy */ scan_param_cp.scanning_filter_policy = 0; n = sizeof(scan_param_rp); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), (void *)&scan_param_cp, sizeof(scan_param_cp), (void *)&scan_param_rp, &n) == ERROR) return (ERROR); if (scan_param_rp.status != 0x00) { fprintf(stdout, "LE_Set_Scan_Parameters failed. Status: %s [%#02x]\n", hci_status2str(scan_param_rp.status), scan_param_rp.status); return (FAILED); } /* Enable scanning */ n = sizeof(scan_enable_rp); scan_enable_cp.le_scan_enable = 1; scan_enable_cp.filter_duplicates = 1; if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_SCAN_ENABLE), (void *)&scan_enable_cp, sizeof(scan_enable_cp), (void *)&scan_enable_rp, &n) == ERROR) return (ERROR); if (scan_enable_rp.status != 0x00) { fprintf(stdout, "LE_Scan_Enable enable failed. Status: %s [%#02x]\n", hci_status2str(scan_enable_rp.status), scan_enable_rp.status); return (FAILED); } scancount = 0; while (scancount < numscans) { /* wait for scan events */ bufsize = sizeof(b); if (hci_recv(s, b, &bufsize) == ERROR) { return (ERROR); } if (bufsize < sizeof(*e)) { errno = EIO; return (ERROR); } scancount++; if (e->event == NG_HCI_EVENT_LE) { fprintf(stdout, "Scan %d\n", scancount); handle_le_event(e, verbose); } } fprintf(stdout, "Scan complete\n"); /* Disable scanning */ n = sizeof(scan_enable_rp); scan_enable_cp.le_scan_enable = 0; if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_SET_SCAN_ENABLE), (void *)&scan_enable_cp, sizeof(scan_enable_cp), (void *)&scan_enable_rp, &n) == ERROR) return (ERROR); if (scan_enable_rp.status != 0x00) { fprintf(stdout, "LE_Scan_Enable disable failed. Status: %s [%#02x]\n", hci_status2str(scan_enable_rp.status), scan_enable_rp.status); return (FAILED); } return (OK); } static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose) { int rc; ng_hci_le_ep *leer = (ng_hci_le_ep *)(e + 1); ng_hci_le_advertising_report_ep *advrep = (ng_hci_le_advertising_report_ep *)(leer + 1); ng_hci_le_advreport *reports = (ng_hci_le_advreport *)(advrep + 1); if (leer->subevent_code == NG_HCI_LEEV_ADVREP) { fprintf(stdout, "Scan result, num_reports: %d\n", advrep->num_reports); for(rc = 0; rc < advrep->num_reports; rc++) { uint8_t length = (uint8_t)reports[rc].length_data; fprintf(stdout, "\tBD_ADDR %s \n", hci_bdaddr2str(&reports[rc].bdaddr)); fprintf(stdout, "\tAddress type: %s\n", hci_addrtype2str(reports[rc].addr_type)); if (length > 0 && verbose) { dump_adv_data(length, reports[rc].data); print_adv_data(length, reports[rc].data); fprintf(stdout, "\tRSSI: %d dBm\n", (int8_t)reports[rc].data[length]); fprintf(stdout, "\n"); } } } } static int le_read_white_list_size(int s, int argc, char *argv[]) { ng_hci_le_read_white_list_size_rp rp; int n; n = sizeof(rp); if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE), (void *)&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, "White list size: %d\n", (uint8_t)rp.white_list_size); return (OK); } static int le_clear_white_list(int s, int argc, char *argv[]) { ng_hci_le_clear_white_list_rp rp; int n; n = sizeof(rp); if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_CLEAR_WHITE_LIST), (void *)&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, "White list cleared\n"); return (OK); } static int le_add_device_to_white_list(int s, int argc, char *argv[]) { ng_hci_le_add_device_to_white_list_cp cp; ng_hci_le_add_device_to_white_list_rp rp; int n; char ch; optreset = 1; optind = 0; bool addr_set = false; n = sizeof(rp); cp.address_type = 0x00; while ((ch = getopt(argc, argv , "t:a:")) != -1) { switch(ch) { case 't': if (strcmp(optarg, "public") == 0) cp.address_type = 0x00; else if (strcmp(optarg, "random") == 0) cp.address_type = 0x01; else return (USAGE); break; case 'a': addr_set = true; if (!bt_aton(optarg, &cp.address)) { struct hostent *he = NULL; if ((he = bt_gethostbyname(optarg)) == NULL) return (USAGE); memcpy(&cp.address, he->h_addr, sizeof(cp.address)); } break; } } if (addr_set == false) return (USAGE); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST), (void *)&cp, sizeof(cp), (void *)&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, "Address added to white list\n"); return (OK); } static int le_remove_device_from_white_list(int s, int argc, char *argv[]) { ng_hci_le_remove_device_from_white_list_cp cp; ng_hci_le_remove_device_from_white_list_rp rp; int n; char ch; optreset = 1; optind = 0; bool addr_set = false; n = sizeof(rp); cp.address_type = 0x00; while ((ch = getopt(argc, argv , "t:a:")) != -1) { switch(ch) { case 't': if (strcmp(optarg, "public") == 0) cp.address_type = 0x00; else if (strcmp(optarg, "random") == 0) cp.address_type = 0x01; else return (USAGE); break; case 'a': addr_set = true; if (!bt_aton(optarg, &cp.address)) { struct hostent *he = NULL; if ((he = bt_gethostbyname(optarg)) == NULL) return (USAGE); memcpy(&cp.address, he->h_addr, sizeof(cp.address)); } break; } } if (addr_set == false) return (USAGE); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST), (void *)&cp, sizeof(cp), (void *)&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, "Address removed from white list\n"); return (OK); } static int le_connect(int s, int argc, char *argv[]) { ng_hci_le_create_connection_cp cp; ng_hci_status_rp rp; char b[512]; ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; int n, scancount, bufsize; char ch; bool addr_set = false; bool verbose = false; optreset = 1; optind = 0; /* minimal scan interval (2.5ms) */ cp.scan_interval = htole16(4); cp.scan_window = htole16(4); /* Don't use the whitelist */ cp.filter_policy = 0x00; /* Default to public peer address */ cp.peer_addr_type = 0x00; /* Own address type public */ cp.own_address_type = 0x00; /* 18.75ms min connection interval */ cp.conn_interval_min = htole16(0x000F); /* 18.75ms max connection interval */ cp.conn_interval_max = htole16(0x000F); /* 0 events connection latency */ cp.conn_latency = htole16(0x0000); /* 32s supervision timeout */ cp.supervision_timeout = htole16(0x0C80); /* Min CE Length 0.625 ms */ cp.min_ce_length = htole16(1); /* Max CE Length 0.625 ms */ cp.max_ce_length = htole16(1); while ((ch = getopt(argc, argv , "a:t:v")) != -1) { switch(ch) { case 't': if (strcmp(optarg, "public") == 0) cp.peer_addr_type = 0x00; else if (strcmp(optarg, "random") == 0) cp.peer_addr_type = 0x01; else return (USAGE); break; case 'a': addr_set = true; if (!bt_aton(optarg, &cp.peer_addr)) { struct hostent *he = NULL; if ((he = bt_gethostbyname(optarg)) == NULL) return (USAGE); memcpy(&cp.peer_addr, he->h_addr, sizeof(cp.peer_addr)); } break; case 'v': verbose = true; break; } } if (addr_set == false) return (USAGE); n = sizeof(rp); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_CREATE_CONNECTION), (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) return (ERROR); if (rp.status != 0x00) { fprintf(stdout, "Create connection failed. Status: %s [%#02x]\n", hci_status2str(rp.status), rp.status); return (FAILED); } scancount = 0; while (scancount < 3) { /* wait for connection events */ bufsize = sizeof(b); if (hci_recv(s, b, &bufsize) == ERROR) { return (ERROR); } if (bufsize < sizeof(*e)) { errno = EIO; return (ERROR); } scancount++; if (e->event == NG_HCI_EVENT_LE) { handle_le_connection_event(e, verbose); break; } } return (OK); } static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose) { ng_hci_le_ep *ev_pkt; ng_hci_le_connection_complete_ep *conn_event; ev_pkt = (ng_hci_le_ep *)(e + 1); if (ev_pkt->subevent_code == NG_HCI_LEEV_CON_COMPL) { conn_event =(ng_hci_le_connection_complete_ep *)(ev_pkt + 1); fprintf(stdout, "Handle: %d\n", le16toh(conn_event->handle)); if (verbose) { fprintf(stdout, "Status: %s\n", hci_status2str(conn_event->status)); fprintf(stdout, "Role: %s\n", hci_role2str(conn_event->role)); fprintf(stdout, "Address Type: %s\n", hci_addrtype2str(conn_event->address_type)); fprintf(stdout, "Address: %s\n", hci_bdaddr2str(&conn_event->address)); fprintf(stdout, "Interval: %.2fms\n", 6.25 * le16toh(conn_event->interval)); fprintf(stdout, "Latency: %d events\n", conn_event->latency); fprintf(stdout, "Supervision timeout: %dms\n", 10 * le16toh(conn_event->supervision_timeout)); fprintf(stdout, "Master clock accuracy: %s\n", hci_mc_accuracy2str( conn_event->master_clock_accuracy)); } } } static int le_read_channel_map(int s, int argc, char *argv[]) { ng_hci_le_read_channel_map_cp cp; ng_hci_le_read_channel_map_rp rp; int n; char buffer[2048]; /* parse command parameters */ switch (argc) { case 1: /* connection handle */ if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff) return (USAGE); cp.connection_handle = (uint16_t) (n & 0x0fff); cp.connection_handle = htole16(cp.connection_handle); break; default: return (USAGE); } n = sizeof(rp); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_READ_CHANNEL_MAP), (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) return (ERROR); if (rp.status != 0x00) { fprintf(stdout, "Read channel map failed. Status: %s [%#02x]\n", hci_status2str(rp.status), rp.status); return (FAILED); } fprintf(stdout, "Connection handle: %d\n", le16toh(rp.connection_handle)); fprintf(stdout, "Used channels:\n"); fprintf(stdout, "\n%s\n", hci_le_chanmap2str(rp.le_channel_map, buffer, sizeof(buffer))); return (OK); } /* le_read_channel_map */ static int le_read_remote_features(int s, int argc, char *argv[]) { ng_hci_le_read_remote_used_features_cp cp; ng_hci_status_rp rp; int n, bufsize; char b[512]; ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; /* parse command parameters */ switch (argc) { case 1: /* connection handle */ if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff) return (USAGE); cp.connection_handle = (uint16_t) (n & 0x0fff); cp.connection_handle = htole16(cp.connection_handle); break; default: return (USAGE); } n = sizeof(rp); if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_READ_REMOTE_USED_FEATURES), (void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR) return (ERROR); if (rp.status != 0x00) { fprintf(stdout, "Read remote features failed. Status: %s [%#02x]\n", hci_status2str(rp.status), rp.status); return (FAILED); } /* wait for connection events */ bufsize = sizeof(b); if (hci_recv(s, b, &bufsize) == ERROR) { return (ERROR); } if (bufsize < sizeof(*e)) { errno = EIO; return (ERROR); } if (e->event == NG_HCI_EVENT_LE) { handle_le_remote_features_event(e); } return (OK); } /* le_read_remote_features */ static void handle_le_remote_features_event(ng_hci_event_pkt_t* e) { ng_hci_le_ep *ev_pkt; ng_hci_le_read_remote_features_ep *feat_event; char buffer[2048]; ev_pkt = (ng_hci_le_ep *)(e + 1); if (ev_pkt->subevent_code == NG_HCI_LEEV_READ_REMOTE_FEATURES_COMPL) { feat_event =(ng_hci_le_read_remote_features_ep *)(ev_pkt + 1); fprintf(stdout, "Handle: %d\n", le16toh(feat_event->connection_handle)); fprintf(stdout, "Status: %s\n", hci_status2str(feat_event->status)); fprintf(stdout, "Features:\n%s\n", hci_le_features2str(feat_event->features, buffer, sizeof(buffer))); } } /* handle_le_remote_features_event */ static int le_rand(int s, int argc, char *argv[]) { ng_hci_le_rand_rp rp; int n; n = sizeof(rp); if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, NG_HCI_OCF_LE_RAND), (void *)&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, "Random number : %08llx\n", (unsigned long long)le64toh(rp.random_number)); 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_states", "le_read_supported_states\n" "read supported status" , &le_read_supported_states, }, { "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 }, { "le_set_advertising_enable", "le_set_advertising_enable [enable|disable] \n" "start or stop advertising", &le_set_advertising_enable }, { "le_read_advertising_channel_tx_power", "le_read_advertising_channel_tx_power\n" "read host advertising transmit poser level (dBm)", &le_read_advertising_channel_tx_power }, { "le_set_advertising_param", "le_set_advertising_param [-m min_interval(ms)] [-M max_interval(ms)]\n" "[-t advertising_type] [-o own_address_type] [-p peer_address_type]\n" "[-c advertising_channel_map] [-f advertising_filter_policy]\n" "[-a peer_address]\n" "set LE device advertising parameters", &le_set_advertising_param }, { "le_set_advertising_data", "le_set_advertising_data -n $name -f $flag -u $uuid16,$uuid16 \n" "set LE device advertising packed data", &le_set_advertising_data }, { "le_read_buffer_size", "le_read_buffer_size [-v 1|2]\n" "Read the maximum size of ACL and ISO data packets", &le_read_buffer_size }, { "le_scan", "le_scan [-a] [-v] [-n number_of_scans]\n" "Do an LE scan", &le_scan }, { "le_read_white_list_size", "le_read_white_list_size\n" "Read total number of white list entries that can be stored", &le_read_white_list_size }, { "le_clear_white_list", "le_clear_white_list\n" "Clear the white list in the controller", &le_clear_white_list }, { "le_add_device_to_white_list", "le_add_device_to_white_list\n" "[-t public|random] -a address\n" "Add device to the white list", &le_add_device_to_white_list }, { "le_remove_device_from_white_list", "le_remove_device_from_white_list\n" "[-t public|random] -a address\n" "Remove device from the white list", &le_remove_device_from_white_list }, { "le_connect", "le_connect -a address [-t public|random] [-v]\n" "Connect to an LE device", &le_connect }, { "le_read_channel_map", "le_read_channel_map \n" "Read the channel map for a connection", &le_read_channel_map }, { "le_read_remote_features", "le_read_remote_features \n" "Read supported features for the device\n" "identified by the connection handle", &le_read_remote_features }, { "le_rand", "le_rand\n" "Generate 64 bits of random data", &le_rand }, { NULL, } };