From e0fbc4174cccc8754b2533f9ba3bc26260b74a91 Mon Sep 17 00:00:00 2001 From: Niel Nielsen Date: Sun, 13 Sep 2026 12:43:10 +0200 Subject: [PATCH] Add includes for string and time functions Signed-off-by: Niel Nielsen --- client/src/uart/ble_posix.c | 140 ++++++++++++++++++++++++++++++++++++ 1 file changed, 140 insertions(+) diff --git a/client/src/uart/ble_posix.c b/client/src/uart/ble_posix.c index 588c3f5e1..05c1ddb51 100644 --- a/client/src/uart/ble_posix.c +++ b/client/src/uart/ble_posix.c @@ -17,13 +17,17 @@ #include #include +#include // strcasecmp #include #include #include +#include #include #include #include #include +#include +#include #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;