Harden OBD-II K-line handler (code review findings)

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.

firmware/lib/AutoWire/KLineObd2.cpp

@@ -1,8 +1,13 @@
 // 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),
@@ -10,31 +15,51 @@ KLineObd2::KLineObd2(KLineTransport& transport)
       _kw1(0),
       _kw2(0),
       _testerAddr(obd2::TESTER_ADDR),
-      _ecuAddr(0) {}
+      _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.
-// The UART peripheral must be detached from the TX pin during this sequence,
-// then re-attached for normal 10400 baud communication.
 //
 // 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, wait for ECU response
+//   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) {
-    int8_t txp = _transport.txPin();
+    // Detach UART TX so we can bit-bang
-    uint8_t unum = _transport.uartNum();
+    detachUartTx();
-
-    // Detach UART from TX pin so we can bit-bang
-    uart_set_pin((uart_port_t)unum, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE,
-                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
 
     // 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)
@@ -43,8 +68,11 @@ bool KLineObd2::slowInit(uint8_t targetAddr) {
     sendByte5Baud(targetAddr);
 
     // Re-attach UART to TX pin
-    uart_set_pin((uart_port_t)unum, txp, UART_PIN_NO_CHANGE,
+    reattachUartTx();
-                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+
+    // 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);
@@ -64,13 +92,13 @@ bool KLineObd2::slowInit(uint8_t targetAddr) {
     // W3: 25-50ms before tester responds
     delay(30);
 
-    // Send inverted keyword2 back to ECU
+    // Send inverted keyword2 back to ECU and verify echo
     uint8_t invKw2 = ~_kw2;
     _transport.serial()->write(invKw2);
     _transport.serial()->flush();
-
+    if (!clearEcho(&invKw2, 1)) {
-    // Clear our own echo (half-duplex bus)
+        return false;  // bus contention during handshake
-    clearEcho(1);
+    }
 
     // W4: ECU confirms by sending inverted target address
     int ack = readByteTimeout(50);
@@ -83,6 +111,7 @@ bool KLineObd2::slowInit(uint8_t targetAddr) {
 
     _ecuAddr = targetAddr;
     _initialized = true;
+    _lastRequestMs = millis();
     return true;
 }
 
@@ -90,13 +119,10 @@ bool KLineObd2::slowInit(uint8_t targetAddr) {
 // 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();
-    uint8_t unum = _transport.uartNum();
-
-    // Detach UART from TX pin for the wake-up pulse
-    uart_set_pin((uart_port_t)unum, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE,
-                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
-
     pinMode(txp, OUTPUT);
     digitalWrite(txp, HIGH);
     delay(300);  // idle before pulse
@@ -107,12 +133,11 @@ bool KLineObd2::fastInit() {
     digitalWrite(txp, HIGH);
     delay(25);
 
-    // Re-attach UART
+    // Re-attach UART and flush stale RX data
-    uart_set_pin((uart_port_t)unum, txp, UART_PIN_NO_CHANGE,
+    reattachUartTx();
-                 UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+    _transport.flushRx();
 
-    // Send StartCommunication request (KWP2000 service 0x81)
+    // Build StartCommunication request (KWP2000 service 0x81)
-    // Format: [fmt+len, target, source, service_id]
     uint8_t startComm[] = {
         0xC1,                    // format: functional addressing, 1 data byte
         obd2::FUNC_ADDR,        // target: functional address
@@ -121,56 +146,64 @@ bool KLineObd2::fastInit() {
     };
     uint8_t checksum = KLineTransport::checksumMod256(startComm, 4);
 
-    for (uint8_t i = 0; i < 4; i++) {
+    // Full message with checksum for echo verification
-        _transport.serial()->write(startComm[i]);
+    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()->write(checksum);
     _transport.serial()->flush();
 
-    // Clear echo (5 bytes: 4 data + 1 checksum)
+    // Verify echo (5 bytes: 4 data + 1 checksum)
-    clearEcho(5);
+    if (!clearEcho(fullMsg, 5)) {
+        return false;  // bus contention during fast init
+    }
 
     // Read StartComm positive response
-    // Expected: [fmt+len, source, target, 0xC1, kw1, kw2, checksum]
+    uint8_t resp[12];
-    uint8_t resp[7];
     uint8_t respLen = readResponse(resp, sizeof(resp), 300);
     if (respLen < 4) return false;
 
-    // Positive response service ID is request + 0x40
+    // Parse response structurally using ISO 14230 format byte
-    // Find the service byte — it's after header bytes
+    uint8_t dLen = 0;
-    // For physical addressing response: [fmt, target, source, 0xC1, kw1, kw2, chk]
+    int8_t dataStart = parseFrameData(resp, respLen, &dLen);
-    bool foundResponse = false;
+    if (dataStart < 0) return false;
-    for (uint8_t i = 0; i < respLen; i++) {
+    if (dLen < 3) return false;  // need service_id + kw1 + kw2
-        if (resp[i] == 0xC1) {  // StartComm positive response
-            if (i + 2 < respLen) {
-                _kw1 = resp[i + 1];
-                _kw2 = resp[i + 2];
-                foundResponse = true;
-            }
-            break;
-        }
-    }
 
-    if (!foundResponse) return false;
+    // 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) {
-    uint8_t checksum = KLineTransport::checksumMod256(data, len);
+    if (len > KLINE_MAX_MSG) return;
 
-    for (uint8_t i = 0; i < len; i++) {
+    // Build complete message with checksum for echo verification
-        _transport.serial()->write(data[i]);
+    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()->write(checksum);
     _transport.serial()->flush();
 
-    // Clear echo: data bytes + checksum byte
+    // Verify echo (data bytes + checksum byte)
-    clearEcho(len + 1);
+    clearEcho(buf, totalLen);
+
+    _lastRequestMs = millis();
 }
 
 uint8_t KLineObd2::readResponse(uint8_t* buf, uint8_t maxLen, uint16_t timeoutMs) {
@@ -183,37 +216,72 @@ uint8_t KLineObd2::readResponse(uint8_t* buf, uint8_t maxLen, uint16_t timeoutMs
     buf[count++] = (uint8_t)first;
 
     // Determine expected length from format byte
-    // Bits 6-7: address mode (00=no addr, 10=phys, 11=func)
-    // Bits 0-5: data length (0 = length in separate byte)
     uint8_t fmt = (uint8_t)first;
-    uint8_t addrBytes = (fmt & 0x80) ? 2 : 0;  // target + source if addressing present
+    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++) {
-        int b = readByteTimeout(timeoutMs - (millis() - start));
+        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) {
-        int lenByte = readByteTimeout(timeoutMs - (millis() - start));
+        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 remaining = dataLen + 1;  // +1 for checksum
+    uint8_t expected = dataLen + 1;  // +1 for checksum
-    for (uint8_t i = 0; i < remaining && count < maxLen; i++) {
+    for (uint8_t i = 0; i < expected; i++) {
-        uint16_t elapsed = millis() - start;
+        if (count >= maxLen) {
-        if (elapsed >= timeoutMs) return count;
+#ifdef KLINE_DEBUG
-        int b = readByteTimeout(timeoutMs - elapsed);
+            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;
 }
 
@@ -221,6 +289,11 @@ 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
@@ -232,27 +305,57 @@ uint8_t KLineObd2::requestPid(uint8_t mode, uint8_t pid,
 
     sendRequest(req, sizeof(req));
 
-    uint8_t raw[16];
+    uint8_t raw[32];
     uint8_t rawLen = readResponse(raw, sizeof(raw));
     if (rawLen < 5) return 0;  // minimum: fmt + target + source + response_mode + pid
 
-    // Find response data — skip header, copy from response_mode onward
+    // Parse response structurally
-    // Response mode = request mode + 0x40
+    uint8_t dLen = 0;
-    uint8_t respMode = mode + 0x40;
+    int8_t dataStart = parseFrameData(raw, rawLen, &dLen);
-    for (uint8_t i = 0; i < rawLen; i++) {
+    if (dataStart < 0 || dLen < 2) return 0;
-        if (raw[i] == respMode && (i + 1) < rawLen && raw[i + 1] == pid) {
+
-            // Copy data bytes after mode+pid, excluding checksum
+    // Check for negative response (service 0x7F)
-            uint8_t dataStart = i + 2;
+    if (raw[dataStart] == 0x7F) {
-            uint8_t dataEnd = rawLen - 1;  // exclude checksum
+#ifdef KLINE_DEBUG
-            uint8_t dataLen = 0;
+        if (dLen >= 3) {
-            for (uint8_t j = dataStart; j < dataEnd && dataLen < maxLen; j++) {
+            Serial.print("KLINE: NRC 0x");
-                response[dataLen++] = raw[j];
+            Serial.println(raw[dataStart + 2], HEX);
-            }
-            return dataLen;
         }
+#endif
+        return 0;
     }
 
-    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 ---
@@ -275,13 +378,35 @@ void KLineObd2::sendByte5Baud(uint8_t data) {
     delay(200);
 }
 
-void KLineObd2::clearEcho(uint8_t count) {
+bool KLineObd2::clearEcho(const uint8_t* expected, uint8_t count) {
+    bool match = true;
     for (uint8_t i = 0; i < count; i++) {
-        readByteTimeout(KLINE_ECHO_TIMEOUT_MS);
+        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();
@@ -292,3 +417,42 @@ int KLineObd2::readByteTimeout(uint16_t timeoutMs) {
     }
     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;
+}

firmware/lib/AutoWire/KLineObd2.h

@@ -34,7 +34,7 @@ public:
     void sendRequest(const uint8_t* data, uint8_t len);
 
     // Read response from ECU. Blocks until response received or timeout.
-    // Returns number of bytes read, or 0 on timeout.
+    // Returns number of bytes read, or 0 on timeout/checksum error.
     uint8_t readResponse(uint8_t* buf, uint8_t maxLen,
                          uint16_t timeoutMs = KLINE_RESPONSE_TIMEOUT_MS);
 
@@ -43,6 +43,12 @@ public:
     uint8_t requestPid(uint8_t mode, uint8_t pid,
                        uint8_t* response, uint8_t maxLen);
 
+    // Send TesterPresent (service 0x3E) to keep the diagnostic session alive.
+    void sendTesterPresent();
+
+    // Reset initialized state (for re-init after session loss)
+    void reset() { _initialized = false; }
+
     bool isInitialized() const { return _initialized; }
 
     // Protocol info from init handshake
@@ -53,16 +59,34 @@ private:
     // Bit-bang a single byte at 5 baud (200ms per bit) on TX pin
     void sendByte5Baud(uint8_t data);
 
-    // Discard echo bytes from half-duplex bus
+    // Detach UART TX from GPIO pin (so we can bit-bang)
-    void clearEcho(uint8_t count);
+    void detachUartTx();
+
+    // Re-attach UART TX to GPIO pin (after bit-bang init)
+    void reattachUartTx();
+
+    // Discard echo bytes from half-duplex bus, verify they match expected.
+    // Returns true if all echo bytes matched, false on bus contention.
+    bool clearEcho(const uint8_t* expected, uint8_t count);
 
     // Read a single byte with timeout
     int readByteTimeout(uint16_t timeoutMs);
 
+    // Parse ISO 14230 frame structure. Returns index of first data byte
+    // (service ID position), or -1 on parse error.
+    // Sets *dataLen to the number of data bytes.
+    int8_t parseFrameData(const uint8_t* buf, uint8_t bufLen, uint8_t* dataLen);
+
+    // Remaining timeout helper — clamps to 0 if already expired
+    static uint16_t remainingMs(unsigned long startMs, uint16_t timeoutMs);
+
     KLineTransport& _transport;
     bool _initialized;
     uint8_t _kw1;
     uint8_t _kw2;
     uint8_t _testerAddr;   // our address (0xF1 = external test equipment)
     uint8_t _ecuAddr;      // ECU address from init response
+    unsigned long _lastRequestMs;  // for P3 keepalive timing
+
+    static constexpr uint16_t P3_KEEPALIVE_MS = 4000;  // send TesterPresent before P3max
 };

firmware/lib/AutoWire/KLineTransport.cpp

@@ -112,6 +112,13 @@ int KLineTransport::rxRead() {
     return _rxRing.read();
 }
 
+void KLineTransport::flushRx() {
+    uart_flush_input((uart_port_t)_uartNum);
+    while (_rxRing.available() > 0) {
+        _rxRing.read();
+    }
+}
+
 // --- LED control ---
 
 void KLineTransport::ledOn() {

firmware/lib/AutoWire/KLineTransport.h

@@ -73,6 +73,9 @@ public:
     void rxRemove(int n);
     int  rxRead();
 
+    // Flush both UART FIFO and RX ring buffer (discard all pending data)
+    void flushRx();
+
     // Bus idle state
     bool clearToSend() const { return _clearToSend; }
 

firmware/src/obd2_scanner.cpp

@@ -15,6 +15,39 @@ KLineObd2* scanner = nullptr;
 
 static unsigned long lastPollMs = 0;
 static const unsigned long POLL_INTERVAL_MS = 500;
+static uint8_t consecutiveFailures = 0;
+static const uint8_t MAX_FAILURES_BEFORE_REINIT = 5;
+
+// Attempt ECU initialization (slow init, then fast init fallback)
+static bool tryInit() {
+    Serial.print("Slow init (0x");
+    Serial.print(KLINE_INIT_ADDR, HEX);
+    Serial.print(")... ");
+
+    if (scanner->slowInit(KLINE_INIT_ADDR)) {
+        Serial.println("OK");
+        Serial.print("Keywords: ");
+        Serial.print(scanner->keyword1(), HEX);
+        Serial.print(" ");
+        Serial.println(scanner->keyword2(), HEX);
+        return true;
+    }
+
+    Serial.println("failed");
+    Serial.print("Fast init... ");
+
+    if (scanner->fastInit()) {
+        Serial.println("OK");
+        Serial.print("Keywords: ");
+        Serial.print(scanner->keyword1(), HEX);
+        Serial.print(" ");
+        Serial.println(scanner->keyword2(), HEX);
+        return true;
+    }
+
+    Serial.println("failed");
+    return false;
+}
 
 void setup() {
     Serial.begin(115200);
@@ -44,31 +77,13 @@ void setup() {
                     KLINE_UART_NUM, config);
 
     scanner = new KLineObd2(transport);
+    if (!scanner) {
+        Serial.println("ERROR: allocation failed");
+        return;
+    }
 
-    // Try slow init first (ISO 9141-2)
+    if (!tryInit()) {
-    Serial.print("Slow init (0x");
+        Serial.println("No ECU response. Check wiring and key position.");
-    Serial.print(KLINE_INIT_ADDR, HEX);
-    Serial.print(")... ");
-
-    if (scanner->slowInit(KLINE_INIT_ADDR)) {
-        Serial.println("OK");
-        Serial.print("Keywords: ");
-        Serial.print(scanner->keyword1(), HEX);
-        Serial.print(" ");
-        Serial.println(scanner->keyword2(), HEX);
-    } else {
-        Serial.println("failed");
-        Serial.print("Fast init... ");
-        if (scanner->fastInit()) {
-            Serial.println("OK");
-            Serial.print("Keywords: ");
-            Serial.print(scanner->keyword1(), HEX);
-            Serial.print(" ");
-            Serial.println(scanner->keyword2(), HEX);
-        } else {
-            Serial.println("failed");
-            Serial.println("No ECU response. Check wiring and key position.");
-        }
     }
 
     Serial.println();
@@ -80,15 +95,28 @@ void setup() {
 }
 
 void loop() {
-    if (!scanner || !scanner->isInitialized()) {
+    if (!scanner) {
         delay(1000);
         return;
     }
 
+    // Re-init after consecutive failures (session lost)
+    if (!scanner->isInitialized()) {
+        delay(3000);
+        Serial.println("Retrying init...");
+        scanner->reset();
+        if (tryInit()) {
+            consecutiveFailures = 0;
+            Serial.println("Polling PIDs...");
+        }
+        return;
+    }
+
     if (millis() - lastPollMs < POLL_INTERVAL_MS) return;
     lastPollMs = millis();
 
     uint8_t data[4];
+    uint8_t successes = 0;
 
     // RPM (2 data bytes)
     uint8_t len = scanner->requestPid(obd2::MODE_CURRENT, obd2::PID_RPM, data, sizeof(data));
@@ -96,6 +124,7 @@ void loop() {
         float rpm = obd2::decodeRpm(data[0], data[1]);
         Serial.print(rpm, 0);
         Serial.print(" rpm");
+        successes++;
     } else {
         Serial.print("---     ");
     }
@@ -106,6 +135,7 @@ void loop() {
     if (len >= 1) {
         Serial.print(obd2::decodeSpeed(data[0]));
         Serial.print(" km/h");
+        successes++;
     } else {
         Serial.print("---    ");
     }
@@ -116,6 +146,7 @@ void loop() {
     if (len >= 1) {
         Serial.print(obd2::decodeCoolantTemp(data[0]));
         Serial.print(" C");
+        successes++;
     } else {
         Serial.print("---    ");
     }
@@ -126,6 +157,7 @@ void loop() {
     if (len >= 1) {
         Serial.print(obd2::decodeThrottle(data[0]), 1);
         Serial.print(" %");
+        successes++;
     } else {
         Serial.print("---     ");
     }
@@ -136,9 +168,22 @@ void loop() {
     if (len >= 2) {
         Serial.print(obd2::decodeControlVoltage(data[0], data[1]), 1);
         Serial.print(" V");
+        successes++;
     } else {
         Serial.print("---   ");
     }
 
     Serial.println();
+
+    // Track consecutive total failures for session loss detection
+    if (successes == 0) {
+        consecutiveFailures++;
+        if (consecutiveFailures >= MAX_FAILURES_BEFORE_REINIT) {
+            Serial.println("Session lost — will re-initialize.");
+            scanner->reset();
+            consecutiveFailures = 0;
+        }
+    } else {
+        consecutiveFailures = 0;
+    }
 }