348 lines
11 KiB
C
Executable File
348 lines
11 KiB
C
Executable File
/*
|
|
* bledsp.c -- BLE + DSP hardware layer implementation
|
|
*
|
|
* SPDX-License-Identifier: GPL-2.0-or-later
|
|
*
|
|
* Manages Bluetooth LE scanning via BlueZ HCI, applies a rolling-
|
|
* average RSSI filter (the "DSP" part -- simple fixed-point math
|
|
* suitable for MIPS32r2 without requiring the DSP ASE), and
|
|
* maintains a device table that the proxvec layer consumes.
|
|
*
|
|
* If the HCI socket cannot be opened (no BT hardware, or hci0 not
|
|
* up), the module automatically falls back to SIMULATION MODE that
|
|
* generates synthetic device data for UI development and testing.
|
|
*
|
|
* FUTURE: a WiFi RSSI source can be added by implementing
|
|
* bledsp_poll_wifi() alongside bledsp_poll_ble() and filling the
|
|
* same device table with source = BLE_SRC_WIFI.
|
|
*/
|
|
|
|
#include "bledsp.h"
|
|
#include "log_manager.h"
|
|
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <time.h>
|
|
|
|
/* BlueZ headers -- present when -lbluetooth is available */
|
|
#include <bluetooth/bluetooth.h>
|
|
#include <bluetooth/hci.h>
|
|
#include <bluetooth/hci_lib.h>
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Internal state */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
static bledsp_device_t dev_table[BLE_DSP_MAX_DEVICES];
|
|
static int dev_count; /* number of slots ever used */
|
|
static int sim_mode; /* 1 = simulation fallback */
|
|
static int hci_fd = -1; /* HCI socket descriptor */
|
|
static int8_t thresh_near = BLE_DSP_THRESH_NEAR;
|
|
static uint32_t last_poll_ms;
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Helpers */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
static uint32_t mono_ms(void)
|
|
{
|
|
struct timespec ts;
|
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
|
return (uint32_t)(ts.tv_sec * 1000U + ts.tv_nsec / 1000000U);
|
|
}
|
|
|
|
/* Find a slot by MAC, or allocate a new one. Returns index or -1. */
|
|
static int find_or_alloc(const uint8_t *mac)
|
|
{
|
|
int i, oldest = -1;
|
|
uint32_t oldest_ts = UINT32_MAX;
|
|
|
|
/* Search for existing MAC */
|
|
for (i = 0; i < dev_count; i++) {
|
|
if (dev_table[i].active && memcmp(dev_table[i].mac, mac, 6) == 0)
|
|
return i;
|
|
}
|
|
|
|
/* Allocate a free or oldest-inactive slot */
|
|
for (i = 0; i < BLE_DSP_MAX_DEVICES; i++) {
|
|
if (!dev_table[i].active) {
|
|
if (dev_count <= i) dev_count = i + 1;
|
|
memset(&dev_table[i], 0, sizeof(bledsp_device_t));
|
|
memcpy(dev_table[i].mac, mac, 6);
|
|
dev_table[i].source = BLE_SRC_BLE;
|
|
return i;
|
|
}
|
|
if (dev_table[i].last_seen_ms < oldest_ts) {
|
|
oldest_ts = dev_table[i].last_seen_ms;
|
|
oldest = i;
|
|
}
|
|
}
|
|
|
|
/* Evict the stalest device */
|
|
if (oldest >= 0) {
|
|
memset(&dev_table[oldest], 0, sizeof(bledsp_device_t));
|
|
memcpy(dev_table[oldest].mac, mac, 6);
|
|
dev_table[oldest].source = BLE_SRC_BLE;
|
|
return oldest;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/* Simple rolling-average filter: keeps a window of BLE_DSP_RSSI_WINDOW
|
|
* samples using integer arithmetic. New sample pushes oldest out. */
|
|
static int8_t smooth_rssi(int8_t old_smoothed, int8_t new_raw, int scan_count)
|
|
{
|
|
/* Weighted blend: trust the smoothed value more as we see more samples.
|
|
* After BLE_DSP_RSSI_WINDOW samples, it's a pure equal-weight average. */
|
|
int weight;
|
|
if (scan_count >= BLE_DSP_RSSI_WINDOW)
|
|
weight = BLE_DSP_RSSI_WINDOW;
|
|
else
|
|
weight = scan_count;
|
|
|
|
/* (old * (w-1) + new) / w -- all in integers */
|
|
return (int8_t)(((int)old_smoothed * (weight - 1) + (int)new_raw) / weight);
|
|
}
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Real BLE scanning */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
static int ble_start_scan(void)
|
|
{
|
|
int dd;
|
|
int err;
|
|
|
|
dd = hci_open_dev(BLE_DSP_HCI_DEV);
|
|
if (dd < 0) {
|
|
LOG_ERR("bledsp: hci_open_dev(%d) failed: %s", BLE_DSP_HCI_DEV, strerror(errno));
|
|
return -1;
|
|
}
|
|
|
|
/* Set LE scan parameters: active scan, interval 0x10 (10ms),
|
|
* window 0x10 (10ms), own type 0 (public), filter 0 (accept all) */
|
|
err = hci_le_set_scan_parameters(dd, 0x01, htobs(0x0010), htobs(0x0010),
|
|
0x00, 0x00, 1000);
|
|
if (err < 0) {
|
|
LOG_ERR("bledsp: set_scan_parameters failed: %s", strerror(errno));
|
|
hci_close_dev(dd);
|
|
return -1;
|
|
}
|
|
|
|
/* Enable LE scan with duplicate filtering */
|
|
err = hci_le_set_scan_enable(dd, 0x01, 0x01, 1000);
|
|
if (err < 0) {
|
|
LOG_ERR("bledsp: set_scan_enable failed: %s", strerror(errno));
|
|
hci_close_dev(dd);
|
|
return -1;
|
|
}
|
|
|
|
hci_fd = dd;
|
|
LOG_INF("bledsp: BLE LE scan started on hci%d", BLE_DSP_HCI_DEV);
|
|
return 0;
|
|
}
|
|
|
|
static int ble_poll(void)
|
|
{
|
|
unsigned char buf[HCI_MAX_EVENT_SIZE];
|
|
struct hci_filter nf, of;
|
|
socklen_t olen = sizeof(of);
|
|
ssize_t len;
|
|
|
|
if (hci_fd < 0) return -1;
|
|
|
|
/* Save old filter, set new one for LE Meta events */
|
|
getsockopt(hci_fd, SOL_HCI, HCI_FILTER, &of, &olen);
|
|
|
|
hci_filter_clear(&nf);
|
|
hci_filter_set_ptype(HCI_EVENT_PKT, &nf);
|
|
hci_filter_set_event(EVT_LE_META_EVENT, &nf);
|
|
setsockopt(hci_fd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf));
|
|
|
|
/* Mark all devices inactive for this cycle */
|
|
for (int i = 0; i < dev_count; i++)
|
|
dev_table[i].active = 0;
|
|
|
|
/* Read available events with a short timeout */
|
|
while (1) {
|
|
len = read(hci_fd, buf, sizeof(buf));
|
|
if (len <= 0) break;
|
|
|
|
/* We only care about LE advertising reports */
|
|
if (buf[0] != HCI_EVENT_PKT) continue;
|
|
/* evt_le_meta_event */
|
|
if (buf[1] != EVT_LE_META_EVENT) continue;
|
|
/* subevent 0x02 = LE Advertising Report */
|
|
if (buf[3] != 0x02) continue;
|
|
|
|
/* Parse LE Advertising Report */
|
|
int num_reports = buf[4];
|
|
unsigned char *ptr = &buf[5];
|
|
|
|
for (int r = 0; r < num_reports; r++) {
|
|
if (ptr + 9 > buf + len) break;
|
|
|
|
uint8_t evt_type = ptr[0];
|
|
uint8_t addr_type = ptr[1];
|
|
/* uint8_t addr[6] at ptr[2..7] */
|
|
uint8_t data_len = ptr[8];
|
|
/* data at ptr[9 .. 9+data_len-1] */
|
|
int8_t rssi_val = (int8_t)ptr[9 + data_len];
|
|
|
|
(void)evt_type;
|
|
(void)addr_type;
|
|
|
|
/* Ignore very weak signals */
|
|
if (rssi_val < BLE_DSP_RSSI_FLOOR) {
|
|
ptr += 9 + data_len + 1;
|
|
continue;
|
|
}
|
|
|
|
int idx = find_or_alloc(ptr + 2);
|
|
if (idx >= 0) {
|
|
int8_t prev = dev_table[idx].rssi_smoothed;
|
|
dev_table[idx].rssi_raw = rssi_val;
|
|
dev_table[idx].rssi_smoothed = smooth_rssi(
|
|
prev, rssi_val, dev_table[idx].scan_count + 1);
|
|
dev_table[idx].rssi_delta =
|
|
dev_table[idx].rssi_smoothed - prev;
|
|
dev_table[idx].last_seen_ms = mono_ms();
|
|
dev_table[idx].active = 1;
|
|
dev_table[idx].scan_count++;
|
|
}
|
|
|
|
ptr += 9 + data_len + 1;
|
|
}
|
|
}
|
|
|
|
/* Restore old filter */
|
|
setsockopt(hci_fd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
|
|
return 0;
|
|
}
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Simulation mode (fallback when no BT hardware) */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
/* A small pool of fake MAC addresses that drift around */
|
|
static const uint8_t sim_macs[][6] = {
|
|
{ 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0x01 },
|
|
{ 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0x02 },
|
|
{ 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0x03 },
|
|
};
|
|
#define SIM_MAC_COUNT (int)(sizeof(sim_macs) / sizeof(sim_macs[0]))
|
|
|
|
static int sim_tick;
|
|
|
|
static int sim_poll(void)
|
|
{
|
|
uint32_t now = mono_ms();
|
|
|
|
/* Mark all inactive */
|
|
for (int i = 0; i < dev_count; i++)
|
|
dev_table[i].active = 0;
|
|
|
|
sim_tick++;
|
|
|
|
for (int m = 0; m < SIM_MAC_COUNT; m++) {
|
|
/* Simulate an approaching-then-receding pattern.
|
|
* RSSI oscillates between -90 and -40 dBm over ~20 ticks. */
|
|
int phase = (sim_tick + m * 7) % 20;
|
|
int rssi;
|
|
if (phase < 10)
|
|
rssi = -90 + phase * 5; /* -90 -> -40 (approaching) */
|
|
else
|
|
rssi = -40 - (phase - 10) * 5; /* -40 -> -90 (receding) */
|
|
|
|
/* Add some noise */
|
|
rssi += (sim_tick * 13 + m * 37) % 7 - 3;
|
|
|
|
int idx = find_or_alloc(sim_macs[m]);
|
|
if (idx >= 0) {
|
|
int8_t prev = dev_table[idx].rssi_smoothed;
|
|
dev_table[idx].rssi_raw = (int8_t)rssi;
|
|
dev_table[idx].rssi_smoothed = smooth_rssi(
|
|
prev, (int8_t)rssi, dev_table[idx].scan_count + 1);
|
|
dev_table[idx].rssi_delta = dev_table[idx].rssi_smoothed - prev;
|
|
dev_table[idx].last_seen_ms = now;
|
|
dev_table[idx].active = 1;
|
|
dev_table[idx].scan_count++;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Public API */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
int bledsp_init(void)
|
|
{
|
|
memset(dev_table, 0, sizeof(dev_table));
|
|
dev_count = 0;
|
|
hci_fd = -1;
|
|
sim_tick = 0;
|
|
last_poll_ms = mono_ms();
|
|
|
|
if (ble_start_scan() < 0) {
|
|
LOG_WARN("bledsp: BLE init failed, entering simulation mode");
|
|
sim_mode = 1;
|
|
} else {
|
|
sim_mode = 0;
|
|
}
|
|
|
|
LOG_INF("bledsp: initialised (mode=%s)", sim_mode ? "SIM" : "BLE");
|
|
return 0;
|
|
}
|
|
|
|
int bledsp_poll(void)
|
|
{
|
|
last_poll_ms = mono_ms();
|
|
return sim_mode ? sim_poll() : ble_poll();
|
|
}
|
|
|
|
const bledsp_device_t *bledsp_find(const uint8_t *mac)
|
|
{
|
|
for (int i = 0; i < dev_count; i++) {
|
|
if (dev_table[i].active && memcmp(dev_table[i].mac, mac, 6) == 0)
|
|
return &dev_table[i];
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
int bledsp_get_devices(bledsp_device_t *out, int max)
|
|
{
|
|
int written = 0;
|
|
for (int i = 0; i < dev_count && written < max; i++) {
|
|
if (dev_table[i].active) {
|
|
out[written++] = dev_table[i];
|
|
}
|
|
}
|
|
return written;
|
|
}
|
|
|
|
int16_t bledsp_get_smoothed_rssi(const uint8_t *mac)
|
|
{
|
|
const bledsp_device_t *d = bledsp_find(mac);
|
|
return d ? (int16_t)d->rssi_smoothed : (int16_t)BLE_DSP_RSSI_FLOOR;
|
|
}
|
|
|
|
void bledsp_set_near_threshold(int8_t dBm) { thresh_near = dBm; }
|
|
int8_t bledsp_get_near_threshold(void) { return thresh_near; }
|
|
int bledsp_is_simulated(void) { return sim_mode; }
|
|
|
|
void bledsp_shutdown(void)
|
|
{
|
|
if (hci_fd >= 0) {
|
|
/* Disable LE scan */
|
|
hci_le_set_scan_enable(hci_fd, 0x00, 0x01, 1000);
|
|
hci_close_dev(hci_fd);
|
|
hci_fd = -1;
|
|
}
|
|
memset(dev_table, 0, sizeof(dev_table));
|
|
dev_count = 0;
|
|
LOG_INF("bledsp: shut down");
|
|
} |