t.mettler/giesCANne
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Converted more code to be easier to generalize to larger CAN payloads

Browse Source 
for CAN-FD. Improved some code to be faster. Fixed up some compile
time warnings.
This commit is contained in:
Collin Kidder
2019-12-15 15:02:06 -05:00
parent ef24036d1c
commit 8beb0e31a0
28 changed files with 589 additions and 435 deletions
Download Patch File Download Diff File Expand all files Collapse all files
bus_protocols/isotp_handler.cpp
+39 -37
View File
@@ -47,7 +47,7 @@ void ISOTP_HANDLER::setReception(bool mode)
}
}
void ISOTP_HANDLER::sendISOTPFrame(int bus, int ID, QVector<unsigned char> data)
void ISOTP_HANDLER::sendISOTPFrame(int bus, int ID, QByteArray data)
{
CANFrame frame;
frame.setFrameType(QCanBusFrame::DataFrame);
@@ -159,6 +159,8 @@ void ISOTP_HANDLER::processFrame(const CANFrame &frame)
//int offset;
ISOTP_MESSAGE msg;
ISOTP_MESSAGE *pMsg;
unsigned char *data = reinterpret_cast<unsigned char *>(frame.payload().data());
//int dataLen = frame.payload().count();
frameType = 0;
frameLen = 0;
@@ -166,14 +168,14 @@ void ISOTP_HANDLER::processFrame(const CANFrame &frame)
if (useExtendedAddressing)
{
ID = ID << 8;
ID += frame.payload()[0];
frameType = frame.payload()[1] >> 4;
frameLen = frame.payload()[1] & 0xF;
ID += data[0];
frameType = data[1] >> 4;
frameLen = data[1] & 0xF;
}
else
{
frameType = frame.payload()[0] >> 4;
frameLen = frame.payload()[0] & 0xF;
frameType = data[0] >> 4;
frameLen = data[0] & 0xF;
}
switch(frameType)
@@ -186,42 +188,42 @@ void ISOTP_HANDLER::processFrame(const CANFrame &frame)
if (frameLen > 7) return;
msg.bus = frame.bus;
msg.extended = frame.hasExtendedFrameFormat();
msg.ID = ID;
msg.setExtendedFrameFormat( frame.hasExtendedFrameFormat() );
msg.setFrameId(ID);
msg.isReceived = frame.isReceived;
msg.len = frameLen;
msg.data.reserve(frameLen);
msg.timestamp = frame.timeStamp().microSeconds();
msg.payload().reserve(frameLen);
msg.reportedLength = frameLen;
msg.setTimeStamp(frame.timeStamp());
msg.isMultiframe = false;
if (useExtendedAddressing) for (int j = 0; j < frameLen; j++) msg.data.append(frame.payload()[j+2]);
else for (int j = 0; j < frameLen; j++) msg.data.append(frame.payload()[j+1]);
if (useExtendedAddressing) for (int j = 0; j < frameLen; j++) msg.payload().append(frame.payload()[j+2]);
else for (int j = 0; j < frameLen; j++) msg.payload().append(frame.payload()[j+1]);
//qDebug() << "Emitting single frame ISOTP message";
emit newISOMessage(msg);
break;
case 1: //first frame of a multi-frame message
checkNeedFlush(ID);
msg.bus = frame.bus;
msg.extended = frame.hasExtendedFrameFormat();
msg.ID = ID;
msg.timestamp = frame.timeStamp().microSeconds();
msg.setExtendedFrameFormat( frame.hasExtendedFrameFormat() );
msg.setFrameId(ID);
msg.setTimeStamp(frame.timeStamp());
msg.isReceived = frame.isReceived;
msg.isMultiframe = true;
frameLen = frameLen << 8;
if (useExtendedAddressing)
{
frameLen += frame.payload()[2];
frameLen += data[2];
frameLen = frameLen & 0xFFF;
msg.len = frameLen;
msg.data.reserve(frameLen);
for (int j = 0; j < 5; j++) msg.data.append(frame.payload()[3 + j]);
msg.payload().reserve(frameLen);
msg.reportedLength = frameLen;
for (int j = 0; j < 5; j++) msg.payload().append(frame.payload()[3 + j]);
}
else
{
frameLen += frame.payload()[1];
frameLen += data[1];
frameLen = frameLen & 0xFFF;
msg.len = frameLen;
msg.data.reserve(frameLen);
for (int j = 0; j < 6; j++) msg.data.append(frame.payload()[2 + j]);
msg.payload().reserve(frameLen);
msg.reportedLength = frameLen;
for (int j = 0; j < 6; j++) msg.payload().append(frame.payload()[2 + j]);
}
msg.lastSequence = -1;
messageBuffer.append(msg);
@@ -245,7 +247,7 @@ void ISOTP_HANDLER::processFrame(const CANFrame &frame)
pMsg = nullptr;
for (int i = 0; i < messageBuffer.length(); i++)
{
if (messageBuffer[i].ID == ID)
if (messageBuffer[i].frameId() == ID)
{
pMsg = &messageBuffer[i];
break;
@@ -253,22 +255,22 @@ void ISOTP_HANDLER::processFrame(const CANFrame &frame)
}
if (!pMsg) return;
if (!pMsg->isMultiframe) return; //if we didn't get a frame type 1 (start of multiframe) first then ignore this frame.
ln = pMsg->len - pMsg->data.count();
ln = pMsg->payload().length() - pMsg->payload().count();
//offset = pMsg->data.count();
if (useExtendedAddressing)
{
if (ln > 6) ln = 6;
for (int j = 0; j < ln; j++) pMsg->data.append(frame.payload()[j+2]);
for (int j = 0; j < ln; j++) pMsg->payload().append(frame.payload()[j+2]);
}
else
{
if (ln > 7) ln = 7;
for (int j = 0; j < ln; j++) pMsg->data.append(frame.payload()[j+1]);
for (int j = 0; j < ln; j++) pMsg->payload().append(frame.payload()[j+1]);
}
if (pMsg->len <= pMsg->data.count())
if (pMsg->reportedLength <= pMsg->payload().count())
{
//qDebug() << "Emitting multiframe ISOTP message";
checkNeedFlush(pMsg->ID);
checkNeedFlush(pMsg->frameId());
}
break;
case 3: //flow control messages
@@ -277,10 +279,10 @@ void ISOTP_HANDLER::processFrame(const CANFrame &frame)
case 0: //continue to send frames but maybe change inter-frame delay
waitingForFlow = false;
//data[1] contains number of frames to send before waiting for next flow control
framesUntilFlow = frame.payload()[1];
framesUntilFlow = data[1];
if (framesUntilFlow == 0) framesUntilFlow = -1; //-1 means don't count frames and just keep going
//data[2] contains the interframe delay to use (0xF1 through 0xF9 are special through)
if (frame.payload()[2] < (char)0xF1) frameTimer.start(frame.payload()[2]); //set proper delay between frames
if (data[2] < 0xF1) frameTimer.start(data[2]); //set proper delay between frames
else frameTimer.start(1); //can't do sub-millisecond sending with this code so just use 1ms timing
break;
case 1: //wait - do not send any more frames until other side says so
@@ -303,18 +305,18 @@ void ISOTP_HANDLER::checkNeedFlush(uint64_t ID)
{
for (int i = 0; i < messageBuffer.length(); i++)
{
if (messageBuffer[i].ID == ID)
if (messageBuffer[i].frameId() == ID)
{
//used to pass by reference but now newISOMessage should pass by value which makes it easier to use cross thread
if (messageBuffer[i].ID > 0x600 && messageBuffer[i].ID < 0x630)
if (messageBuffer[i].frameId() > 0x600 && messageBuffer[i].frameId() < 0x630)
{
if (messageBuffer[i].len <= messageBuffer[i].data.count())
if (messageBuffer[i].reportedLength <= messageBuffer[i].payload().count())
{
qDebug() << "Flushing full frame" << QString::number(messageBuffer[i].ID, 16) << " " << messageBuffer[i].len << " " << messageBuffer[i].data.count();
qDebug() << "Flushing full frame" << QString::number(messageBuffer[i].frameId(), 16) << " " << messageBuffer[i].reportedLength << " " << messageBuffer[i].payload().count();
}
else
{
qDebug() << "Flushing a partial frame " << QString::number(messageBuffer[i].ID, 16) << " " << messageBuffer[i].len << " " << messageBuffer[i].data.count();
qDebug() << "Flushing a partial frame " << QString::number(messageBuffer[i].frameId(), 16) << " " << messageBuffer[i].reportedLength << " " << messageBuffer[i].payload().count();
}
}
emit newISOMessage(messageBuffer[i]);
bus_protocols/isotp_handler.h
+1 -1
View File
@@ -19,7 +19,7 @@ public:
~ISOTP_HANDLER();
void setExtendedAddressing(bool mode);
void setReception(bool mode); //set whether to accept and forward frames or not
void sendISOTPFrame(int bus, int ID, QVector<unsigned char> data);
void sendISOTPFrame(int bus, int ID, QByteArray data);
void setProcessAll(bool state);
void setFlowCtrl(bool state);
void addFilter(uint32_t pBusId, uint32_t ID, uint32_t mask);
bus_protocols/isotp_message.h
+5 -10
View File
@@ -3,21 +3,16 @@
#include <Qt>
#include <QVector>
#include <can_structs.h>
//the same as the CANFrame struct but with arbitrary data size.
class ISOTP_MESSAGE
//Now a child class of CANFrame. We just add the ability to track how long it was supposed to be and other
//ISOTP related details. But, mostly just CANFrame.
class ISOTP_MESSAGE : public CANFrame
{
public:
uint32_t ID;
int bus;
bool extended;
bool isReceived;
int len; //# of bytes this message should have (as reported)
int actualSize; //# we actually got
int reportedLength;
int lastSequence;
bool isMultiframe;
QVector<unsigned char> data;
uint64_t timestamp;
};
#endif // ISOTP_MESSAGE_H
bus_protocols/uds_handler.cpp
+56 -55
View File
@@ -158,7 +158,6 @@ UDS_MESSAGE::UDS_MESSAGE()
subFunc = 0;
service = 0;
subFuncLen = 1;
extended = false;
isErrorReply = false;
}
@@ -178,39 +177,39 @@ UDS_HANDLER::~UDS_HANDLER()
void UDS_HANDLER::gotISOTPFrame(ISOTP_MESSAGE msg)
{
qDebug() << "UDS handler got ISOTP frame";
unsigned char *data = reinterpret_cast<unsigned char *>(msg.payload().data());
int dataLen = msg.payload().count();
UDS_MESSAGE udsMsg;
udsMsg.bus = msg.bus;
udsMsg.extended = msg.extended;
udsMsg.ID = msg.ID;
udsMsg.setExtendedFrameFormat(msg.hasExtendedFrameFormat());
udsMsg.setFrameId(msg.frameId());
udsMsg.isReceived = msg.isReceived;
udsMsg.timestamp = msg.timestamp;
udsMsg.actualSize = msg.actualSize;
udsMsg.setTimeStamp(msg.timeStamp());
udsMsg.reportedLength = msg.reportedLength;
udsMsg.service = 0;
udsMsg.subFunc = 0;
udsMsg.subFuncLen = 0;
udsMsg.isErrorReply = false;
udsMsg.len = msg.len;
if (msg.data.length() > 0) {
udsMsg.service = msg.data.at(0);
udsMsg.setPayload(msg.payload());
if (dataLen > 0) {
udsMsg.service = data[0];
if (udsMsg.service == 0x7F)
{
udsMsg.isErrorReply = true;
if (msg.data.length() > 1)
if (dataLen > 1)
{
udsMsg.service = msg.data.at(1);
if (msg.data.length() > 2) udsMsg.subFunc = msg.data.at(2);
udsMsg.service = data[1];
if (dataLen > 2) udsMsg.subFunc = data[2];
else return;
}
else return;
udsMsg.data = msg.data.mid(2, -1); //don't copy error byte nor service byte
udsMsg.len -= 2; //and remove those bytes from the length reported too.
udsMsg.payload().remove(0, 2);
}
else
{
udsMsg.isErrorReply = false;
if (msg.data.length() > 1) udsMsg.subFunc = msg.data.at(1);
udsMsg.data = msg.data.mid(1, -1); //don't copy service byte
udsMsg.len -= 1; //and remove service byte from length too.
if (dataLen > 1) udsMsg.subFunc = data[1];
udsMsg.payload().remove(0, 1);
}
}
else return;
@@ -245,7 +244,7 @@ void UDS_HANDLER::setReception(bool mode)
void UDS_HANDLER::sendUDSFrame(const UDS_MESSAGE &msg)
{
QVector<unsigned char> data;
QByteArray data;
if (msg.bus < 0) return;
if (msg.bus >= CANConManager::getInstance()->getNumBuses()) return;
if (msg.service > 0xFF) return;
@@ -256,8 +255,8 @@ void UDS_HANDLER::sendUDSFrame(const UDS_MESSAGE &msg)
data.append((msg.subFunc >> (8 * b)) & 0xFF);
}
data.append(msg.data);
isoHandler->sendISOTPFrame(msg.bus, msg.ID, data);
data.append(msg.payload());
isoHandler->sendISOTPFrame(msg.bus, msg.frameId(), data);
qDebug() << "Sent UDS service: " << getServiceShortDesc(msg.service) << " on bus " << msg.bus;
}
@@ -328,6 +327,8 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
bool isResponse = true;
int dataSize;
int addrSize;
unsigned char *data = reinterpret_cast<unsigned char *>(msg.payload().data());
int dataLen = msg.payload().length();
if (msg.isErrorReply)
{
@@ -363,15 +364,15 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
case UDS_SERVICES::DIAG_CONTROL + 0x40: //positive response
buildString.append("Session Request: " + getLongDesc(UDS_DIAG_CTRL_SUB, msg.subFunc));
//there should be four extra bytes now
if (msg.data.length() < 5) //5 because subfunc codes are left in data so it starts with one subfunc byte
if (dataLen < 5) //5 because subfunc codes are left in data so it starts with one subfunc byte
{
//buildString.append("\nReturned data payload wasn't at least \n4 bytes like it should have been");
}
else
{
int p2 = msg.data[1] * 256 + msg.data[2];
int p2 = data[1] * 256 + data[2];
buildString.append("\nP2MAX (Max Wait / Resp Time): " + QString::number(p2) + "ms");
p2 = (msg.data[3] * 256 + msg.data[4]) * 10;
p2 = (data[3] * 256 + data[4]) * 10;
buildString.append("\nP2 Ext MAX: " + QString::number(p2) + "ms");
}
break;
@@ -382,11 +383,11 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
case UDS_SERVICES::ECU_RESET + 0x40:
buildString.append("Reset Type: " + getLongDesc(UDS_ECU_RESET_SUB, msg.subFunc));
//There should be one additional byte which encodes power down time
if (msg.data.length() > 1)
if (dataLen > 1)
{
if (msg.data[1] < 0xFF)
if (data[1] < 0xFF)
{
buildString.append("\nMinimum powered down time: " + QString::number(msg.data[1]));
buildString.append("\nMinimum powered down time: " + QString::number(data[1]));
}
else buildString.append("\nPowerdown time not available");
}
@@ -395,11 +396,11 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
case UDS_SERVICES::COMM_CTRL:
//Comm control has potentially a lot of parameters. control type, comm type, nodeID
buildString.append("Control type: " + getLongDesc(UDS_COMM_CTRL_SUB, msg.subFunc));
if (msg.data.length() > 1)
buildString.append("\nComm Type: " + QString::number(msg.data[1])); //TODO: no attempt to interpret yet
if (msg.data.length() > 3)
if (dataLen > 1)
buildString.append("\nComm Type: " + QString::number(data[1])); //TODO: no attempt to interpret yet
if (dataLen > 3)
{
int nodeID = (msg.data[2] * 256 + msg.data[3]);
int nodeID = (data[2] * 256 + data[3]);
buildString.append("\nNode ID: " + Utility::formatHexNum(nodeID));
}
break;
@@ -407,19 +408,19 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
if ((msg.subFunc % 2) == 1)
{
buildString.append("Seed request for security level: " + QString::number(msg.subFunc) + "\n");
if (msg.data.length()> 1)
if (dataLen > 1)
{
buildString.append("Data payload: ");
for (int j = 1; j < msg.data.length(); j++) buildString.append(Utility::formatHexNum(msg.data[j]) + " ");
for (int j = 1; j < dataLen; j++) buildString.append(Utility::formatHexNum(data[j]) + " ");
}
}
else
{
buildString.append("Key sending for security level: " + QString::number(msg.subFunc - 1));
if (msg.data.length()> 1) //and it sure as hell should be!
if (dataLen > 1) //and it sure as hell should be!
{
buildString.append("KEY: ");
for (int j = 1; j < msg.data.length(); j++) buildString.append(Utility::formatHexNum(msg.data[j]) + " ");
for (int j = 1; j < dataLen; j++) buildString.append(Utility::formatHexNum(data[j]) + " ");
}
}
break;
@@ -427,10 +428,10 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
if ((msg.subFunc % 2) == 1)
{
buildString.append("Seed response for security level: " + QString::number(msg.subFunc) + "\n");
if (msg.data.length()> 1) //be kinda pointless if it weren't
if (dataLen > 1) //be kinda pointless if it weren't
{
buildString.append("SEED: ");
for (int j = 1; j < msg.data.length(); j++) buildString.append(Utility::formatHexNum(msg.data[j]) + " ");
for (int j = 1; j < dataLen; j++) buildString.append(Utility::formatHexNum(data[j]) + " ");
}
}
else
@@ -441,33 +442,33 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
break;
case UDS_SERVICES::READ_BY_ID:
//parameter is groups of two bytes, each of which specify an ID to read
if (msg.data.length() > 2)
if (dataLen > 2)
{
uint32_t id;
for (int i = 1; i < msg.data.length(); i = i + 2)
for (int i = 1; i < dataLen; i = i + 2)
{
id = (msg.data[i] * 256) + msg.data[i+1];
id = (data[i] * 256) + data[i+1];
buildString.append("\nID to read: " + Utility::formatHexNum(id));
}
}
break;
case UDS_SERVICES::READ_BY_ID + 0x40: //reply
buildString.append("Reply is non-standard and so no decoding is done. The format is (ID) followed by how ever much data that ID returns, followed by more ID/data pairs if applicable.\nPayload: ");
for (int i = 1; i < msg.data.length(); i++)
for (int i = 1; i < dataLen; i++)
{
buildString.append(Utility::formatHexNum(msg.data[i]) + " ");
buildString.append(Utility::formatHexNum(data[i]) + " ");
}
break;
case UDS_SERVICES::READ_BY_ADDR:
//subfunc byte specifies address and length format, then address, then size
dataSize = msg.subFunc >> 4;
addrSize = msg.subFunc & 0xF;
if (msg.data.length() > (dataSize + addrSize))
if (dataLen > (dataSize + addrSize))
{
buildString.append("Address: 0x");
for (int i = 0; i < addrSize; i++) buildString.append(QString::number(msg.data[1+i], 16).toUpper().rightJustified(2,'0'));
for (int i = 0; i < addrSize; i++) buildString.append(QString::number(data[1+i], 16).toUpper().rightJustified(2,'0'));
buildString.append("\nSize: 0x");
for (int i = 0; i < dataSize; i++) buildString.append(QString::number(msg.data[1+i+addrSize], 16).toUpper().rightJustified(2,'0'));
for (int i = 0; i < dataSize; i++) buildString.append(QString::number(data[1+i+addrSize], 16).toUpper().rightJustified(2,'0'));
}
else
{
@@ -476,44 +477,44 @@ QString UDS_HANDLER::getDetailedMessageAnalysis(const UDS_MESSAGE &msg)
break;
case UDS_SERVICES::READ_BY_ADDR + 0x40:
buildString.append("Reply is a raw packet of data of the size requested.\nPayload: ");
for (int i = 1; i < msg.data.length(); i++)
for (int i = 1; i < dataLen; i++)
{
buildString.append(Utility::formatHexNum(msg.data[i]) + " ");
buildString.append(Utility::formatHexNum(data[i]) + " ");
}
break;
case UDS_SERVICES::WRITE_BY_ID:
break;
case UDS_SERVICES::ROUTINE_CTRL:
buildString.append("Routine Control: " + getLongDesc(UDS_ROUTINE_SUB, msg.subFunc));
if (msg.data.length() > 2)
if (dataLen > 2)
{
int routineID;
routineID = (msg.data[1] * 256 + msg.data[2]);
routineID = (data[1] * 256 + data[2]);
buildString.append("\nRoutine ID: " + Utility::formatHexNum(routineID));
}
if (msg.data.length() > 3)
if (dataLen > 3)
{
buildString.append("\nParameter bytes to routine: ");
for (int i = 4; i < msg.data.length(); i++)
for (int i = 4; i < dataLen; i++)
{
buildString.append(Utility::formatHexNum(msg.data[i]) + " ");
buildString.append(Utility::formatHexNum(data[i]) + " ");
}
}
break;
case UDS_SERVICES::ROUTINE_CTRL + 0x40:
buildString.append("Routine Control: " + getLongDesc(UDS_ROUTINE_SUB, msg.subFunc));
if (msg.data.length() > 2)
if (dataLen > 2)
{
int routineID;
routineID = (msg.data[1] * 256 + msg.data[2]);
routineID = (data[1] * 256 + data[2]);
buildString.append("\nRoutine ID: " + Utility::formatHexNum(routineID));
}
if (msg.data.length() > 3)
if (dataLen > 3)
{
buildString.append("\nBytes returned by routine: ");
for (int i = 4; i < msg.data.length(); i++)
for (int i = 4; i < dataLen; i++)
{
buildString.append(Utility::formatHexNum(msg.data[i]) + " ");
buildString.append(Utility::formatHexNum(data[i]) + " ");
}
}
break;