dcosnet
/
OreBolt-OS
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
OreBolt-OS/modules/orebolt-proxalarm/bledsp.c

427 lines
14 KiB
C
Executable File
Raw Blame History

/*
* bledsp.c -- Unified RF scanning + DSP hardware layer implementation
*
* SPDX-License-Identifier: GPL-2.0-or-later
*
* Manages BLE LE scanning via BlueZ HCI, applies a rolling-average
* RSSI filter, and maintains a unified device table. Also provides
* a WiFi RSSI poll stub (bledsp_poll_wifi) ready for future nl80211
* integration.
*
* Unified RF scanning layer handling BLE advertisements and RSSI
* smoothing via MIPS DSP ASE intrinsics. Source-agnostic device table
* supports BLE, WiFi (stub), and simulation modes.
*
* Source modes:
* BLE -- Real BlueZ HCI LE scanning (default when hci0 available)
* WIFI -- Stub: bledsp_poll_wifi() is a no-op placeholder for
* future nl80211-based WiFi scanning
* SIM -- Automatic fallback when BLE HCI fails; generates
* synthetic device data for UI development/testing
*
* If the HCI socket cannot be opened (no BT hardware, or hci0 not
* up), the module automatically enters SIMULATION mode.
*/
#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;
static bledsp_source_t active_source = BLE_SRC_BLE;
/* ------------------------------------------------------------------ */
/* 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));
/* Note: bledsp_poll() marks all devices inactive before calling us */
/* 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;
}
}
/* Revert to previous filter state */
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();
/* Note: bledsp_poll() marks all devices inactive before calling us */
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;
}
/* ------------------------------------------------------------------ */
/* WiFi scanning stub */
/* ------------------------------------------------------------------ */
/* This function is the integration point for a future WiFi RSSI source.
* When a WiFi driver (nl80211 / libnl) becomes available on the X1000E,
* implement scanning here and fill the device table the same way
* ble_poll() does, but with source = BLE_SRC_WIFI.
*
* The device MAC for WiFi can be the BSSID (6 bytes) of each AP/station.
* RSSI is available from nl80211 survey data or radiotap headers.
*
* Currently: returns 0 (no devices). The bledsp_poll() caller already
* marks all devices inactive, so WiFi entries from a previous cycle
* will naturally expire via proxvec's stale-target timeout.
*/
int bledsp_poll_wifi(void)
{
/* TODO: implement nl80211-based WiFi scanning
* 1. Open nl80211 socket
* 2. Trigger scan on phy0
* 3. Parse scan results (BSSID + RSSI)
* 4. For each result: find_or_alloc(bssid) with source=BLE_SRC_WIFI
* 5. Apply smooth_rssi() same as BLE path
* 6. Return count of devices updated
*
* Reference: https://www.infradead.org/~tgr/libnl/
* The X1000E has no onboard WiFi in current H2 revisions, but
* a USB WiFi dongle with monitor mode could provide this data.
*/
return 0;
}
/* ------------------------------------------------------------------ */
/* Source management */
/* ------------------------------------------------------------------ */
bledsp_source_t bledsp_get_source(void)
{
return active_source;
}
bledsp_source_t bledsp_cycle_source(void)
{
/* Cycle: BLE -> WIFI -> BLE. SIM is not user-selectable;
* it's the automatic fallback when hardware is unavailable. */
if (active_source == BLE_SRC_BLE) {
active_source = BLE_SRC_WIFI;
LOG_INF("bledsp: source switched to WIFI (stub)");
} else {
active_source = BLE_SRC_BLE;
if (!sim_mode)
LOG_INF("bledsp: source switched to BLE");
else
LOG_INF("bledsp: source switched to BLE (running in SIM mode)");
}
return active_source;
}
const char *bledsp_source_label(void)
{
if (sim_mode) return "SIM";
switch (active_source) {
case BLE_SRC_WIFI: return "WIFI";
default: return "BLE";
}
}
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();
/* Mark all devices inactive for this cycle */
for (int i = 0; i < dev_count; i++)
dev_table[i].active = 0;
/* Poll based on active source */
if (sim_mode) {
return sim_poll();
} else if (active_source == BLE_SRC_WIFI) {
return bledsp_poll_wifi();
} else {
return 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");
}
Reference in New Issue View Git Blame Copy Permalink