Add includes for string and time functions
Signed-off-by: Niel Nielsen <nieldk@gmail.com>
This commit is contained in:
1 parent
72f48c8a02
commit
e0fbc4174c
1 file changed
+140
@@ -17,13 +17,17 @@
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <strings.h> // strcasecmp
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/select.h>
|
||||
#include <bluetooth/bluetooth.h>
|
||||
#include <bluetooth/l2cap.h>
|
||||
#include <bluetooth/hci.h>
|
||||
#include <bluetooth/hci_lib.h>
|
||||
|
||||
#include "ui.h" // PrintAndLogEx
|
||||
#include "pm3_cmd.h" // PM3_* (only for messaging parity; returns are 0/neg here)
|
||||
@@ -183,6 +187,142 @@ static int att_subscribe(int fd, uint16_t cccd_handle) {
|
||||
return (n >= 1) ? 0 : -1;
|
||||
}
|
||||
|
||||
// ---- name -> address resolution (active LE scan over raw HCI) ----
|
||||
|
||||
// AD types carrying the device name (Core Spec, Supplement, Part A).
|
||||
#define BLE_ADV_NAME_SHORT 0x08
|
||||
#define BLE_ADV_NAME_FULL 0x09
|
||||
|
||||
static long ble_now_ms(void) {
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return ts.tv_sec * 1000L + ts.tv_nsec / 1000000L;
|
||||
}
|
||||
|
||||
// Walk the AD structures of one advertising/scan-response report, looking for a
|
||||
// Local Name (complete or shortened) that equals `want` (case-insensitive).
|
||||
// Copies whatever name it finds into `found` for logging. Returns 1 on match.
|
||||
static int ble_ad_name_matches(const char *want, const uint8_t *ad, size_t adlen,
|
||||
char *found, size_t foundsz) {
|
||||
size_t i = 0;
|
||||
while (i < adlen) {
|
||||
uint8_t flen = ad[i]; // length byte counts the type + value
|
||||
if (flen == 0) break; // padding -> end of meaningful data
|
||||
if (i + 1 + flen > adlen) break; // truncated field -> stop, don't overrun
|
||||
uint8_t type = ad[i + 1];
|
||||
if (type == BLE_ADV_NAME_FULL || type == BLE_ADV_NAME_SHORT) {
|
||||
size_t nlen = flen - 1;
|
||||
if (nlen >= foundsz) nlen = foundsz - 1;
|
||||
memcpy(found, &ad[i + 2], nlen);
|
||||
found[nlen] = 0;
|
||||
if (strcasecmp(found, want) == 0) return 1;
|
||||
}
|
||||
i += flen + 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ble_resolve_name(const char *name, char *out_mac, size_t out_mac_sz, int timeout_ms) {
|
||||
if (name == NULL || out_mac == NULL || out_mac_sz < 18) return -1;
|
||||
out_mac[0] = 0;
|
||||
|
||||
int dev_id = hci_get_route(NULL);
|
||||
if (dev_id < 0) {
|
||||
PrintAndLogEx(ERR, "BLE: no local Bluetooth adapter found (%s)", strerror(errno));
|
||||
return -1;
|
||||
}
|
||||
int dd = hci_open_dev(dev_id);
|
||||
if (dd < 0) {
|
||||
PrintAndLogEx(ERR, "BLE: cannot open hci%d (%s)", dev_id, strerror(errno));
|
||||
if (errno == EPERM || errno == EACCES)
|
||||
PrintAndLogEx(INFO, "BLE: scanning by name needs privileges - run with sudo, or grant the client "
|
||||
_YELLOW_("CAP_NET_RAW+CAP_NET_ADMIN"));
|
||||
return -1;
|
||||
}
|
||||
|
||||
// NOTE: despite the header naming its first arg `dev_id`, BlueZ's
|
||||
// hci_le_set_scan_* helpers expect the *open descriptor* (dd) and pass it
|
||||
// straight to hci_send_req(). Passing the raw device id here silently fails.
|
||||
// Active scan (0x01) so we also receive SCAN_RSP, which is where many
|
||||
// devices (incl. the BWM) put the complete local name.
|
||||
if (hci_le_set_scan_parameters(dd, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0x00, 1000) < 0)
|
||||
PrintAndLogEx(INFO, "BLE: set scan parameters failed (%s), continuing", strerror(errno));
|
||||
|
||||
// A scan already left enabled makes the enable below return EIO; clear it first.
|
||||
hci_le_set_scan_enable(dd, 0x00, 0x00, 1000);
|
||||
if (hci_le_set_scan_enable(dd, 0x01, 0x00, 1000) < 0) { // filter_dup off: names may only be in SCAN_RSP
|
||||
PrintAndLogEx(ERR, "BLE: cannot start LE scan (%s)", strerror(errno));
|
||||
if (errno == EPERM || errno == EACCES)
|
||||
PrintAndLogEx(INFO, "BLE: scanning by name needs privileges - run with sudo, or grant the client "
|
||||
_YELLOW_("CAP_NET_RAW+CAP_NET_ADMIN"));
|
||||
hci_close_dev(dd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Receive only HCI event packets carrying LE meta events; save/restore the
|
||||
// socket's previous filter so we leave the handle as we found it.
|
||||
struct hci_filter of, nf;
|
||||
socklen_t olen = sizeof(of);
|
||||
int have_of = (getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &olen) == 0);
|
||||
hci_filter_clear(&nf);
|
||||
hci_filter_set_ptype(HCI_EVENT_PKT, &nf);
|
||||
hci_filter_set_event(EVT_LE_META_EVENT, &nf);
|
||||
setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf));
|
||||
|
||||
PrintAndLogEx(INFO, "BLE: scanning up to " _YELLOW_("%d") " ms for " _YELLOW_("%s") " ...", timeout_ms, name);
|
||||
|
||||
int rc = -1;
|
||||
char nm[64];
|
||||
long deadline = ble_now_ms() + timeout_ms;
|
||||
uint8_t buf[HCI_MAX_EVENT_SIZE];
|
||||
|
||||
while (ble_now_ms() < deadline) {
|
||||
long rem = deadline - ble_now_ms();
|
||||
if (rem <= 0) break;
|
||||
struct timeval tv = { .tv_sec = rem / 1000, .tv_usec = (rem % 1000) * 1000 };
|
||||
fd_set rs;
|
||||
FD_ZERO(&rs);
|
||||
FD_SET(dd, &rs);
|
||||
int s = select(dd + 1, &rs, NULL, NULL, &tv);
|
||||
if (s < 0) { if (errno == EINTR) break; break; } // Ctrl-C or error -> give up cleanly
|
||||
if (s == 0) break; // timeout
|
||||
|
||||
int n = read(dd, buf, sizeof(buf));
|
||||
if (n < (int)(1 + HCI_EVENT_HDR_SIZE + 1)) continue;
|
||||
uint8_t *pkt_end = buf + n;
|
||||
|
||||
// layout: [0]=HCI_EVENT_PKT, [1..2]=hci_event_hdr, then evt_le_meta_event
|
||||
evt_le_meta_event *meta = (evt_le_meta_event *)(buf + 1 + HCI_EVENT_HDR_SIZE);
|
||||
if ((uint8_t *)meta->data > pkt_end) continue;
|
||||
if (meta->subevent != EVT_LE_ADVERTISING_REPORT) continue;
|
||||
|
||||
uint8_t *p = meta->data;
|
||||
uint8_t num = *p++; // number of reports in this event
|
||||
for (uint8_t r = 0; r < num; r++) {
|
||||
if (p + LE_ADVERTISING_INFO_SIZE > pkt_end) break;
|
||||
le_advertising_info *info = (le_advertising_info *)p;
|
||||
uint8_t *next = p + LE_ADVERTISING_INFO_SIZE + info->length + 1; // +1 trailing RSSI
|
||||
if (next > pkt_end) break; // malformed / truncated report
|
||||
if (ble_ad_name_matches(name, info->data, info->length, nm, sizeof(nm))) {
|
||||
ba2str(&info->bdaddr, out_mac);
|
||||
PrintAndLogEx(SUCCESS, "BLE: found " _GREEN_("%s") " at " _GREEN_("%s"), nm, out_mac);
|
||||
rc = 0;
|
||||
break;
|
||||
}
|
||||
p = next;
|
||||
}
|
||||
if (rc == 0) break;
|
||||
}
|
||||
|
||||
hci_le_set_scan_enable(dd, 0x00, 0x00, 1000);
|
||||
if (have_of) setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
|
||||
hci_close_dev(dd);
|
||||
|
||||
if (rc != 0)
|
||||
PrintAndLogEx(ERR, "BLE: no advertising device named " _YELLOW_("%s") " seen within %d ms", name, timeout_ms);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int ble_connect(const char *mac, uint16_t chr_uuid16, ble_conn_t *conn) {
|
||||
memset(conn, 0, sizeof(*conn));
|
||||
conn->fd = -1;
|
||||
|
||||
Reference in new issue
Block a user