Add CAN bus sniffer functionality with filtering
This commit is contained in:
Finn
303
dumperwithfilter.ino
Normal file
303
dumperwithfilter.ino
Normal file
@@ -0,0 +1,303 @@
|
||||
#include <SPI.h>
|
||||
#include "mcp_can.h"
|
||||
|
||||
const uint8_t CAN_CS_PIN = 8;
|
||||
const uint8_t CAN_INT_PIN = 2;
|
||||
|
||||
MCP_CAN CAN(CAN_CS_PIN);
|
||||
|
||||
const byte CAN_SPEED = CAN_500KBPS;
|
||||
const byte OSC = MCP_8MHZ;
|
||||
|
||||
const size_t MAX_IDS = 256;
|
||||
uint32_t seenIds[MAX_IDS];
|
||||
uint16_t seenCounts[MAX_IDS];
|
||||
size_t idsUsed = 0;
|
||||
|
||||
const size_t MAX_FILTER_IDS = 16;
|
||||
unsigned long filterIds[MAX_FILTER_IDS];
|
||||
size_t filterCount = 0;
|
||||
bool filterEnabled = false; // set true to enable filtering at startup
|
||||
|
||||
char lineBuf[64];
|
||||
size_t lineLen = 0;
|
||||
|
||||
inline uint32_t packId(unsigned long id, bool extended) {
|
||||
return (extended ? 0x80000000UL : 0) | (id & 0x1FFFFFFFUL);
|
||||
}
|
||||
inline bool isExtendedPacked(uint32_t packed) { return (packed & 0x80000000UL) != 0; }
|
||||
inline unsigned long unpackId(uint32_t packed) { return packed & 0x1FFFFFFFUL; }
|
||||
|
||||
int16_t findSeenIndex(uint32_t packed) {
|
||||
for (size_t i = 0; i < idsUsed; i++) {
|
||||
if (seenIds[i] == packed) return (int16_t)i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool rememberId(uint32_t packed) {
|
||||
int16_t idx = findSeenIndex(packed);
|
||||
if (idx >= 0) {
|
||||
if (seenCounts[idx] != 0xFFFF) seenCounts[idx]++;
|
||||
return false;
|
||||
}
|
||||
if (idsUsed < MAX_IDS) {
|
||||
seenIds[idsUsed] = packed;
|
||||
seenCounts[idsUsed] = 1;
|
||||
idsUsed++;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void printSummary() {
|
||||
Serial.print(F("\nSummary: "));
|
||||
Serial.print(idsUsed);
|
||||
Serial.println(F(" unique IDs seen."));
|
||||
for (size_t i = 0; i < idsUsed; i++) {
|
||||
bool ext = isExtendedPacked(seenIds[i]);
|
||||
unsigned long id = unpackId(seenIds[i]);
|
||||
Serial.print(F(" ID:0x"));
|
||||
Serial.print(id, HEX);
|
||||
if (ext) Serial.print(F(" (ext)"));
|
||||
Serial.print(F(" count:"));
|
||||
Serial.println(seenCounts[i]);
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
bool idInFilter(unsigned long id) {
|
||||
if (!filterEnabled || filterCount == 0) return true;
|
||||
for (size_t i = 0; i < filterCount; i++) {
|
||||
if (filterIds[i] == id) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool addFilterId(unsigned long id) {
|
||||
for (size_t i = 0; i < filterCount; i++) {
|
||||
if (filterIds[i] == id) return false;
|
||||
}
|
||||
if (filterCount >= MAX_FILTER_IDS) return false;
|
||||
filterIds[filterCount++] = id;
|
||||
return true;
|
||||
}
|
||||
|
||||
void clearFilter() {
|
||||
filterCount = 0;
|
||||
}
|
||||
|
||||
void listFilter() {
|
||||
Serial.print(F("Filter "));
|
||||
Serial.print(filterEnabled ? F("ENABLED") : F("DISABLED"));
|
||||
Serial.print(F(". IDs ("));
|
||||
Serial.print(filterCount);
|
||||
Serial.println(F("):"));
|
||||
for (size_t i = 0; i < filterCount; i++) {
|
||||
Serial.print(F(" 0x"));
|
||||
Serial.println(filterIds[i], HEX);
|
||||
}
|
||||
if (filterCount == 0) Serial.println(F(" <none>"));
|
||||
}
|
||||
|
||||
static void trimInPlace(char* s) {
|
||||
while (*s == ' ' || *s == '\t') { memmove(s, s+1, strlen(s)); }
|
||||
size_t n = strlen(s);
|
||||
while (n && (s[n-1] == ' ' || s[n-1] == '\t')) { s[--n] = 0; }
|
||||
}
|
||||
|
||||
static char* nextToken(char* &p) {
|
||||
while (*p == ' ' || *p == '\t') p++;
|
||||
if (*p == 0) return nullptr;
|
||||
char* start = p;
|
||||
while (*p && *p != ' ' && *p != '\t') p++;
|
||||
if (*p) { *p = 0; p++; }
|
||||
return start;
|
||||
}
|
||||
|
||||
bool parseHexToken(const char* tok, unsigned long &out) {
|
||||
if (!tok || !*tok) return false;
|
||||
// optional 0x/0X
|
||||
if ((tok[0] == '0') && (tok[1] == 'x' || tok[1] == 'X')) tok += 2;
|
||||
char* endp = nullptr;
|
||||
unsigned long val = strtoul(tok, &endp, 16);
|
||||
if (endp == tok) return false;
|
||||
out = val;
|
||||
return true;
|
||||
}
|
||||
|
||||
void printHelp() {
|
||||
Serial.println(F("\nCommands:"));
|
||||
Serial.println(F(" s -> summary of all IDs seen"));
|
||||
Serial.println(F(" c -> clear remembered IDs"));
|
||||
Serial.println(F(" fe -> enable show-only filter"));
|
||||
Serial.println(F(" fd -> disable show-only filter"));
|
||||
Serial.println(F(" fl -> list filter IDs"));
|
||||
Serial.println(F(" fx -> clear filter IDs"));
|
||||
Serial.println(F(" f <hex> -> set single filter to <hex> (replaces list)"));
|
||||
Serial.println(F(" a <hex> -> add <hex> to filter list"));
|
||||
Serial.println(F("Examples: f 100 (only show 0x100), a 203 (also show 0x203), fe"));
|
||||
}
|
||||
|
||||
void handleCommandLine(char* line) {
|
||||
trimInPlace(line);
|
||||
if (line[0] == 0) return;
|
||||
|
||||
Serial.print(F("> "));
|
||||
Serial.println(line);
|
||||
|
||||
char* p = line;
|
||||
char* cmd = nextToken(p);
|
||||
|
||||
// Single-letter quick commands
|
||||
if (strcmp(cmd, "s") == 0 || strcmp(cmd, "S") == 0) {
|
||||
printSummary();
|
||||
return;
|
||||
}
|
||||
if (strcmp(cmd, "c") == 0 || strcmp(cmd, "C") == 0) {
|
||||
idsUsed = 0;
|
||||
Serial.println(F("Cleared remembered IDs."));
|
||||
return;
|
||||
}
|
||||
|
||||
if (strcasecmp(cmd, "fe") == 0) {
|
||||
filterEnabled = true;
|
||||
Serial.println(F("Filter ENABLED."));
|
||||
listFilter();
|
||||
return;
|
||||
}
|
||||
if (strcasecmp(cmd, "fd") == 0) {
|
||||
filterEnabled = false;
|
||||
Serial.println(F("Filter DISABLED. All IDs will be shown."));
|
||||
return;
|
||||
}
|
||||
if (strcasecmp(cmd, "fl") == 0) {
|
||||
listFilter();
|
||||
return;
|
||||
}
|
||||
if (strcasecmp(cmd, "fx") == 0) {
|
||||
clearFilter();
|
||||
Serial.println(F("Filter list cleared."));
|
||||
return;
|
||||
}
|
||||
|
||||
// Commands with arguments
|
||||
if (strcasecmp(cmd, "f") == 0) {
|
||||
// set single filter ID
|
||||
char* tok = nextToken(p);
|
||||
unsigned long id;
|
||||
if (parseHexToken(tok, id)) {
|
||||
clearFilter();
|
||||
addFilterId(id);
|
||||
filterEnabled = true;
|
||||
Serial.print(F("Filter set to only show ID 0x"));
|
||||
Serial.println(id, HEX);
|
||||
return;
|
||||
}
|
||||
Serial.println(F("Invalid hex after 'f'. Example: f 100"));
|
||||
return;
|
||||
}
|
||||
|
||||
if (strcasecmp(cmd, "a") == 0) {
|
||||
char* tok = nextToken(p);
|
||||
unsigned long id;
|
||||
if (parseHexToken(tok, id)) {
|
||||
if (addFilterId(id)) {
|
||||
Serial.print(F("Added 0x"));
|
||||
Serial.print(id, HEX);
|
||||
Serial.println(F(" to filter list."));
|
||||
} else {
|
||||
Serial.println(F("Failed to add (duplicate or list full)."));
|
||||
}
|
||||
filterEnabled = true;
|
||||
return;
|
||||
}
|
||||
Serial.println(F("Invalid hex after 'a'. Example: a 203"));
|
||||
return;
|
||||
}
|
||||
printHelp();
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
delay(50);
|
||||
|
||||
pinMode(CAN_INT_PIN, INPUT);
|
||||
|
||||
if (CAN.begin(MCP_ANY, CAN_SPEED, OSC) != CAN_OK) {
|
||||
Serial.println(F("CAN init failed. Check wiring, bitrate, and 8 MHz oscillator."));
|
||||
while (1) { delay(1000); }
|
||||
}
|
||||
|
||||
CAN.setMode(MCP_LISTENONLY); // sniffer mode
|
||||
Serial.println(F("CAN listen-only started."));
|
||||
printHelp();
|
||||
listFilter();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
while (Serial.available() > 0) {
|
||||
char ch = (char)Serial.read();
|
||||
if (ch == '\r') {
|
||||
if (lineLen > 0) {
|
||||
lineBuf[lineLen] = 0;
|
||||
handleCommandLine(lineBuf);
|
||||
lineLen = 0;
|
||||
}
|
||||
} else if (ch == '\n') {
|
||||
lineBuf[lineLen] = 0;
|
||||
handleCommandLine(lineBuf);
|
||||
lineLen = 0;
|
||||
} else {
|
||||
if (lineLen < sizeof(lineBuf) - 1) {
|
||||
lineBuf[lineLen++] = ch;
|
||||
} else {
|
||||
lineBuf[lineLen] = 0;
|
||||
handleCommandLine(lineBuf);
|
||||
lineLen = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
while (CAN_MSGAVAIL == CAN.checkReceive()) {
|
||||
unsigned long id = 0;
|
||||
byte ext = 0;
|
||||
byte len = 0;
|
||||
byte buf[8];
|
||||
if (CAN.readMsgBuf(&id, &ext, &len, buf) != CAN_OK) {
|
||||
continue;
|
||||
}
|
||||
bool extended = (ext != 0);
|
||||
|
||||
uint32_t packed = packId(id, extended);
|
||||
bool isNew = rememberId(packed);
|
||||
|
||||
if (!idInFilter(id)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Dump the frame
|
||||
Serial.print(millis());
|
||||
Serial.print(F("ms "));
|
||||
Serial.print(F("ID:0x"));
|
||||
Serial.print(id, HEX);
|
||||
if (extended) Serial.print(F(" (ext)"));
|
||||
Serial.print(F(" DLC:"));
|
||||
Serial.print(len);
|
||||
Serial.print(F(" DATA:"));
|
||||
for (byte i = 0; i < len; i++) {
|
||||
if (buf[i] < 16) Serial.print('0');
|
||||
Serial.print(buf[i], HEX);
|
||||
Serial.print(' ');
|
||||
}
|
||||
Serial.println();
|
||||
|
||||
// Optional: notify when an ID is seen for the first time
|
||||
if (isNew) {
|
||||
//Serial.print(F(" New ID observed -> 0x"));
|
||||
//Serial.print(id, HEX);
|
||||
//if (extended) Serial.print(F(" (ext)"));
|
||||
//Serial.print(F(" | total unique: "));
|
||||
//Serial.println(idsUsed);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user