Ryan Malloy
a2dcd6d58d
Fix UART pin detach: pinMatrixOutDetach() instead of no-op uart_set_pin(ALL_NO_CHANGE). Fix timeout underflow in readResponse() with remainingMs() helper. Add checksum validation on received frames. Echo verification in clearEcho() detects bus contention. Flush RX buffers after init sequences. Structural ISO 14230 frame parsing replaces byte-scanning. TesterPresent keepalive prevents P3 session timeout. Scanner re-initializes after consecutive failures.
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;
|
|
}
|