Eun0us/espilon-source
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
main
espilon-source/espilon_bot/components/mod_recon/mod_mlat.c
Eun0us 6d45770d98 epsilon: merge command system into core + add 5 new modules 
Move command registry from components/command/ into components/core/.
New modules: mod_canbus, mod_honeypot, mod_fallback, mod_redteam, mod_ota.
Replace mod_proxy with tun_core (multiplexed SOCKS5 tunnel).
Kconfig extended with per-module settings and async worker config.
2026-02-28 20:07:59 +01:00

796 lines
23 KiB
C
Raw Permalink Blame History

/**
* @file mod_mlat.c
* @brief Multilateration Scanner Module (BLE + WiFi)
*
* This module turns an ESP32 into an RSSI scanner for multilateration.
* Supports both BLE and WiFi modes, switchable at runtime from C2.
* Position is configured from C2, and RSSI readings are sent back via TCP.
*
* Supports two coordinate systems:
* - GPS (lat/lon in degrees) for outdoor tracking with real maps
* - Local (x/y in meters) for indoor tracking with floor plans
*
* Commands:
* mlat config gps <lat> <lon> - Set GPS position (degrees)
* mlat config local <x> <y> - Set local position (meters)
* mlat config <lat> <lon> - Backward compat: GPS mode
* mlat mode <ble|wifi> - Set scanning mode
* mlat start <mac> - Start scanning for target MAC
* mlat stop - Stop scanning
* mlat status - Show current config and state
*
* Data format sent to C2:
* MLAT:G;<lat>;<lon>;<rssi> - GPS coordinates
* MLAT:L;<x>;<y>;<rssi> - Local coordinates (meters)
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <ctype.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_err.h"
#include "nvs_flash.h"
/* BLE */
#include "esp_bt.h"
#include "esp_gap_ble_api.h"
#include "esp_bt_main.h"
/* WiFi */
#include "esp_wifi.h"
#include "esp_event.h"
#include "utils.h"
#if defined(CONFIG_RECON_MODE_MLAT)
/* ============================================================
* CONFIG
* ============================================================ */
#define TAG "MLAT"
#define SEND_INTERVAL_MS 2000 /* Send aggregated RSSI every 2s */
#define RSSI_HISTORY_SIZE 10 /* Keep last N readings for averaging */
#define CHANNEL_HOP_MS 200 /* WiFi channel hop interval */
/* ============================================================
* TYPES
* ============================================================ */
typedef enum {
MLAT_MODE_NONE = 0,
MLAT_MODE_BLE,
MLAT_MODE_WIFI
} mlat_mode_t;
typedef enum {
COORD_GPS = 0, /* lat/lon (degrees) */
COORD_LOCAL /* x/y (meters) */
} coord_type_t;
/* WiFi frame header for promiscuous mode */
typedef struct {
unsigned frame_ctrl:16;
unsigned duration_id:16;
uint8_t addr1[6]; /* Destination */
uint8_t addr2[6]; /* Source */
uint8_t addr3[6]; /* BSSID */
unsigned seq_ctrl:16;
} __attribute__((packed)) wifi_mgmt_hdr_t;
/* ============================================================
* STATE
* ============================================================ */
static bool mlat_configured = false;
static bool mlat_running = false;
static mlat_mode_t mlat_mode = MLAT_MODE_BLE; /* Default to BLE */
/* Hardware init state */
static bool ble_initialized = false;
static bool wifi_promisc_enabled = false;
/* Scanner position (set via mlat config) */
static coord_type_t coord_type = COORD_GPS;
static double scanner_lat = 0.0; /* GPS latitude (degrees) */
static double scanner_lon = 0.0; /* GPS longitude (degrees) */
static double scanner_x = 0.0; /* Local X position (meters) */
static double scanner_y = 0.0; /* Local Y position (meters) */
/* Target MAC */
static uint8_t target_mac[6] = {0};
static char target_mac_str[20] = {0};
/* RSSI history for averaging */
static int8_t rssi_history[RSSI_HISTORY_SIZE];
static size_t rssi_count = 0;
static size_t rssi_index = 0;
/* Task handles */
static TaskHandle_t send_task_handle = NULL;
static TaskHandle_t hop_task_handle = NULL;
/* WiFi current channel */
static uint8_t current_channel = 1;
/* ============================================================
* UTILS
* ============================================================ */
static bool parse_mac_str(const char *input, uint8_t *mac_out)
{
char clean[13] = {0};
int j = 0;
for (int i = 0; input[i] && j < 12; i++) {
char c = input[i];
if (c == ':' || c == '-' || c == ' ')
continue;
if (!isxdigit((unsigned char)c))
return false;
clean[j++] = toupper((unsigned char)c);
}
if (j != 12) return false;
for (int i = 0; i < 6; i++) {
char b[3] = { clean[i*2], clean[i*2+1], 0 };
mac_out[i] = (uint8_t)strtol(b, NULL, 16);
}
return true;
}
static void mac_to_str(const uint8_t *mac, char *out, size_t len)
{
snprintf(out, len, "%02X:%02X:%02X:%02X:%02X:%02X",
mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}
static int8_t get_average_rssi(void)
{
if (rssi_count == 0) return 0;
int32_t sum = 0;
size_t count = (rssi_count < RSSI_HISTORY_SIZE) ? rssi_count : RSSI_HISTORY_SIZE;
for (size_t i = 0; i < count; i++) {
sum += rssi_history[i];
}
return (int8_t)(sum / (int32_t)count);
}
static void add_rssi_reading(int8_t rssi)
{
rssi_history[rssi_index] = rssi;
rssi_index = (rssi_index + 1) % RSSI_HISTORY_SIZE;
if (rssi_count < RSSI_HISTORY_SIZE) {
rssi_count++;
}
}
static void reset_rssi_history(void)
{
memset(rssi_history, 0, sizeof(rssi_history));
rssi_count = 0;
rssi_index = 0;
}
static const char *mode_to_str(mlat_mode_t mode)
{
switch (mode) {
case MLAT_MODE_BLE: return "BLE";
case MLAT_MODE_WIFI: return "WiFi";
default: return "none";
}
}
/* ============================================================
* BLE CALLBACK
* ============================================================ */
static void ble_scan_cb(esp_gap_ble_cb_event_t event,
esp_ble_gap_cb_param_t *param)
{
if (!mlat_running || mlat_mode != MLAT_MODE_BLE) return;
if (event != ESP_GAP_BLE_SCAN_RESULT_EVT ||
param->scan_rst.search_evt != ESP_GAP_SEARCH_INQ_RES_EVT)
return;
/* Check if this is our target */
if (memcmp(param->scan_rst.bda, target_mac, 6) != 0)
return;
/* Store RSSI reading */
add_rssi_reading(param->scan_rst.rssi);
}
/* ============================================================
* WIFI PROMISCUOUS CALLBACK
* ============================================================ */
static void IRAM_ATTR wifi_promisc_cb(void *buf, wifi_promiscuous_pkt_type_t type)
{
if (!mlat_running || mlat_mode != MLAT_MODE_WIFI) return;
/* Only interested in management frames (probe requests, etc.) */
if (type != WIFI_PKT_MGMT) return;
wifi_promiscuous_pkt_t *pkt = (wifi_promiscuous_pkt_t *)buf;
wifi_mgmt_hdr_t *hdr = (wifi_mgmt_hdr_t *)pkt->payload;
/* Check if source MAC (addr2) matches our target */
if (memcmp(hdr->addr2, target_mac, 6) != 0) return;
/* Store RSSI reading */
add_rssi_reading(pkt->rx_ctrl.rssi);
}
/* ============================================================
* WIFI CHANNEL HOP TASK
* ============================================================ */
static void channel_hop_task(void *arg)
{
(void)arg;
while (mlat_running && mlat_mode == MLAT_MODE_WIFI) {
vTaskDelay(pdMS_TO_TICKS(CHANNEL_HOP_MS));
if (!mlat_running || mlat_mode != MLAT_MODE_WIFI) break;
current_channel = (current_channel % 13) + 1;
esp_wifi_set_channel(current_channel, WIFI_SECOND_CHAN_NONE);
}
hop_task_handle = NULL;
ESP_LOGI(TAG, "channel hop task stopped");
vTaskDelete(NULL);
}
/* ============================================================
* SEND TASK - Periodically send RSSI to C2
* ============================================================ */
static void mlat_send_task(void *arg)
{
(void)arg;
char msg[128];
while (mlat_running) {
vTaskDelay(pdMS_TO_TICKS(SEND_INTERVAL_MS));
if (!mlat_running) break;
if (rssi_count > 0) {
int8_t avg_rssi = get_average_rssi();
/*
* Send MLAT data to C2 via msg_info
* Format GPS: MLAT:G;<lat>;<lon>;<rssi>
* Format Local: MLAT:L;<x>;<y>;<rssi>
* The C2 will parse messages starting with "MLAT:" and extract the data
*/
if (coord_type == COORD_GPS) {
snprintf(msg, sizeof(msg), "MLAT:G;%.6f;%.6f;%d",
scanner_lat, scanner_lon, avg_rssi);
ESP_LOGD(TAG, "sent: GPS=(%.6f,%.6f) rssi=%d (avg of %d)",
scanner_lat, scanner_lon, avg_rssi, rssi_count);
} else {
snprintf(msg, sizeof(msg), "MLAT:L;%.2f;%.2f;%d",
scanner_x, scanner_y, avg_rssi);
ESP_LOGD(TAG, "sent: local=(%.2f,%.2f)m rssi=%d (avg of %d)",
scanner_x, scanner_y, avg_rssi, rssi_count);
}
msg_info(TAG, msg, NULL);
}
}
send_task_handle = NULL;
ESP_LOGI(TAG, "send task stopped");
vTaskDelete(NULL);
}
/* ============================================================
* BLE INIT / DEINIT
* ============================================================ */
static bool ble_init(void)
{
if (ble_initialized) {
return true;
}
esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
esp_err_t ret = esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
ESP_LOGE(TAG, "bt mem release failed: %s", esp_err_to_name(ret));
return false;
}
ret = esp_bt_controller_init(&bt_cfg);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "bt controller init failed: %s", esp_err_to_name(ret));
return false;
}
ret = esp_bt_controller_enable(ESP_BT_MODE_BLE);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "bt controller enable failed: %s", esp_err_to_name(ret));
return false;
}
ret = esp_bluedroid_init();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "bluedroid init failed: %s", esp_err_to_name(ret));
return false;
}
ret = esp_bluedroid_enable();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "bluedroid enable failed: %s", esp_err_to_name(ret));
return false;
}
ret = esp_ble_gap_register_callback(ble_scan_cb);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "gap register callback failed: %s", esp_err_to_name(ret));
return false;
}
esp_ble_scan_params_t scan_params = {
.scan_type = BLE_SCAN_TYPE_ACTIVE,
.own_addr_type = BLE_ADDR_TYPE_PUBLIC,
.scan_filter_policy = BLE_SCAN_FILTER_ALLOW_ALL,
.scan_interval = 0x50,
.scan_window = 0x30,
.scan_duplicate = BLE_SCAN_DUPLICATE_DISABLE
};
ret = esp_ble_gap_set_scan_params(&scan_params);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "set scan params failed: %s", esp_err_to_name(ret));
return false;
}
ble_initialized = true;
ESP_LOGI(TAG, "BLE initialized");
return true;
}
static bool ble_start_scan(void)
{
esp_err_t ret = esp_ble_gap_start_scanning(0); /* 0 = continuous */
if (ret != ESP_OK) {
ESP_LOGE(TAG, "start BLE scanning failed: %s", esp_err_to_name(ret));
return false;
}
return true;
}
static void ble_stop_scan(void)
{
esp_ble_gap_stop_scanning();
}
/* ============================================================
* WIFI PROMISCUOUS INIT / DEINIT
* ============================================================ */
static bool wifi_promisc_init(void)
{
if (wifi_promisc_enabled) {
return true;
}
/* Enable promiscuous mode */
esp_err_t ret = esp_wifi_set_promiscuous(true);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "set promiscuous failed: %s", esp_err_to_name(ret));
return false;
}
/* Register callback */
ret = esp_wifi_set_promiscuous_rx_cb(wifi_promisc_cb);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "set promiscuous cb failed: %s", esp_err_to_name(ret));
esp_wifi_set_promiscuous(false);
return false;
}
/* Filter only management frames */
wifi_promiscuous_filter_t filter = {
.filter_mask = WIFI_PROMIS_FILTER_MASK_MGMT
};
esp_wifi_set_promiscuous_filter(&filter);
wifi_promisc_enabled = true;
ESP_LOGI(TAG, "WiFi promiscuous mode enabled");
return true;
}
static void wifi_promisc_deinit(void)
{
if (!wifi_promisc_enabled) return;
esp_wifi_set_promiscuous(false);
wifi_promisc_enabled = false;
ESP_LOGI(TAG, "WiFi promiscuous mode disabled");
}
/* ============================================================
* START / STOP SCANNING
* ============================================================ */
static bool start_scanning(void)
{
reset_rssi_history();
if (mlat_mode == MLAT_MODE_BLE) {
if (!ble_init()) return false;
if (!ble_start_scan()) return false;
}
else if (mlat_mode == MLAT_MODE_WIFI) {
if (!wifi_promisc_init()) return false;
/* Start channel hop task for WiFi */
BaseType_t ret = xTaskCreate(
channel_hop_task,
"mlat_hop",
2048,
NULL,
4,
&hop_task_handle
);
if (ret != pdPASS) {
ESP_LOGE(TAG, "failed to create hop task");
wifi_promisc_deinit();
return false;
}
}
/* Start send task */
BaseType_t ret = xTaskCreate(
mlat_send_task,
"mlat_send",
4096,
NULL,
5,
&send_task_handle
);
if (ret != pdPASS) {
ESP_LOGE(TAG, "failed to create send task");
if (mlat_mode == MLAT_MODE_BLE) {
ble_stop_scan();
} else {
wifi_promisc_deinit();
}
return false;
}
return true;
}
static void stop_scanning(void)
{
if (mlat_mode == MLAT_MODE_BLE) {
ble_stop_scan();
}
else if (mlat_mode == MLAT_MODE_WIFI) {
wifi_promisc_deinit();
}
}
/* ============================================================
* COMMAND: mlat config <gps|local> <coord1> <coord2>
* mlat config gps <lat> <lon> - GPS coordinates (degrees)
* mlat config local <x> <y> - Local coordinates (meters)
* mlat config <lat> <lon> - Backward compat: GPS mode
* ============================================================ */
static int cmd_mlat_config(int argc, char **argv, const char *req, void *ctx)
{
(void)ctx;
if (argc < 2) {
msg_error(TAG, "usage: mlat config [gps|local] <coord1> <coord2>", req);
return -1;
}
char msg[100];
/* Check if first arg is coordinate type */
if (argc == 3 && strcasecmp(argv[0], "gps") == 0) {
/* GPS mode: mlat config gps <lat> <lon> */
double lat = strtod(argv[1], NULL);
double lon = strtod(argv[2], NULL);
if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) {
msg_error(TAG, "invalid GPS coords (lat:-90~90, lon:-180~180)", req);
return -1;
}
coord_type = COORD_GPS;
scanner_lat = lat;
scanner_lon = lon;
mlat_configured = true;
snprintf(msg, sizeof(msg), "GPS position: (%.6f, %.6f)", lat, lon);
msg_info(TAG, msg, req);
ESP_LOGI(TAG, "configured GPS: lat=%.6f lon=%.6f", scanner_lat, scanner_lon);
}
else if (argc == 3 && strcasecmp(argv[0], "local") == 0) {
/* Local mode: mlat config local <x> <y> */
double x = strtod(argv[1], NULL);
double y = strtod(argv[2], NULL);
coord_type = COORD_LOCAL;
scanner_x = x;
scanner_y = y;
mlat_configured = true;
snprintf(msg, sizeof(msg), "Local position: (%.2f, %.2f) meters", x, y);
msg_info(TAG, msg, req);
ESP_LOGI(TAG, "configured local: x=%.2f y=%.2f", scanner_x, scanner_y);
}
else if (argc == 2) {
/* Backward compat: mlat config <lat> <lon> -> GPS mode */
double lat = strtod(argv[0], NULL);
double lon = strtod(argv[1], NULL);
if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) {
msg_error(TAG, "invalid GPS coords (lat:-90~90, lon:-180~180)", req);
return -1;
}
coord_type = COORD_GPS;
scanner_lat = lat;
scanner_lon = lon;
mlat_configured = true;
snprintf(msg, sizeof(msg), "GPS position: (%.6f, %.6f)", lat, lon);
msg_info(TAG, msg, req);
ESP_LOGI(TAG, "configured GPS: lat=%.6f lon=%.6f", scanner_lat, scanner_lon);
}
else {
msg_error(TAG, "usage: mlat config [gps|local] <coord1> <coord2>", req);
return -1;
}
return 0;
}
/* ============================================================
* COMMAND: mlat mode <ble|wifi>
* ============================================================ */
static int cmd_mlat_mode(int argc, char **argv, const char *req, void *ctx)
{
(void)ctx;
if (argc != 1) {
msg_error(TAG, "usage: mlat mode <ble|wifi>", req);
return -1;
}
if (mlat_running) {
msg_error(TAG, "stop scanning first", req);
return -1;
}
const char *mode_str = argv[0];
if (strcasecmp(mode_str, "ble") == 0) {
mlat_mode = MLAT_MODE_BLE;
}
else if (strcasecmp(mode_str, "wifi") == 0) {
mlat_mode = MLAT_MODE_WIFI;
}
else {
msg_error(TAG, "invalid mode (use: ble, wifi)", req);
return -1;
}
char msg[32];
snprintf(msg, sizeof(msg), "mode set to %s", mode_to_str(mlat_mode));
msg_info(TAG, msg, req);
ESP_LOGI(TAG, "mode changed to %s", mode_to_str(mlat_mode));
return 0;
}
/* ============================================================
* COMMAND: mlat start <mac>
* ============================================================ */
static int cmd_mlat_start(int argc, char **argv, const char *req, void *ctx)
{
(void)ctx;
if (argc != 1) {
msg_error(TAG, "usage: mlat start <mac>", req);
return -1;
}
if (mlat_running) {
msg_error(TAG, "already running", req);
return -1;
}
if (!mlat_configured) {
msg_error(TAG, "not configured - run 'mlat config [gps|local] <c1> <c2>' first", req);
return -1;
}
/* Parse target MAC */
if (!parse_mac_str(argv[0], target_mac)) {
msg_error(TAG, "invalid MAC address", req);
return -1;
}
mac_to_str(target_mac, target_mac_str, sizeof(target_mac_str));
mlat_running = true;
if (!start_scanning()) {
mlat_running = false;
msg_error(TAG, "scan start failed", req);
return -1;
}
char msg[128];
if (coord_type == COORD_GPS) {
snprintf(msg, sizeof(msg), "scanning for %s at GPS(%.6f, %.6f) [%s]",
target_mac_str, scanner_lat, scanner_lon, mode_to_str(mlat_mode));
ESP_LOGI(TAG, "started: target=%s GPS=(%.6f,%.6f) mode=%s",
target_mac_str, scanner_lat, scanner_lon, mode_to_str(mlat_mode));
} else {
snprintf(msg, sizeof(msg), "scanning for %s at local(%.2f, %.2f)m [%s]",
target_mac_str, scanner_x, scanner_y, mode_to_str(mlat_mode));
ESP_LOGI(TAG, "started: target=%s local=(%.2f,%.2f)m mode=%s",
target_mac_str, scanner_x, scanner_y, mode_to_str(mlat_mode));
}
msg_info(TAG, msg, req);
return 0;
}
/* ============================================================
* COMMAND: mlat stop
* ============================================================ */
static int cmd_mlat_stop(int argc, char **argv, const char *req, void *ctx)
{
(void)argc;
(void)argv;
(void)ctx;
if (!mlat_running) {
msg_error(TAG, "not running", req);
return -1;
}
mlat_running = false;
stop_scanning();
msg_info(TAG, "stopped", req);
ESP_LOGI(TAG, "stopped");
return 0;
}
/* ============================================================
* COMMAND: mlat status
* ============================================================ */
static int cmd_mlat_status(int argc, char **argv, const char *req, void *ctx)
{
(void)argc;
(void)argv;
(void)ctx;
char msg[180];
const char *coord_str = (coord_type == COORD_GPS) ? "GPS" : "Local";
if (!mlat_configured) {
snprintf(msg, sizeof(msg), "not configured | mode=%s", mode_to_str(mlat_mode));
msg_info(TAG, msg, req);
return 0;
}
/* Format position based on coord type */
char pos_str[60];
if (coord_type == COORD_GPS) {
snprintf(pos_str, sizeof(pos_str), "GPS=(%.6f,%.6f)", scanner_lat, scanner_lon);
} else {
snprintf(pos_str, sizeof(pos_str), "local=(%.2f,%.2f)m", scanner_x, scanner_y);
}
if (mlat_running) {
int8_t avg = get_average_rssi();
if (mlat_mode == MLAT_MODE_WIFI) {
snprintf(msg, sizeof(msg),
"running [%s] | %s | target=%s | rssi=%d (%d) | ch=%d",
mode_to_str(mlat_mode), pos_str,
target_mac_str, avg, rssi_count, current_channel);
} else {
snprintf(msg, sizeof(msg),
"running [%s] | %s | target=%s | rssi=%d (%d samples)",
mode_to_str(mlat_mode), pos_str,
target_mac_str, avg, rssi_count);
}
} else {
snprintf(msg, sizeof(msg),
"stopped | mode=%s | %s",
mode_to_str(mlat_mode), pos_str);
}
msg_info(TAG, msg, req);
return 0;
}
/* ============================================================
* COMMAND DEFINITIONS
* ============================================================ */
static const command_t cmd_mlat_config_def = {
.name = "mlat",
.sub = "config",
.help = "Set position: mlat config [gps|local] <c1> <c2>",
.handler = cmd_mlat_config,
.ctx = NULL,
.async = false,
.min_args = 2,
.max_args = 3
};
static const command_t cmd_mlat_mode_def = {
.name = "mlat",
.sub = "mode",
.help = "Set scan mode: mlat mode <ble|wifi>",
.handler = cmd_mlat_mode,
.ctx = NULL,
.async = false,
.min_args = 1,
.max_args = 1
};
static const command_t cmd_mlat_start_def = {
.name = "mlat",
.sub = "start",
.help = "Start scanning: mlat start <mac>",
.handler = cmd_mlat_start,
.ctx = NULL,
.async = false,
.min_args = 1,
.max_args = 1
};
static const command_t cmd_mlat_stop_def = {
.name = "mlat",
.sub = "stop",
.help = "Stop scanning",
.handler = cmd_mlat_stop,
.ctx = NULL,
.async = false,
.min_args = 0,
.max_args = 0
};
static const command_t cmd_mlat_status_def = {
.name = "mlat",
.sub = "status",
.help = "Show MLAT status",
.handler = cmd_mlat_status,
.ctx = NULL,
.async = false,
.min_args = 0,
.max_args = 0
};
/* ============================================================
* REGISTER
* ============================================================ */
void mod_mlat_register_commands(void)
{
command_register(&cmd_mlat_config_def);
command_register(&cmd_mlat_mode_def);
command_register(&cmd_mlat_start_def);
command_register(&cmd_mlat_stop_def);
command_register(&cmd_mlat_status_def);
ESP_LOGI(TAG, "commands registered (BLE+WiFi)");
}
#endif /* CONFIG_RECON_MODE_MLAT */
Reference in New Issue View Git Blame Copy Permalink