sven
/
libicsneo
mirror of https://github.com/intrepidcs/libicsneo.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

CANPacket: Ensure correct padding when padding by the DLC

v0.3.0-dev
Paul Hollinsky 2021-05-25 22:19:03 -04:00
parent 76b6ecbf04
commit ed5132e852
1 changed files with 67 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
communication/packet/canpacket.cpp
View File

@ -4,26 +4,54 @@
using namespace icsneo; using namespace icsneo;
static optional<uint8_t> CANFD_DLCToLength(uint8_t length) { static optional<uint8_t> CAN_DLCToLength(uint8_t length, bool fd) {
if (length < 8) if (length <= 8)
return length; return length;
switch(length) { if (fd) {
case 0x9: switch(length) {
return 12; case 0x9:
case 0xa: return 12;
return 16; case 0xa:
case 0xb: return 16;
return 20; case 0xb:
case 0xc: return 20;
return 24; case 0xc:
case 0xd: return 24;
return 32; case 0xd:
case 0xe: return 32;
return 48; case 0xe:
case 0xf: return 48;
return 64; case 0xf:
return 64;
}
} }
return nullopt;
}
static optional<uint8_t> CAN_LengthToDLC(size_t dataLength, bool fd)
{
if (dataLength <= 8)
return dataLength;
if (fd) {
if (dataLength <= 12)
return 0x9;
else if (dataLength <= 16)
return 0xA;
else if (dataLength <= 20)
return 0xB;
else if (dataLength <= 24)
return 0xC;
else if (dataLength <= 32)
return 0xD;
else if (dataLength <= 48)
return 0xE;
else if (dataLength <= 64)
return 0xF;
}
return nullopt; return nullopt;
} }
@ -66,7 +94,7 @@ std::shared_ptr<Message> HardwareCANPacket::DecodeToMessage(const std::vector<ui
msg->isCANFD = true; msg->isCANFD = true;
msg->baudrateSwitch = data->header.BRS; // CAN FD Baudrate Switch msg->baudrateSwitch = data->header.BRS; // CAN FD Baudrate Switch
msg->errorStateIndicator = data->header.ESI; msg->errorStateIndicator = data->header.ESI;
const optional<uint8_t> lenFromDLC = CANFD_DLCToLength(length); const optional<uint8_t> lenFromDLC = CAN_DLCToLength(length, true);
if (lenFromDLC) if (lenFromDLC)
length = *lenFromDLC; length = *lenFromDLC;
} else if(length > 8) { // This is a standard CAN frame with a length of more than 8 } else if(length > 8) { // This is a standard CAN frame with a length of more than 8
@ -105,112 +133,35 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
} }
const size_t dataSize = message.data.size(); const size_t dataSize = message.data.size();
if(dataSize > 64 || (dataSize > 8 && !message.isCANFD)) { optional<uint8_t> dlc = CAN_LengthToDLC(dataSize, message.isCANFD);
if (!dlc.has_value()) {
report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error); report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error);
return false; // Too much data for the protocol return false; // Too much data for the protocol
} }
if(message.dlcOnWire > 0xf) { if (message.dlcOnWire != 0) {
// The DLC is only a nibble if(message.dlcOnWire > 0xf) {
// It is actually possible to transmit a standard CAN frame with a DLC > 8 // The DLC is only a nibble
// While it is invalid, most controllers will still pass along the received // It is actually possible to transmit a standard CAN frame with a DLC > 8
// frame and 8 bytes of data, so it may be desirable to test behavior with // While it is invalid, most controllers will still pass along the received
// these frames. We let you do it if you set `message.dlcOnWire` for transmit. // frame and 8 bytes of data, so it may be desirable to test behavior with
report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error); // these frames. We let you do it if you set `message.dlcOnWire` for transmit.
return false;
}
uint8_t lengthNibble = uint8_t(message.data.size());
const optional<uint8_t> lenFromDLC = CANFD_DLCToLength(message.dlcOnWire);
if (lenFromDLC.has_value() && *lenFromDLC != 0) {
if (*lenFromDLC < lengthNibble) {
report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error); report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error);
return false; return false;
} }
if (*lenFromDLC > lengthNibble) if (message.dlcOnWire < *dlc) {
lengthNibble = *lenFromDLC; report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error);
return false;
}
if (message.dlcOnWire > *dlc)
dlc = message.dlcOnWire;
} }
uint8_t paddingBytes = 0; // The only way this fails is if we're transmitting a DLC > 8 on standard CAN
if(lengthNibble > 8) { const uint8_t paddedLength = CAN_DLCToLength(*dlc, message.isCANFD).value_or(8);
switch(lengthNibble) { const uint8_t paddingBytes = paddedLength - dataSize;
case 9: paddingBytes++;
case 10: paddingBytes++;
case 11: paddingBytes++;
case 12:
lengthNibble = 0x9;
break;
case 13: paddingBytes++;
case 14: paddingBytes++;
case 15: paddingBytes++;
case 16:
lengthNibble = 0xA;
break;
case 17: paddingBytes++;
case 18: paddingBytes++;
case 19: paddingBytes++;
case 20:
lengthNibble = 0xB;
break;
case 21: paddingBytes++;
case 22: paddingBytes++;
case 23: paddingBytes++;
case 24:
lengthNibble = 0xC;
break;
case 25: paddingBytes++;
case 26: paddingBytes++;
case 27: paddingBytes++;
case 28: paddingBytes++;
case 29: paddingBytes++;
case 30: paddingBytes++;
case 31: paddingBytes++;
case 32:
lengthNibble = 0xD;
break;
case 33: paddingBytes++;
case 34: paddingBytes++;
case 35: paddingBytes++;
case 36: paddingBytes++;
case 37: paddingBytes++;
case 38: paddingBytes++;
case 39: paddingBytes++;
case 40: paddingBytes++;
case 41: paddingBytes++;
case 42: paddingBytes++;
case 43: paddingBytes++;
case 44: paddingBytes++;
case 45: paddingBytes++;
case 46: paddingBytes++;
case 47: paddingBytes++;
case 48:
lengthNibble = 0xE;
break;
case 49: paddingBytes++;
case 50: paddingBytes++;
case 51: paddingBytes++;
case 52: paddingBytes++;
case 53: paddingBytes++;
case 54: paddingBytes++;
case 55: paddingBytes++;
case 56: paddingBytes++;
case 57: paddingBytes++;
case 58: paddingBytes++;
case 59: paddingBytes++;
case 60: paddingBytes++;
case 61: paddingBytes++;
case 62: paddingBytes++;
case 63: paddingBytes++;
case 64:
lengthNibble = 0xF;
break;
default:
report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error);
return false; // CAN FD frame may have had an incorrect byte count
}
}
// Pre-allocate as much memory as we will possibly need for speed // Pre-allocate as much memory as we will possibly need for speed
result.reserve(17 + dataSize + paddingBytes); result.reserve(17 + dataSize + paddingBytes);
@ -248,7 +199,7 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
// Status and DLC bits // Status and DLC bits
if(message.isCANFD) { if(message.isCANFD) {
result.push_back(0x0F); // FD Frame result.push_back(0x0F); // FD Frame
uint8_t fdStatusByte = lengthNibble; uint8_t fdStatusByte = *dlc;
if(message.baudrateSwitch) if(message.baudrateSwitch)
fdStatusByte |= 0x80; // BRS status bit fdStatusByte |= 0x80; // BRS status bit
// The firmware does not yet support transmitting ESI // The firmware does not yet support transmitting ESI
@ -256,7 +207,7 @@ bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector
} else { } else {
// TODO Support high voltage wakeup, bitwise-or in 0x8 here to enable // TODO Support high voltage wakeup, bitwise-or in 0x8 here to enable
uint8_t statusNibble = message.isRemote ? 0x4 : 0x0; uint8_t statusNibble = message.isRemote ? 0x4 : 0x0;
result.push_back((statusNibble << 4) | lengthNibble); result.push_back((statusNibble << 4) | *dlc);
} }
// Now finally the payload // Now finally the payload