sven/libicsneo
1
0
Fork 0
mirror of https://github.com/intrepidcs/libicsneo.git synced 2026-08-05 01:18:36 +02:00
Code Issues Projects Releases Wiki Activity

Support Ethernet and Broad-R Reach TX and RX

Browse Source
This commit is contained in:
Paul Hollinsky
2018-12-21 20:32:27 -05:00
parent 603d532d2d
commit d37d5bb23e
14 changed files with 490 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
communication/decoder.cpp
+19 -71
View File
@@ -5,6 +5,8 @@
#include "icsneo/communication/message/readsettingsmessage.h"
#include "icsneo/communication/command.h"
#include "icsneo/device/device.h"
#include "icsneo/communication/packet/canpacket.h"
#include "icsneo/communication/packet/ethernetpacket.h"
#include <iostream>
using namespace icsneo;
@@ -18,82 +20,28 @@ uint64_t Decoder::GetUInt64FromLEBytes(uint8_t* bytes) {
bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Packet>& packet) {
switch(packet->network.getType()) {
case Network::Type::Ethernet:
result = HardwareEthernetPacket::DecodeToMessage(packet->data);
if(!result)
return false; // A nullptr was returned, the packet was not long enough to decode
// Timestamps are in (multiplier) ns increments since 1/1/2007 GMT 00:00:00.0000
// The resolution (multiplier) depends on the device
result->timestamp *= timestampMultiplier;
result->network = packet->network;
return true;
case Network::Type::CAN: {
if(packet->data.size() < 24)
return false;
HardwareCANPacket* data = (HardwareCANPacket*)packet->data.data();
auto msg = std::make_shared<CANMessage>();
msg->network = packet->network;
result = HardwareCANPacket::DecodeToMessage(packet->data);
if(!result)
return false; // A nullptr was returned, the packet was malformed
// Timestamp calculation
msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000
// Arb ID
if(data->header.IDE) { // Extended 29-bit ID
msg->arbid = (data->header.SID & 0x7ff) << 18;
msg->arbid |= (data->eid.EID & 0xfff) << 6;
msg->arbid |= (data->dlc.EID2 & 0x3f);
msg->isExtended = true;
} else { // Standard 11-bit ID
msg->arbid = data->header.SID;
}
// DLC
uint8_t length = data->dlc.DLC;
msg->dlcOnWire = length; // This will hold the real DLC on wire 0x0 - 0xF
if(data->header.EDL && data->timestamp.IsExtended) { // CAN FD
msg->isCANFD = true;
msg->baudrateSwitch = data->header.BRS; // CAN FD Baudrate Switch
if(length > 8) {
switch(length) { // CAN FD Length Decoding
case 0x9:
length = 12;
break;
case 0xa:
length = 16;
break;
case 0xb:
length = 20;
break;
case 0xc:
length = 24;
break;
case 0xd:
length = 32;
break;
case 0xe:
length = 48;
break;
case 0xf:
length = 64;
break;
default:
return false;
}
}
} else if(length > 8) { // This is a standard CAN frame with a length of more than 8
// Yes, this is possible. On the wire, the length field is a nibble, and we do want to return an accurate value
// We don't want to overread our buffer, though, so make sure we cap the length
length = 8;
}
// Data
// The first 8 bytes are always in the standard place
if((data->dlc.RTR && data->header.IDE) || (!data->header.IDE && data->header.SRR)) { // Remote Request Frame
msg->data.resize(length); // This data will be all zeros, but the length will be set
msg->isRemote = true;
} else {
msg->data.reserve(length);
msg->data.insert(msg->data.end(), data->data, data->data + (length > 8 ? 8 : length));
if(length > 8) { // If there are more than 8 bytes, they come at the end of the message
// Messages with extra data are formatted as message, then uint16_t netid, then uint16_t length, then extra data
uint8_t* extraDataStart = packet->data.data() + sizeof(HardwareCANPacket) + 2 + 2;
msg->data.insert(msg->data.end(), extraDataStart, extraDataStart + (length - 8));
}
}
result = msg;
// Timestamps are in (multiplier) ns increments since 1/1/2007 GMT 00:00:00.0000
// The resolution (multiplier) depends on the device
result->timestamp *= timestampMultiplier;
result->network = packet->network;
return true;
}
case Network::Type::Internal: {
communication/encoder.cpp
+17 -83
View File
@@ -1,4 +1,7 @@
#include "icsneo/communication/encoder.h"
#include "icsneo/communication/message/ethernetmessage.h"
#include "icsneo/communication/packet/ethernetpacket.h"
#include "icsneo/communication/packet/canpacket.h"
using namespace icsneo;
@@ -8,9 +11,18 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
result.clear();
switch(message->network.getType()) {
case Network::Type::CAN: {
useResultAsBuffer = true;
case Network::Type::Ethernet: {
auto ethmsg = std::dynamic_pointer_cast<EthernetMessage>(message);
if(!ethmsg)
return false; // The message was not a properly formed EthernetMessage
useResultAsBuffer = true;
if(!HardwareEthernetPacket::EncodeFromMessage(*ethmsg, result))
return false;
break;
} // End of Network::Type::Ethernet
case Network::Type::CAN: {
auto canmsg = std::dynamic_pointer_cast<CANMessage>(message);
if(!canmsg)
return false; // The message was not a properly formed CANMessage
@@ -18,88 +30,10 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
if(!supportCANFD && canmsg->isCANFD)
return false; // This device does not support CAN FD
if(canmsg->isCANFD && canmsg->isRemote)
return false; // RTR frames can not be used with CAN FD
useResultAsBuffer = true;
if(!HardwareCANPacket::EncodeFromMessage(*canmsg, result))
return false; // The CANMessage was malformed
const size_t dataSize = canmsg->data.size();
if(dataSize > 64 || (dataSize > 8 && !canmsg->isCANFD))
return false; // Too much data for the protocol
uint8_t lengthNibble = uint8_t(canmsg->data.size());
if(lengthNibble > 8) {
switch(lengthNibble) {
case 12:
lengthNibble = 0x9;
break;
case 16:
lengthNibble = 0xA;
break;
case 20:
lengthNibble = 0xB;
break;
case 24:
lengthNibble = 0xC;
break;
case 32:
lengthNibble = 0xD;
break;
case 48:
lengthNibble = 0xE;
break;
case 64:
lengthNibble = 0xF;
break;
default:
return false; // CAN FD frame may have had an incorrect byte count
}
}
// Pre-allocate as much memory as we will possibly need for speed
result.reserve(17 + dataSize);
result.push_back(0 /* byte count here later */ << 4 | (uint8_t(canmsg->network.getNetID()) & 0xF));
result.insert(result.end(), {0,0}); // Two bytes for Description ID, big endian, not used in API currently
// Next 2-4 bytes are ArbID
if(canmsg->isExtended) {
if(canmsg->arbid >= 0x20000000) // Extended messages use 29-bit arb IDs
return false;
result.insert(result.end(), {
(uint8_t)(canmsg->arbid >> 21),
(uint8_t)(((((canmsg->arbid & 0x001C0000) >> 13) & 0xFF) + (((canmsg->arbid & 0x00030000) >> 16) & 0xFF)) | 8),
(uint8_t)(canmsg->arbid >> 8),
(uint8_t)canmsg->arbid
});
} else {
if(canmsg->arbid >= 0x800) // Standard messages use 11-bit arb IDs
return false;
result.insert(result.end(), {
(uint8_t)(canmsg->arbid >> 3),
(uint8_t)((canmsg->arbid & 0x7) << 5)
});
}
// Status and DLC bits
if(canmsg->isCANFD) {
result.push_back(0x0F); // FD Frame
uint8_t fdStatusByte = lengthNibble;
if(canmsg->baudrateSwitch)
fdStatusByte |= 0x80; // BRS status bit
result.push_back(fdStatusByte);
} else {
// TODO Support high voltage wakeup, bitwise-or in 0x8 here to enable
uint8_t statusNibble = canmsg->isRemote ? 0x4 : 0x0;
result.push_back((statusNibble << 4) | lengthNibble);
}
// Now finally the payload
result.insert(result.end(), canmsg->data.begin(), canmsg->data.end());
result.push_back(0);
// Fill in the length byte from earlier
result[0] |= result.size() << 4;
break;
} // End of Network::Type::CAN
default:
communication/message/neomessage.cpp
+14
View File
@@ -1,5 +1,6 @@
#include "icsneo/communication/message/neomessage.h"
#include "icsneo/communication/message/canmessage.h"
#include "icsneo/communication/message/ethernetmessage.h"
using namespace icsneo;
@@ -13,6 +14,8 @@ neomessage_t icsneo::CreateNeoMessage(const std::shared_ptr<Message> message) {
neomsg.length = message->data.size();
neomsg.data = message->data.data();
neomsg.timestamp = message->timestamp;
neomsg.status.globalError = message->error;
neomsg.status.transmitMessage = message->transmitted;
switch(type) {
case Network::Type::CAN: {
@@ -27,6 +30,17 @@ neomessage_t icsneo::CreateNeoMessage(const std::shared_ptr<Message> message) {
can.status.canfdBRS = canmsg->baudrateSwitch;
break;
}
case Network::Type::Ethernet: {
neomessage_eth_t& eth = *(neomessage_eth_t*)&neomsg;
auto ethmsg = std::static_pointer_cast<EthernetMessage>(message);
eth.preemptionFlags = ethmsg->preemptionFlags;
eth.status.incompleteFrame = ethmsg->frameTooShort;
// TODO Fill in extra status bits
//eth.status.xyz = ethmsg->preemptionEnabled;
//eth.status.xyz = ethmsg->fcsAvailable;
//eth.status.xyz = ethmsg->noPadding;
break;
}
default:
// TODO Implement others
break;
communication/packet/canpacket.cpp
+164
View File
@@ -0,0 +1,164 @@
#include "icsneo/communication/packet/canpacket.h"
using namespace icsneo;
std::shared_ptr<CANMessage> HardwareCANPacket::DecodeToMessage(const std::vector<uint8_t>& bytestream) {
const HardwareCANPacket* data = (const HardwareCANPacket*)bytestream.data();
auto msg = std::make_shared<CANMessage>();
// Arb ID
if(data->header.IDE) { // Extended 29-bit ID
msg->arbid = (data->header.SID & 0x7ff) << 18;
msg->arbid |= (data->eid.EID & 0xfff) << 6;
msg->arbid |= (data->dlc.EID2 & 0x3f);
msg->isExtended = true;
} else { // Standard 11-bit ID
msg->arbid = data->header.SID;
}
// DLC
uint8_t length = data->dlc.DLC;
msg->dlcOnWire = length; // This will hold the real DLC on wire 0x0 - 0xF
if(data->header.EDL && data->timestamp.IsExtended) { // CAN FD
msg->isCANFD = true;
msg->baudrateSwitch = data->header.BRS; // CAN FD Baudrate Switch
if(length > 8) {
switch(length) { // CAN FD Length Decoding
case 0x9:
length = 12;
break;
case 0xa:
length = 16;
break;
case 0xb:
length = 20;
break;
case 0xc:
length = 24;
break;
case 0xd:
length = 32;
break;
case 0xe:
length = 48;
break;
case 0xf:
length = 64;
break;
default:
return nullptr;
}
}
} else if(length > 8) { // This is a standard CAN frame with a length of more than 8
// Yes, this is possible. On the wire, the length field is a nibble, and we do want to return an accurate value
// We don't want to overread our buffer, though, so make sure we cap the length
length = 8;
}
// Data
// The first 8 bytes are always in the standard place
if((data->dlc.RTR && data->header.IDE) || (!data->header.IDE && data->header.SRR)) { // Remote Request Frame
msg->data.resize(length); // This data will be all zeros, but the length will be set
msg->isRemote = true;
} else {
msg->data.reserve(length);
msg->data.insert(msg->data.end(), data->data, data->data + (length > 8 ? 8 : length));
if(length > 8) { // If there are more than 8 bytes, they come at the end of the message
// Messages with extra data are formatted as message, then uint16_t netid, then uint16_t length, then extra data
const auto extraDataStart = bytestream.begin() + sizeof(HardwareCANPacket) + 2 + 2;
msg->data.insert(msg->data.end(), extraDataStart, extraDataStart + (length - 8));
}
}
return msg;
}
bool HardwareCANPacket::EncodeFromMessage(const CANMessage& message, std::vector<uint8_t>& result) {
if(message.isCANFD && message.isRemote)
return false; // RTR frames can not be used with CAN FD
const size_t dataSize = message.data.size();
if(dataSize > 64 || (dataSize > 8 && !message.isCANFD))
return false; // Too much data for the protocol
uint8_t lengthNibble = uint8_t(message.data.size());
if(lengthNibble > 8) {
switch(lengthNibble) {
case 12:
lengthNibble = 0x9;
break;
case 16:
lengthNibble = 0xA;
break;
case 20:
lengthNibble = 0xB;
break;
case 24:
lengthNibble = 0xC;
break;
case 32:
lengthNibble = 0xD;
break;
case 48:
lengthNibble = 0xE;
break;
case 64:
lengthNibble = 0xF;
break;
default:
return false; // CAN FD frame may have had an incorrect byte count
}
}
// Pre-allocate as much memory as we will possibly need for speed
result.reserve(17 + dataSize);
result.push_back(0 /* byte count here later */ << 4 | (uint8_t(message.network.getNetID()) & 0xF));
// Two bytes for Description ID, big endian
result.insert(result.end(), { uint8_t(message.description >> 8), uint8_t(message.description) });
// Next 2-4 bytes are ArbID
if(message.isExtended) {
if(message.arbid >= 0x20000000) // Extended messages use 29-bit arb IDs
return false;
result.insert(result.end(), {
(uint8_t)(message.arbid >> 21),
(uint8_t)(((((message.arbid & 0x001C0000) >> 13) & 0xFF) + (((message.arbid & 0x00030000) >> 16) & 0xFF)) | 8),
(uint8_t)(message.arbid >> 8),
(uint8_t)message.arbid
});
} else {
if(message.arbid >= 0x800) // Standard messages use 11-bit arb IDs
return false;
result.insert(result.end(), {
(uint8_t)(message.arbid >> 3),
(uint8_t)((message.arbid & 0x7) << 5)
});
}
// Status and DLC bits
if(message.isCANFD) {
result.push_back(0x0F); // FD Frame
uint8_t fdStatusByte = lengthNibble;
if(message.baudrateSwitch)
fdStatusByte |= 0x80; // BRS status bit
result.push_back(fdStatusByte);
} else {
// TODO Support high voltage wakeup, bitwise-or in 0x8 here to enable
uint8_t statusNibble = message.isRemote ? 0x4 : 0x0;
result.push_back((statusNibble << 4) | lengthNibble);
}
// Now finally the payload
result.insert(result.end(), message.data.begin(), message.data.end());
result.push_back(0);
// Fill in the length byte from earlier
result[0] |= result.size() << 4;
return true;
}
communication/packet/ethernetpacket.cpp
+86
View File
@@ -0,0 +1,86 @@
#include "icsneo/communication/packet/ethernetpacket.h"
#include <cstring> // memcpy
#include <iostream>
using namespace icsneo;
std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const std::vector<uint8_t>& bytestream) {
const HardwareEthernetPacket* packet = (const HardwareEthernetPacket*)((const void*)bytestream.data());
const uint16_t* rawWords = (const uint16_t*)bytestream.data();
// Make sure we have enough to read the packet length first
if(bytestream.size() < sizeof(HardwareEthernetPacket))
return nullptr;
// packet->Length will also encompass the two uint16_t's at the end of the struct, make sure that at least they are here
if(packet->Length < 4)
return nullptr;
size_t bytesOnWire = packet->Length - (sizeof(uint16_t) * 2);
if(bytestream.size() < sizeof(HardwareEthernetPacket) + bytesOnWire)
return nullptr;
if(bytestream.size() > sizeof(HardwareEthernetPacket) + bytesOnWire)
std::cout << "There is an extra " << (sizeof(HardwareEthernetPacket) + bytesOnWire) << " bytes at the end" << std::endl;
auto messagePtr = std::make_shared<EthernetMessage>();
EthernetMessage& message = *messagePtr;
message.transmitted = packet->eid.TXMSG;
if(message.transmitted)
message.description = packet->stats;
message.preemptionEnabled = packet->header.PREEMPTION_ENABLED;
if(message.preemptionEnabled)
message.preemptionFlags = (uint8_t)((rawWords[0] & 0x03F8) >> 4);
message.fcsAvailable = packet->header.FCS_AVAIL;
message.frameTooShort = packet->header.RUNT_FRAME;
if(message.frameTooShort)
message.error = true;
// This timestamp is raw off the device (in timestampMultiplier increments)
// Decoder will fix as it has information about the timestampMultiplier increments
message.timestamp = packet->timestamp.TS;
// Network ID is also not set, this will be fixed in the Decoder as well
const std::vector<uint8_t>::const_iterator databegin = bytestream.begin() + (sizeof(HardwareEthernetPacket) - (sizeof(uint16_t) * 2));
const std::vector<uint8_t>::const_iterator dataend = databegin + bytesOnWire;
message.data.insert(message.data.begin(), databegin, dataend);
return messagePtr;
}
bool HardwareEthernetPacket::EncodeFromMessage(const EthernetMessage& message, std::vector<uint8_t>& bytestream) {
const size_t unpaddedSize = message.data.size();
size_t paddedSize = unpaddedSize;
if(!message.noPadding && unpaddedSize < 60)
paddedSize = 60; // Pad out short messages
size_t sizeWithHeader = paddedSize + 5; // DescriptionID and Premption Flags
bytestream.reserve(sizeWithHeader + 8); // Also reserve space for the bytes we'll use later on
bytestream.resize(sizeWithHeader);
size_t index = 0;
// Padded size, little endian
bytestream[index++] = uint8_t(paddedSize);
bytestream[index++] = uint8_t(paddedSize >> 8);
// Description ID, big endian
bytestream[index++] = uint8_t(message.description >> 8);
bytestream[index++] = uint8_t(message.description);
// Yes, we reserved and allocated space for the preemption flags even if we're not putting them there
// And yes, the data is intended to move over one byte
if(message.preemptionEnabled)
bytestream[index++] = message.preemptionFlags;
// We only copy in the unpadded size, the rest will be 0
memcpy(bytestream.data() + index, message.data.data(), unpaddedSize);
return true;
}