572 lines
17 KiB
C++
572 lines
17 KiB
C++
|
/*
|
||
|
|
; Project: Open Vehicle Monitor System
|
||
|
|
; Date: 14th March 2017
|
||
|
|
;
|
||
|
|
; Changes:
|
||
|
|
; 1.0 Initial release
|
||
|
|
;
|
||
|
|
; (C) 2011 Michael Stegen / Stegen Electronics
|
||
|
|
; (C) 2011-2017 Mark Webb-Johnson
|
||
|
|
; (C) 2011 Sonny Chen @ EPRO/DX
|
||
|
|
; (C) 2021 Michael Balzer <dexter@dexters-web.de>
|
||
|
|
;
|
||
|
|
; Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
|
|
; of this software and associated documentation files (the "Software"), to deal
|
||
|
|
; in the Software without restriction, including without limitation the rights
|
||
|
|
; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
|
|
; copies of the Software, and to permit persons to whom the Software is
|
||
|
|
; furnished to do so, subject to the following conditions:
|
||
|
|
;
|
||
|
|
; The above copyright notice and this permission notice shall be included in
|
||
|
|
; all copies or substantial portions of the Software.
|
||
|
|
;
|
||
|
|
; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
|
; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
|
; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
|
; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
|
; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
|
|
; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
|
|
; THE SOFTWARE.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "ovms_log.h"
|
||
|
|
static const char *TAG = "vehicle-isotp";
|
||
|
|
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <algorithm>
|
||
|
|
#include "vehicle.h"
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* PollerISOTPStart: start ISO-TP request
|
||
|
|
*/
|
||
|
|
void OvmsVehicle::PollerISOTPStart(bool fromTicker)
|
||
|
|
{
|
||
|
|
if (m_poll_plcur->rxmoduleid != 0)
|
||
|
|
{
|
||
|
|
// send to <moduleid>, listen to response from <rmoduleid>:
|
||
|
|
m_poll_moduleid_sent = m_poll_plcur->txmoduleid;
|
||
|
|
m_poll_moduleid_low = m_poll_plcur->rxmoduleid;
|
||
|
|
m_poll_moduleid_high = m_poll_plcur->rxmoduleid;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// broadcast: send to 0x7df, listen to all responses:
|
||
|
|
m_poll_moduleid_sent = 0x7df;
|
||
|
|
m_poll_moduleid_low = 0x7e8;
|
||
|
|
m_poll_moduleid_high = 0x7ef;
|
||
|
|
}
|
||
|
|
|
||
|
|
ESP_LOGD(TAG, "PollerISOTPStart(%d): send [bus=%d, type=%02X, pid=%X], expecting %03x/%03x-%03x",
|
||
|
|
fromTicker, m_poll_plcur->pollbus, m_poll_type, m_poll_pid, m_poll_moduleid_sent,
|
||
|
|
m_poll_moduleid_low, m_poll_moduleid_high);
|
||
|
|
|
||
|
|
//
|
||
|
|
// Assemble ISO-TP single/first frame
|
||
|
|
//
|
||
|
|
|
||
|
|
uint8_t* fr_data; // Frame data address
|
||
|
|
uint8_t fr_maxlen; // Frame data max length
|
||
|
|
uint16_t tp_len; // TP payload length including this frame (0…4095)
|
||
|
|
uint8_t* tp_data; // TP frame data section address
|
||
|
|
uint8_t tp_datalen; // TP frame data section length (0…7)
|
||
|
|
|
||
|
|
const uint8_t* tx_data; // Payload data
|
||
|
|
uint16_t tx_datalen; // Payload data length
|
||
|
|
uint16_t tx_datasent; // Payload data length sent with this frame
|
||
|
|
|
||
|
|
if (m_poll_plcur->xargs.tag == POLL_TXDATA)
|
||
|
|
{
|
||
|
|
tx_data = m_poll_plcur->xargs.data;
|
||
|
|
tx_datalen = m_poll_plcur->xargs.datalen;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
tx_data = m_poll_plcur->args.data;
|
||
|
|
tx_datalen = m_poll_plcur->args.datalen;
|
||
|
|
}
|
||
|
|
|
||
|
|
CAN_frame_t txframe = {};
|
||
|
|
txframe.origin = m_poll_bus;
|
||
|
|
txframe.callback = &m_poll_txcallback;
|
||
|
|
txframe.FIR.B.DLC = 8;
|
||
|
|
std::fill_n(txframe.data.u8, sizeof_array(txframe.data.u8), 0x55);
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTFRAME)
|
||
|
|
txframe.FIR.B.FF = CAN_frame_ext;
|
||
|
|
else
|
||
|
|
txframe.FIR.B.FF = CAN_frame_std;
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTADR)
|
||
|
|
{
|
||
|
|
txframe.MsgID = m_poll_moduleid_sent >> 8;
|
||
|
|
txframe.data.u8[0] = m_poll_moduleid_sent & 0xff;
|
||
|
|
fr_data = &txframe.data.u8[1];
|
||
|
|
fr_maxlen = 7;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
txframe.MsgID = m_poll_moduleid_sent;
|
||
|
|
fr_data = &txframe.data.u8[0];
|
||
|
|
fr_maxlen = 8;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Do we need to split this request into multiple frames?
|
||
|
|
if (POLL_TYPE_HAS_16BIT_PID(m_poll_plcur->type))
|
||
|
|
tp_len = 3 + tx_datalen;
|
||
|
|
else if (POLL_TYPE_HAS_8BIT_PID(m_poll_plcur->type))
|
||
|
|
tp_len = 2 + tx_datalen;
|
||
|
|
else
|
||
|
|
tp_len = 1 + tx_datalen;
|
||
|
|
|
||
|
|
if (tp_len <= fr_maxlen - 1)
|
||
|
|
{
|
||
|
|
fr_data[0] = (ISOTP_FT_SINGLE << 4) + tp_len;
|
||
|
|
tp_data = &fr_data[1];
|
||
|
|
tp_datalen = fr_maxlen - 1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
fr_data[0] = (ISOTP_FT_FIRST << 4) + ((tp_len & 0x0f00) >> 8);
|
||
|
|
fr_data[1] = tp_len & 0xff;
|
||
|
|
tp_data = &fr_data[2];
|
||
|
|
tp_datalen = fr_maxlen - 2;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Add TP data:
|
||
|
|
if (POLL_TYPE_HAS_16BIT_PID(m_poll_plcur->type))
|
||
|
|
{
|
||
|
|
tp_data[0] = m_poll_type;
|
||
|
|
tp_data[1] = m_poll_pid >> 8;
|
||
|
|
tp_data[2] = m_poll_pid & 0xff;
|
||
|
|
tx_datasent = LIMIT_MAX(tx_datalen, tp_datalen - 3);
|
||
|
|
memcpy(&tp_data[3], tx_data, tx_datasent);
|
||
|
|
}
|
||
|
|
else if (POLL_TYPE_HAS_8BIT_PID(m_poll_plcur->type))
|
||
|
|
{
|
||
|
|
tp_data[0] = m_poll_type;
|
||
|
|
tp_data[1] = m_poll_pid;
|
||
|
|
tx_datasent = LIMIT_MAX(tx_datalen, tp_datalen - 2);
|
||
|
|
memcpy(&tp_data[2], tx_data, tx_datasent);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
tp_data[0] = m_poll_type;
|
||
|
|
tx_datasent = LIMIT_MAX(tx_datalen, tp_datalen - 1);
|
||
|
|
memcpy(&tp_data[1], tx_data, tx_datasent);
|
||
|
|
}
|
||
|
|
|
||
|
|
m_poll_txmsgid = txframe.MsgID;
|
||
|
|
m_poll_tx_frame = 0;
|
||
|
|
m_poll_tx_data = tx_data;
|
||
|
|
m_poll_tx_offset = tx_datasent;
|
||
|
|
m_poll_tx_remain = tx_datalen - tx_datasent;
|
||
|
|
m_poll_ml_frame = 0;
|
||
|
|
m_poll_ml_offset = 0;
|
||
|
|
m_poll_ml_remain = 0;
|
||
|
|
m_poll_wait = 2;
|
||
|
|
|
||
|
|
m_poll_bus->Write(&txframe);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* PollerISOTPReceive: process ISO-TP poll response frame
|
||
|
|
*/
|
||
|
|
bool OvmsVehicle::PollerISOTPReceive(CAN_frame_t* frame, uint32_t msgid)
|
||
|
|
{
|
||
|
|
OvmsRecMutexLock lock(&m_poll_mutex);
|
||
|
|
char *hexdump = NULL;
|
||
|
|
|
||
|
|
// After locking the mutex, check again for poll expectance match:
|
||
|
|
if (!m_poll_wait || !m_poll_plist || frame->origin != m_poll_bus ||
|
||
|
|
msgid < m_poll_moduleid_low || msgid > m_poll_moduleid_high)
|
||
|
|
{
|
||
|
|
ESP_LOGD(TAG, "PollerISOTPReceive[%03X]: dropping expired poll response", msgid);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
//
|
||
|
|
// Get & validate ISO-TP meta data
|
||
|
|
//
|
||
|
|
|
||
|
|
uint8_t* fr_data; // Frame data address
|
||
|
|
uint8_t fr_maxlen; // Frame data max length
|
||
|
|
uint8_t tp_frametype; // ISO-TP frame type (0…3)
|
||
|
|
uint8_t tp_frameindex; // TP cyclic frame index (0…15)
|
||
|
|
uint16_t tp_len; // TP remaining payload length including this frame (0…4095)
|
||
|
|
uint8_t* tp_data; // TP frame data section address
|
||
|
|
uint8_t tp_datalen; // TP frame data section length (0…7)
|
||
|
|
|
||
|
|
uint8_t tp_fc_command; // Flow control command (0 = continue, 1 = wait, 2 = abort)
|
||
|
|
uint8_t tp_fc_framecnt; // Flow control max frame count (0 = unlimited)
|
||
|
|
uint8_t tp_fc_septime; // Flow control frame separation time
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTADR)
|
||
|
|
{
|
||
|
|
fr_data = &frame->data.u8[1];
|
||
|
|
fr_maxlen = 7;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
fr_data = &frame->data.u8[0];
|
||
|
|
fr_maxlen = 8;
|
||
|
|
}
|
||
|
|
|
||
|
|
tp_frametype = fr_data[0] >> 4;
|
||
|
|
|
||
|
|
switch (tp_frametype)
|
||
|
|
{
|
||
|
|
case ISOTP_FT_SINGLE:
|
||
|
|
tp_frameindex = 0;
|
||
|
|
tp_len = fr_data[0] & 0x0f;
|
||
|
|
tp_data = &fr_data[1];
|
||
|
|
tp_datalen = tp_len;
|
||
|
|
break;
|
||
|
|
case ISOTP_FT_FIRST:
|
||
|
|
tp_frameindex = 0;
|
||
|
|
tp_len = (fr_data[0] & 0x0f) << 8 | fr_data[1];
|
||
|
|
tp_data = &fr_data[2];
|
||
|
|
tp_datalen = (tp_len > fr_maxlen-2) ? fr_maxlen-2 : tp_len;
|
||
|
|
break;
|
||
|
|
case ISOTP_FT_CONSECUTIVE:
|
||
|
|
tp_frameindex = fr_data[0] & 0x0f;
|
||
|
|
tp_len = m_poll_ml_remain;
|
||
|
|
tp_data = &fr_data[1];
|
||
|
|
tp_datalen = (tp_len > fr_maxlen-1) ? fr_maxlen-1 : tp_len;
|
||
|
|
break;
|
||
|
|
case ISOTP_FT_FLOWCTRL:
|
||
|
|
tp_fc_command = fr_data[0] & 0x0f;
|
||
|
|
tp_fc_framecnt = fr_data[1];
|
||
|
|
tp_fc_septime = fr_data[2];
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
{
|
||
|
|
// This is most likely an indication there is a non ISO-TP device sending
|
||
|
|
// in our expected RX ID range, so we log the frame and abort:
|
||
|
|
FormatHexDump(&hexdump, (const char*)frame->data.u8, 8, 8);
|
||
|
|
ESP_LOGW(TAG, "PollerISOTPReceive[%03X]: ignoring unknown/invalid ISO TP frame: %s",
|
||
|
|
msgid, hexdump ? hexdump : "-");
|
||
|
|
if (hexdump) free(hexdump);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Handle TX flow control:
|
||
|
|
if (tp_frametype == ISOTP_FT_FLOWCTRL)
|
||
|
|
{
|
||
|
|
if (tp_fc_command > 2 || m_poll_tx_remain == 0)
|
||
|
|
{
|
||
|
|
FormatHexDump(&hexdump, (const char*)frame->data.u8, 8, 8);
|
||
|
|
ESP_LOGW(TAG, "PollerISOTPReceive[%03X]: ignoring unexpected/invalid ISO TP flow control frame: %s",
|
||
|
|
msgid, hexdump ? hexdump : "-");
|
||
|
|
if (hexdump) free(hexdump);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (tp_fc_command == 1)
|
||
|
|
{
|
||
|
|
// add some wait time:
|
||
|
|
m_poll_wait++;
|
||
|
|
}
|
||
|
|
else if (tp_fc_command == 2)
|
||
|
|
{
|
||
|
|
// abort TX:
|
||
|
|
m_poll_tx_remain = 0;
|
||
|
|
// (but still wait for response)
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// continue TX:
|
||
|
|
CAN_frame_t tx_frame = {};
|
||
|
|
uint8_t* tx_data;
|
||
|
|
uint8_t tx_datalen;
|
||
|
|
uint8_t tx_datasent;
|
||
|
|
uint32_t txid;
|
||
|
|
tx_frame.origin = frame->origin;
|
||
|
|
tx_frame.FIR.B.DLC = 8;
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTFRAME)
|
||
|
|
tx_frame.FIR.B.FF = CAN_frame_ext;
|
||
|
|
else
|
||
|
|
tx_frame.FIR.B.FF = CAN_frame_std;
|
||
|
|
|
||
|
|
if (m_poll_moduleid_sent == 0x7df)
|
||
|
|
{
|
||
|
|
// broadcast request: derive module ID from response ID:
|
||
|
|
// (Note: this only works for the SAE standard ID scheme)
|
||
|
|
txid = frame->MsgID - 8;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// use known module ID:
|
||
|
|
txid = m_poll_moduleid_sent;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTADR)
|
||
|
|
{
|
||
|
|
tx_frame.MsgID = txid >> 8;
|
||
|
|
tx_frame.data.u8[0] = txid & 0xff;
|
||
|
|
tx_data = &tx_frame.data.u8[1];
|
||
|
|
tx_datalen = 6;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
tx_frame.MsgID = txid;
|
||
|
|
tx_data = &tx_frame.data.u8[0];
|
||
|
|
tx_datalen = 7;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Send next chunk of frames:
|
||
|
|
while (m_poll_tx_remain > 0)
|
||
|
|
{
|
||
|
|
++m_poll_tx_frame;
|
||
|
|
tx_data[0] = (ISOTP_FT_CONSECUTIVE << 4) + (m_poll_tx_frame & 0x0f);
|
||
|
|
tx_datasent = LIMIT_MAX(m_poll_tx_remain, tx_datalen);
|
||
|
|
memcpy(&tx_data[1], m_poll_tx_data+m_poll_tx_offset, tx_datasent);
|
||
|
|
if (tx_datasent < tx_datalen)
|
||
|
|
memset(&tx_data[1+tx_datasent], 0x55, tx_datalen-tx_datasent);
|
||
|
|
tx_frame.Write();
|
||
|
|
m_poll_tx_offset += tx_datasent;
|
||
|
|
m_poll_tx_remain -= tx_datasent;
|
||
|
|
|
||
|
|
if (m_poll_tx_remain == 0)
|
||
|
|
break;
|
||
|
|
if (tp_fc_framecnt > 0 && --tp_fc_framecnt == 0)
|
||
|
|
break;
|
||
|
|
|
||
|
|
if (tp_fc_septime > 0)
|
||
|
|
{
|
||
|
|
if (tp_fc_septime <= 127)
|
||
|
|
usleep(tp_fc_septime * 1000);
|
||
|
|
else if (tp_fc_septime > 240)
|
||
|
|
usleep((tp_fc_septime - 240) * 100);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (m_poll_tx_remain > 0)
|
||
|
|
m_poll_wait = 2;
|
||
|
|
}
|
||
|
|
|
||
|
|
return true;
|
||
|
|
} // if (tp_frametype == ISOTP_FT_FLOWCTRL)
|
||
|
|
|
||
|
|
|
||
|
|
// Check frame index:
|
||
|
|
if (tp_frametype == ISOTP_FT_CONSECUTIVE)
|
||
|
|
{
|
||
|
|
// Note: we tolerate an index less than the expected one, as some devices
|
||
|
|
// begin counting at the first consecutive frame
|
||
|
|
if (m_poll_ml_remain == 0 || tp_frameindex > (m_poll_ml_frame & 0x0f))
|
||
|
|
{
|
||
|
|
FormatHexDump(&hexdump, (const char*)frame->data.u8, 8, 8);
|
||
|
|
ESP_LOGW(TAG, "PollerISOTPReceive[%03X]: unexpected/out of sequence ISO TP frame (%d vs %d), aborting poll %02X(%X): %s",
|
||
|
|
msgid, tp_frameindex, m_poll_ml_frame & 0x0f, m_poll_type, m_poll_pid,
|
||
|
|
hexdump ? hexdump : "-");
|
||
|
|
if (hexdump) free(hexdump);
|
||
|
|
m_poll_moduleid_low = m_poll_moduleid_high = 0; // ignore further frames
|
||
|
|
m_poll_wait = 2; // give the bus time to let remaining frames pass
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//
|
||
|
|
// Get & validate OBD/UDS meta data
|
||
|
|
//
|
||
|
|
|
||
|
|
uint8_t response_type = 0; // OBD/UDS response type tag (expected: 0x40 + request type)
|
||
|
|
uint16_t response_pid = 0; // OBD/UDS response PID (expected: request PID)
|
||
|
|
uint8_t* response_data = NULL; // OBD/UDS frame payload address
|
||
|
|
uint16_t response_datalen = 0; // OBD/UDS frame payload length (0…7)
|
||
|
|
uint8_t error_type = 0; // OBD/UDS error response service type (expected: request type)
|
||
|
|
uint8_t error_code = 0; // OBD/UDS error response code (see ISO 14229 Annex A.1)
|
||
|
|
|
||
|
|
if (tp_frametype == ISOTP_FT_CONSECUTIVE)
|
||
|
|
{
|
||
|
|
response_type = 0x40+m_poll_type;
|
||
|
|
response_pid = m_poll_pid;
|
||
|
|
response_data = tp_data;
|
||
|
|
response_datalen = tp_datalen;
|
||
|
|
}
|
||
|
|
else // ISOTP_FT_FIRST || ISOTP_FT_SINGLE
|
||
|
|
{
|
||
|
|
response_type = tp_data[0];
|
||
|
|
if (response_type == UDS_RESP_TYPE_NRC)
|
||
|
|
{
|
||
|
|
error_type = tp_data[1];
|
||
|
|
error_code = tp_data[2];
|
||
|
|
}
|
||
|
|
else if (POLL_TYPE_HAS_16BIT_PID(response_type-0x40))
|
||
|
|
{
|
||
|
|
response_pid = tp_data[1] << 8 | tp_data[2];
|
||
|
|
response_data = &tp_data[3];
|
||
|
|
response_datalen = tp_datalen - 3;
|
||
|
|
}
|
||
|
|
else if (POLL_TYPE_HAS_8BIT_PID(response_type-0x40))
|
||
|
|
{
|
||
|
|
response_pid = tp_data[1];
|
||
|
|
response_data = &tp_data[2];
|
||
|
|
response_datalen = tp_datalen - 2;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
response_pid = m_poll_pid;
|
||
|
|
response_data = &tp_data[1];
|
||
|
|
response_datalen = tp_datalen - 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//
|
||
|
|
// Process OBD/UDS payload
|
||
|
|
//
|
||
|
|
|
||
|
|
if (response_type == UDS_RESP_TYPE_NRC && error_type == m_poll_type)
|
||
|
|
{
|
||
|
|
// Negative Response Code:
|
||
|
|
if (error_code == UDS_RESP_NRC_RCRRP)
|
||
|
|
{
|
||
|
|
// Info: requestCorrectlyReceived-ResponsePending (server busy processing the request)
|
||
|
|
ESP_LOGD(TAG, "PollerISOTPReceive[%03X]: got OBD/UDS info %02X(%X) code=%02X (pending)",
|
||
|
|
msgid, m_poll_type, m_poll_pid, error_code);
|
||
|
|
// add some wait time:
|
||
|
|
m_poll_wait++;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// Error: forward to application:
|
||
|
|
ESP_LOGD(TAG, "PollerISOTPReceive[%03X]: process OBD/UDS error %02X(%X) code=%02X",
|
||
|
|
msgid, m_poll_type, m_poll_pid, error_code);
|
||
|
|
// Running single poll?
|
||
|
|
if (m_poll_single_rxbuf)
|
||
|
|
{
|
||
|
|
m_poll_single_rxerr = error_code;
|
||
|
|
m_poll_single_rxbuf = NULL;
|
||
|
|
m_poll_single_rxdone.Give();
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
IncomingPollError(frame->origin, m_poll_type, m_poll_pid, error_code);
|
||
|
|
}
|
||
|
|
// abort:
|
||
|
|
m_poll_ml_remain = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (response_type == 0x40+m_poll_type && response_pid == m_poll_pid)
|
||
|
|
{
|
||
|
|
// Normal matching poll response, forward to application:
|
||
|
|
m_poll_ml_remain = tp_len - tp_datalen;
|
||
|
|
ESP_LOGD(TAG, "PollerISOTPReceive[%03X]: process OBD/UDS response %02X(%X) frm=%u len=%u off=%u rem=%u",
|
||
|
|
msgid, m_poll_type, m_poll_pid,
|
||
|
|
m_poll_ml_frame, response_datalen, m_poll_ml_offset, m_poll_ml_remain);
|
||
|
|
// Running single poll?
|
||
|
|
if (m_poll_single_rxbuf)
|
||
|
|
{
|
||
|
|
if (m_poll_ml_frame == 0)
|
||
|
|
{
|
||
|
|
m_poll_single_rxbuf->clear();
|
||
|
|
m_poll_single_rxbuf->reserve(response_datalen + m_poll_ml_remain);
|
||
|
|
}
|
||
|
|
m_poll_single_rxbuf->append((char*)response_data, response_datalen);
|
||
|
|
if (m_poll_ml_remain == 0)
|
||
|
|
{
|
||
|
|
m_poll_single_rxerr = 0;
|
||
|
|
m_poll_single_rxbuf = NULL;
|
||
|
|
m_poll_single_rxdone.Give();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
IncomingPollReply(frame->origin, m_poll_type, m_poll_pid, response_data, response_datalen, m_poll_ml_remain);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// This is most likely a late response to a previous poll, log & skip:
|
||
|
|
FormatHexDump(&hexdump, (const char*)frame->data.u8, 8, 8);
|
||
|
|
ESP_LOGW(TAG, "PollerISOTPReceive[%03X]: OBD/UDS response type/PID mismatch, got %02X(%X) vs %02X(%X) => ignoring: %s",
|
||
|
|
msgid, response_type, response_pid, 0x40+m_poll_type, m_poll_pid, hexdump ? hexdump : "-");
|
||
|
|
if (hexdump) free(hexdump);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// Do we expect more data?
|
||
|
|
if (m_poll_ml_remain)
|
||
|
|
{
|
||
|
|
if (tp_frametype == ISOTP_FT_FIRST)
|
||
|
|
{
|
||
|
|
// First frame; send flow control frame:
|
||
|
|
CAN_frame_t txframe;
|
||
|
|
uint8_t* txdata;
|
||
|
|
uint32_t txid;
|
||
|
|
memset(&txframe,0,sizeof(txframe));
|
||
|
|
txframe.origin = frame->origin;
|
||
|
|
txframe.FIR.B.DLC = 8;
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTFRAME)
|
||
|
|
txframe.FIR.B.FF = CAN_frame_ext;
|
||
|
|
else
|
||
|
|
txframe.FIR.B.FF = CAN_frame_std;
|
||
|
|
|
||
|
|
if (m_poll_moduleid_sent == 0x7df)
|
||
|
|
{
|
||
|
|
// broadcast request: derive module ID from response ID:
|
||
|
|
// (Note: this only works for the SAE standard ID scheme)
|
||
|
|
txid = frame->MsgID - 8;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// use known module ID:
|
||
|
|
txid = m_poll_moduleid_sent;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (m_poll_protocol == ISOTP_EXTADR)
|
||
|
|
{
|
||
|
|
txframe.MsgID = txid >> 8;
|
||
|
|
txframe.data.u8[0] = txid & 0xff;
|
||
|
|
txdata = &txframe.data.u8[1];
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
txframe.MsgID = txid;
|
||
|
|
txdata = &txframe.data.u8[0];
|
||
|
|
}
|
||
|
|
|
||
|
|
txdata[0] = 0x30; // flow control frame type
|
||
|
|
txdata[1] = 0x00; // request all frames available
|
||
|
|
txdata[2] = m_poll_fc_septime; // with configured separation timing (default 25 ms)
|
||
|
|
txframe.Write();
|
||
|
|
m_poll_ml_frame = 1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
m_poll_ml_frame++;
|
||
|
|
}
|
||
|
|
|
||
|
|
m_poll_ml_offset += response_datalen; // next frame application payload offset
|
||
|
|
m_poll_wait = 2;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// Request response complete:
|
||
|
|
m_poll_wait = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// Immediately send the next poll for this tick if…
|
||
|
|
// - we are not waiting for another frame
|
||
|
|
// - the poll was no broadcast (with potential further responses from other devices)
|
||
|
|
// - poll throttling is unlimited or limit isn't reached yet
|
||
|
|
if (m_poll_wait == 0 &&
|
||
|
|
m_poll_moduleid_sent != 0x7df &&
|
||
|
|
(!m_poll_sequence_max || m_poll_sequence_cnt < m_poll_sequence_max))
|
||
|
|
{
|
||
|
|
PollerSend(false);
|
||
|
|
}
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|