Ryan Malloy
2aaa6e32a5
Library dir: lib/AutoWire/ -> lib/KLine/ Site title, docs, CLAUDE.md, platformio.ini all updated. All 4 firmware environments build clean.
459 lines
13 KiB
C++
459 lines
13 KiB
C++
// OBD-II K-line protocol handler (ISO 9141 / ISO 14230)
|
|
// 5-baud slow init, fast init, request/response with echo clearing.
|
|
//
|
|
// Post-review hardening: proper pin detach (C1), timeout guards (C2),
|
|
// checksum validation (C3), echo verification (C4), RX flush (I1),
|
|
// structural frame parsing (I2/I3/I5), negative response handling.
|
|
|
|
#include "KLineObd2.h"
|
|
#include "Obd2Pids.h"
|
|
#include <string.h>
|
|
|
|
KLineObd2::KLineObd2(KLineTransport& transport)
|
|
: _transport(transport),
|
|
_initialized(false),
|
|
_kw1(0),
|
|
_kw2(0),
|
|
_testerAddr(obd2::TESTER_ADDR),
|
|
_ecuAddr(0),
|
|
_lastRequestMs(0) {}
|
|
|
|
// --- UART pin management for init sequences ---
|
|
|
|
void KLineObd2::detachUartTx() {
|
|
// Disconnect UART TX peripheral signal from the GPIO pin.
|
|
// pinMatrixOutDetach() routes the plain GPIO output register
|
|
// to the pin instead of the UART TX signal, so digitalWrite()
|
|
// has full control. Without this, UART and GPIO fight over the pin.
|
|
pinMatrixOutDetach(_transport.txPin(), false, false);
|
|
}
|
|
|
|
void KLineObd2::reattachUartTx() {
|
|
// Re-connect UART TX signal to the GPIO pin for normal serial I/O
|
|
uart_set_pin((uart_port_t)_transport.uartNum(),
|
|
_transport.txPin(), UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
}
|
|
|
|
// --- Timeout helper ---
|
|
|
|
uint16_t KLineObd2::remainingMs(unsigned long startMs, uint16_t timeoutMs) {
|
|
unsigned long elapsed = millis() - startMs;
|
|
return (elapsed < timeoutMs) ? (uint16_t)(timeoutMs - elapsed) : 0;
|
|
}
|
|
|
|
// --- 5-baud slow init (ISO 9141-2) ---
|
|
// Bit-bang the target address at 5 baud on the TX pin.
|
|
//
|
|
// Sequence:
|
|
// 1. TX pin LOW for 200ms (start bit at 5 baud)
|
|
// 2. 8 data bits of targetAddr, LSB first, 200ms each
|
|
// 3. TX pin HIGH for 200ms (stop bit)
|
|
// 4. Re-attach UART, flush RX, wait for ECU response
|
|
// 5. ECU sends: 0x55 (sync), keyword1, keyword2
|
|
// 6. Tester sends: inverted keyword2
|
|
// 7. ECU sends: inverted tester address (0xCC for 0x33)
|
|
|
|
bool KLineObd2::slowInit(uint8_t targetAddr) {
|
|
// Detach UART TX so we can bit-bang
|
|
detachUartTx();
|
|
|
|
// Configure TX pin as GPIO output, start HIGH (idle)
|
|
int8_t txp = _transport.txPin();
|
|
pinMode(txp, OUTPUT);
|
|
digitalWrite(txp, HIGH);
|
|
delay(300); // W0: idle before init (>300ms per ISO 9141)
|
|
|
|
// Bit-bang target address at 5 baud
|
|
sendByte5Baud(targetAddr);
|
|
|
|
// Re-attach UART to TX pin
|
|
reattachUartTx();
|
|
|
|
// Flush stale bytes — UART RX was receiving framing errors
|
|
// during the 2-second bit-bang period
|
|
_transport.flushRx();
|
|
|
|
// Wait for ECU sync byte (0x55) — W1: 20-300ms per ISO 9141
|
|
int sync = readByteTimeout(300);
|
|
if (sync != obd2::SYNC_BYTE) {
|
|
return false;
|
|
}
|
|
|
|
// Read keyword bytes — W2: 5-20ms between bytes
|
|
int kw1 = readByteTimeout(50);
|
|
if (kw1 < 0) return false;
|
|
_kw1 = (uint8_t)kw1;
|
|
|
|
int kw2 = readByteTimeout(50);
|
|
if (kw2 < 0) return false;
|
|
_kw2 = (uint8_t)kw2;
|
|
|
|
// W3: 25-50ms before tester responds
|
|
delay(30);
|
|
|
|
// Send inverted keyword2 back to ECU and verify echo
|
|
uint8_t invKw2 = ~_kw2;
|
|
_transport.serial()->write(invKw2);
|
|
_transport.serial()->flush();
|
|
if (!clearEcho(&invKw2, 1)) {
|
|
return false; // bus contention during handshake
|
|
}
|
|
|
|
// W4: ECU confirms by sending inverted target address
|
|
int ack = readByteTimeout(50);
|
|
if (ack < 0) return false;
|
|
|
|
// The ack should be the complement of the target address
|
|
if ((uint8_t)ack != (uint8_t)~targetAddr) {
|
|
return false;
|
|
}
|
|
|
|
_ecuAddr = targetAddr;
|
|
_initialized = true;
|
|
_lastRequestMs = millis();
|
|
return true;
|
|
}
|
|
|
|
// --- Fast init (ISO 14230-2, KWP2000) ---
|
|
// 25ms LOW + 25ms HIGH "TiniPulse" on TX, then StartComm request.
|
|
|
|
bool KLineObd2::fastInit() {
|
|
// Detach UART TX for the wake-up pulse
|
|
detachUartTx();
|
|
|
|
int8_t txp = _transport.txPin();
|
|
pinMode(txp, OUTPUT);
|
|
digitalWrite(txp, HIGH);
|
|
delay(300); // idle before pulse
|
|
|
|
// TiniPulse: 25ms LOW + 25ms HIGH
|
|
digitalWrite(txp, LOW);
|
|
delay(25);
|
|
digitalWrite(txp, HIGH);
|
|
delay(25);
|
|
|
|
// Re-attach UART and flush stale RX data
|
|
reattachUartTx();
|
|
_transport.flushRx();
|
|
|
|
// Build StartCommunication request (KWP2000 service 0x81)
|
|
uint8_t startComm[] = {
|
|
0xC1, // format: functional addressing, 1 data byte
|
|
obd2::FUNC_ADDR, // target: functional address
|
|
obd2::TESTER_ADDR, // source: tester
|
|
0x81 // StartCommunication service
|
|
};
|
|
uint8_t checksum = KLineTransport::checksumMod256(startComm, 4);
|
|
|
|
// Full message with checksum for echo verification
|
|
uint8_t fullMsg[5];
|
|
memcpy(fullMsg, startComm, 4);
|
|
fullMsg[4] = checksum;
|
|
|
|
for (uint8_t i = 0; i < 5; i++) {
|
|
_transport.serial()->write(fullMsg[i]);
|
|
}
|
|
_transport.serial()->flush();
|
|
|
|
// Verify echo (5 bytes: 4 data + 1 checksum)
|
|
if (!clearEcho(fullMsg, 5)) {
|
|
return false; // bus contention during fast init
|
|
}
|
|
|
|
// Read StartComm positive response
|
|
uint8_t resp[12];
|
|
uint8_t respLen = readResponse(resp, sizeof(resp), 300);
|
|
if (respLen < 4) return false;
|
|
|
|
// Parse response structurally using ISO 14230 format byte
|
|
uint8_t dLen = 0;
|
|
int8_t dataStart = parseFrameData(resp, respLen, &dLen);
|
|
if (dataStart < 0) return false;
|
|
if (dLen < 3) return false; // need service_id + kw1 + kw2
|
|
|
|
// Verify StartComm positive response (0x81 + 0x40 = 0xC1)
|
|
if (resp[dataStart] != 0xC1) return false;
|
|
|
|
_kw1 = resp[dataStart + 1];
|
|
_kw2 = resp[dataStart + 2];
|
|
|
|
_ecuAddr = obd2::FUNC_ADDR;
|
|
_initialized = true;
|
|
_lastRequestMs = millis();
|
|
return true;
|
|
}
|
|
|
|
// --- Request/response ---
|
|
|
|
void KLineObd2::sendRequest(const uint8_t* data, uint8_t len) {
|
|
if (len > KLINE_MAX_MSG) return;
|
|
|
|
// Build complete message with checksum for echo verification
|
|
uint8_t buf[KLINE_MAX_MSG + 1];
|
|
memcpy(buf, data, len);
|
|
buf[len] = KLineTransport::checksumMod256(data, len);
|
|
uint8_t totalLen = len + 1;
|
|
|
|
for (uint8_t i = 0; i < totalLen; i++) {
|
|
_transport.serial()->write(buf[i]);
|
|
}
|
|
_transport.serial()->flush();
|
|
|
|
// Verify echo (data bytes + checksum byte)
|
|
clearEcho(buf, totalLen);
|
|
|
|
_lastRequestMs = millis();
|
|
}
|
|
|
|
uint8_t KLineObd2::readResponse(uint8_t* buf, uint8_t maxLen, uint16_t timeoutMs) {
|
|
uint8_t count = 0;
|
|
unsigned long start = millis();
|
|
|
|
// Read first byte (format/header) to determine message length
|
|
int first = readByteTimeout(timeoutMs);
|
|
if (first < 0) return 0;
|
|
buf[count++] = (uint8_t)first;
|
|
|
|
// Determine expected length from format byte
|
|
uint8_t fmt = (uint8_t)first;
|
|
uint8_t addrMode = (fmt >> 6) & 0x03;
|
|
uint8_t dataLen = fmt & 0x3F;
|
|
|
|
// Determine address byte count from address mode
|
|
uint8_t addrBytes;
|
|
switch (addrMode) {
|
|
case 0: // no address information
|
|
addrBytes = 0;
|
|
break;
|
|
case 1: // CARB mode — unsupported, treat as 0 extra
|
|
addrBytes = 0;
|
|
break;
|
|
case 2: // physical addressing: target + source
|
|
case 3: // functional addressing: target + source
|
|
default:
|
|
addrBytes = 2;
|
|
break;
|
|
}
|
|
|
|
// Read address bytes if present
|
|
for (uint8_t i = 0; i < addrBytes && count < maxLen; i++) {
|
|
uint16_t rem = remainingMs(start, timeoutMs);
|
|
if (rem == 0) return count;
|
|
int b = readByteTimeout(rem);
|
|
if (b < 0) return count;
|
|
buf[count++] = (uint8_t)b;
|
|
}
|
|
|
|
// If dataLen == 0, next byte is the actual length
|
|
if (dataLen == 0 && count < maxLen) {
|
|
uint16_t rem = remainingMs(start, timeoutMs);
|
|
if (rem == 0) return count;
|
|
int lenByte = readByteTimeout(rem);
|
|
if (lenByte < 0) return count;
|
|
buf[count++] = (uint8_t)lenByte;
|
|
dataLen = (uint8_t)lenByte;
|
|
}
|
|
|
|
// Read data bytes + checksum
|
|
uint8_t expected = dataLen + 1; // +1 for checksum
|
|
for (uint8_t i = 0; i < expected; i++) {
|
|
if (count >= maxLen) {
|
|
#ifdef KLINE_DEBUG
|
|
Serial.println("KLINE: response buffer full");
|
|
#endif
|
|
return count; // truncated — can't validate checksum
|
|
}
|
|
uint16_t rem = remainingMs(start, timeoutMs);
|
|
if (rem == 0) return count;
|
|
int b = readByteTimeout(rem);
|
|
if (b < 0) return count;
|
|
buf[count++] = (uint8_t)b;
|
|
}
|
|
|
|
// Validate checksum (mod256 sum of all bytes except last)
|
|
if (count >= 2) {
|
|
uint8_t computed = KLineTransport::checksumMod256(buf, count - 1);
|
|
if (computed != buf[count - 1]) {
|
|
#ifdef KLINE_DEBUG
|
|
Serial.println("KLINE: checksum fail");
|
|
#endif
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
uint8_t KLineObd2::requestPid(uint8_t mode, uint8_t pid,
|
|
uint8_t* response, uint8_t maxLen) {
|
|
if (!_initialized) return 0;
|
|
|
|
// Send TesterPresent keepalive if idle too long (P3 timeout prevention)
|
|
if (_lastRequestMs > 0 && millis() - _lastRequestMs > P3_KEEPALIVE_MS) {
|
|
sendTesterPresent();
|
|
}
|
|
|
|
// ISO 14230 format: physical addressing, 2 data bytes
|
|
uint8_t req[] = {
|
|
(uint8_t)(0x82), // fmt: physical addressing, 2 data bytes
|
|
_ecuAddr, // target
|
|
_testerAddr, // source
|
|
mode, // service/mode
|
|
pid // PID
|
|
};
|
|
|
|
sendRequest(req, sizeof(req));
|
|
|
|
uint8_t raw[32];
|
|
uint8_t rawLen = readResponse(raw, sizeof(raw));
|
|
if (rawLen < 5) return 0; // minimum: fmt + target + source + response_mode + pid
|
|
|
|
// Parse response structurally
|
|
uint8_t dLen = 0;
|
|
int8_t dataStart = parseFrameData(raw, rawLen, &dLen);
|
|
if (dataStart < 0 || dLen < 2) return 0;
|
|
|
|
// Check for negative response (service 0x7F)
|
|
if (raw[dataStart] == 0x7F) {
|
|
#ifdef KLINE_DEBUG
|
|
if (dLen >= 3) {
|
|
Serial.print("KLINE: NRC 0x");
|
|
Serial.println(raw[dataStart + 2], HEX);
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
// Verify positive response: service ID = request mode + 0x40
|
|
uint8_t respMode = mode + 0x40;
|
|
if (raw[dataStart] != respMode) return 0;
|
|
if (raw[dataStart + 1] != pid) return 0;
|
|
|
|
// Copy data bytes after service_id + pid
|
|
uint8_t copyLen = 0;
|
|
for (uint8_t i = 2; i < dLen && copyLen < maxLen; i++) {
|
|
response[copyLen++] = raw[dataStart + i];
|
|
}
|
|
return copyLen;
|
|
}
|
|
|
|
// --- TesterPresent keepalive ---
|
|
|
|
void KLineObd2::sendTesterPresent() {
|
|
if (!_initialized) return;
|
|
|
|
// ISO 14230: TesterPresent service (0x3E), no sub-function
|
|
uint8_t req[] = {
|
|
(uint8_t)(0x81), // fmt: physical addressing, 1 data byte
|
|
_ecuAddr,
|
|
_testerAddr,
|
|
0x3E // TesterPresent service
|
|
};
|
|
|
|
sendRequest(req, sizeof(req));
|
|
|
|
// Read and discard the positive response
|
|
uint8_t resp[8];
|
|
readResponse(resp, sizeof(resp), 500);
|
|
}
|
|
|
|
// --- Private helpers ---
|
|
|
|
void KLineObd2::sendByte5Baud(uint8_t data) {
|
|
int8_t txp = _transport.txPin();
|
|
|
|
// Start bit (LOW)
|
|
digitalWrite(txp, LOW);
|
|
delay(200);
|
|
|
|
// 8 data bits, LSB first
|
|
for (uint8_t i = 0; i < 8; i++) {
|
|
digitalWrite(txp, (data >> i) & 0x01 ? HIGH : LOW);
|
|
delay(200);
|
|
}
|
|
|
|
// Stop bit (HIGH)
|
|
digitalWrite(txp, HIGH);
|
|
delay(200);
|
|
}
|
|
|
|
bool KLineObd2::clearEcho(const uint8_t* expected, uint8_t count) {
|
|
bool match = true;
|
|
for (uint8_t i = 0; i < count; i++) {
|
|
int b = readByteTimeout(KLINE_ECHO_TIMEOUT_MS);
|
|
if (b < 0) {
|
|
#ifdef KLINE_DEBUG
|
|
Serial.print("KLINE: echo timeout byte ");
|
|
Serial.println(i);
|
|
#endif
|
|
match = false;
|
|
continue;
|
|
}
|
|
if ((uint8_t)b != expected[i]) {
|
|
#ifdef KLINE_DEBUG
|
|
Serial.print("KLINE: echo mismatch byte ");
|
|
Serial.print(i);
|
|
Serial.print(" exp=0x");
|
|
Serial.print(expected[i], HEX);
|
|
Serial.print(" got=0x");
|
|
Serial.println((uint8_t)b, HEX);
|
|
#endif
|
|
match = false;
|
|
}
|
|
}
|
|
return match;
|
|
}
|
|
|
|
int KLineObd2::readByteTimeout(uint16_t timeoutMs) {
|
|
if (timeoutMs == 0) return -1;
|
|
unsigned long start = millis();
|
|
while (millis() - start < timeoutMs) {
|
|
_transport.drainUart();
|
|
if (_transport.rxAvailable() > 0) {
|
|
return _transport.rxRead();
|
|
}
|
|
delayMicroseconds(100); // prevent busy-wait burning CPU
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int8_t KLineObd2::parseFrameData(const uint8_t* buf, uint8_t bufLen, uint8_t* dataLen) {
|
|
if (bufLen < 1) return -1;
|
|
|
|
uint8_t fmt = buf[0];
|
|
uint8_t addrMode = (fmt >> 6) & 0x03;
|
|
uint8_t inlineLen = fmt & 0x3F;
|
|
|
|
uint8_t headerSize = 1; // format byte
|
|
|
|
switch (addrMode) {
|
|
case 0: // no address bytes
|
|
break;
|
|
case 1: // CARB exception — not supported
|
|
#ifdef KLINE_DEBUG
|
|
Serial.println("KLINE: CARB mode unsupported");
|
|
#endif
|
|
return -1;
|
|
case 2: // physical addressing: target + source
|
|
case 3: // functional addressing: target + source
|
|
headerSize += 2;
|
|
break;
|
|
}
|
|
|
|
uint8_t len;
|
|
if (inlineLen > 0) {
|
|
len = inlineLen;
|
|
} else {
|
|
// Length in separate byte after address bytes
|
|
if (bufLen < headerSize + 1) return -1;
|
|
len = buf[headerSize];
|
|
headerSize += 1;
|
|
}
|
|
|
|
if (dataLen) *dataLen = len;
|
|
|
|
if (headerSize > bufLen) return -1;
|
|
return (int8_t)headerSize;
|
|
}
|