Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

CANParser: Update CAN data handling functions to accept raw data and size parameters #1040

Closed
wants to merge 1 commit into from
Closed

CANParser: Update CAN data handling functions to accept raw data and size parameters #1040

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 29 additions & 29 deletions can/common.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
#include "opendbc/can/common.h"


unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int honda_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
int s = 0;
bool extended = address > 0x7FF;
while (address) { s += (address & 0xF); address >>= 4; }
for (int i = 0; i < d.size(); i++) {
uint8_t x = d[i];
if (i == d.size()-1) x >>= 4; // remove checksum
for (int i = 0; i < size; i++) {
uint8_t x = data[i];
if (i == size-1) x >>= 4; // remove checksum
s += (x & 0xF) + (x >> 4);
}
s = 8-s;
Expand All @@ -16,30 +16,30 @@ unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vect
return s & 0xF;
}

unsigned int toyota_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int s = d.size();
unsigned int toyota_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
unsigned int s = size;
while (address) { s += address & 0xFF; address >>= 8; }
for (int i = 0; i < d.size() - 1; i++) { s += d[i]; }
for (int i = 0; i < size - 1; i++) { s += data[i]; }

return s & 0xFF;
}

unsigned int subaru_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int subaru_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
unsigned int s = 0;
while (address) { s += address & 0xFF; address >>= 8; }

// skip checksum in first byte
for (int i = 1; i < d.size(); i++) { s += d[i]; }
for (int i = 1; i < size; i++) { s += data[i]; }

return s & 0xFF;
}

unsigned int chrysler_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int chrysler_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
// jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf
uint8_t checksum = 0xFF;
for (int j = 0; j < (d.size() - 1); j++) {
for (int j = 0; j < (size - 1); j++) {
uint8_t shift = 0x80;
uint8_t curr = d[j];
uint8_t curr = data[j];
for (int i = 0; i < 8; i++) {
uint8_t bit_sum = curr & shift;
uint8_t temp_chk = checksum & 0x80U;
Expand Down Expand Up @@ -108,22 +108,22 @@ void init_crc_lookup_tables() {
gen_crc_lookup_table_16(0x1021, crc16_lut_xmodem); // CRC-16 XMODEM for HKG CAN FD
}

unsigned int volkswagen_mqb_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int volkswagen_mqb_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
// Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with
// a magic variable padding byte tacked onto the end of the payload.
// https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf

uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR

// CRC the payload first, skipping over the first byte where the CRC lives.
for (int i = 1; i < d.size(); i++) {
crc ^= d[i];
for (int i = 1; i < size; i++) {
crc ^= data[i];
crc = crc8_lut_8h2f[crc];
}

// Look up and apply the magic final CRC padding byte, which permutes by CAN
// address, and additionally (for SOME addresses) by the message counter.
uint8_t counter = d[1] & 0x0F;
uint8_t counter = data[1] & 0x0F;
switch (address) {
case 0x86: // LWI_01 Steering Angle
crc ^= (uint8_t[]){0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86}[counter];
Expand Down Expand Up @@ -189,27 +189,27 @@ unsigned int volkswagen_mqb_checksum(uint32_t address, const Signal &sig, const
return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
}

unsigned int xor_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int xor_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
uint8_t checksum = 0;
int checksum_byte = sig.start_bit / 8;

// Simple XOR over the payload, except for the byte where the checksum lives.
for (int i = 0; i < d.size(); i++) {
for (int i = 0; i < size; i++) {
if (i != checksum_byte) {
checksum ^= d[i];
checksum ^= data[i];
}
}

return checksum;
}

unsigned int pedal_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int pedal_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
uint8_t crc = 0xFF;
uint8_t poly = 0xD5; // standard crc8

// skip checksum byte
for (int i = d.size()-2; i >= 0; i--) {
crc ^= d[i];
for (int i = size-2; i >= 0; i--) {
crc ^= data[i];
for (int j = 0; j < 8; j++) {
if ((crc & 0x80) != 0) {
crc = (uint8_t)((crc << 1) ^ poly);
Expand All @@ -221,24 +221,24 @@ unsigned int pedal_checksum(uint32_t address, const Signal &sig, const std::vect
return crc;
}

unsigned int hkg_can_fd_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d) {
unsigned int hkg_can_fd_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size) {
uint16_t crc = 0;

for (int i = 2; i < d.size(); i++) {
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ d[i]];
for (int i = 2; i < size; i++) {
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ data[i]];
}

// Add address to crc
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ ((address >> 0) & 0xFF)];
crc = (crc << 8) ^ crc16_lut_xmodem[(crc >> 8) ^ ((address >> 8) & 0xFF)];

if (d.size() == 8) {
if (size == 8) {
crc ^= 0x5f29;
} else if (d.size() == 16) {
} else if (size == 16) {
crc ^= 0x041d;
} else if (d.size() == 24) {
} else if (size == 24) {
crc ^= 0x819d;
} else if (d.size() == 32) {
} else if (size == 32) {
crc ^= 0x9f5b;
}

Expand Down
18 changes: 9 additions & 9 deletions can/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,14 +26,14 @@
void init_crc_lookup_tables();

// Car specific functions
unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int toyota_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int subaru_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int chrysler_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int volkswagen_mqb_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int xor_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int hkg_can_fd_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int pedal_checksum(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int honda_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int toyota_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int subaru_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int chrysler_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int volkswagen_mqb_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int xor_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int hkg_can_fd_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);
unsigned int pedal_checksum(uint32_t address, const Signal &sig, const uint8_t *data, size_t size);

class MessageState {
public:
Expand All @@ -54,7 +54,7 @@ class MessageState {
bool ignore_checksum = false;
bool ignore_counter = false;

bool parse(uint64_t nanos, const std::vector<uint8_t> &dat);
bool parse(uint64_t nanos, const uint8_t *msg, const size_t msg_size);
bool update_counter_generic(int64_t v, int cnt_size);
};

Expand Down
4 changes: 2 additions & 2 deletions can/common_dbc.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ struct Signal {
double factor, offset;
bool is_little_endian;
SignalType type;
unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const uint8_t *data, const size_t size);
};

struct Msg {
Expand Down Expand Up @@ -68,7 +68,7 @@ typedef struct ChecksumState {
int counter_start_bit;
bool little_endian;
SignalType checksum_type;
unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const std::vector<uint8_t> &d);
unsigned int (*calc_checksum)(uint32_t address, const Signal &sig, const uint8_t *data, const size_t size);
} ChecksumState;

DBC* dbc_parse(const std::string& dbc_path);
Expand Down
2 changes: 1 addition & 1 deletion can/packer.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ std::vector<uint8_t> CANPacker::pack(uint32_t address, const std::vector<SignalP
if (sig_it_checksum != signal_lookup.end()) {
const auto &sig = sig_it_checksum->second;
if (sig.calc_checksum != nullptr) {
unsigned int checksum = sig.calc_checksum(address, sig, ret);
unsigned int checksum = sig.calc_checksum(address, sig, ret.data(), ret.size());
set_value(ret, sig, checksum);
}
}
Expand Down
18 changes: 7 additions & 11 deletions can/parser.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,12 @@
#include "cereal/logger/logger.h"
#include "opendbc/can/common.h"

int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
int64_t get_raw_value(const uint8_t *msg, size_t msg_size, const Signal &sig) {
int64_t ret = 0;

int i = sig.msb / 8;
int bits = sig.size;
while (i >= 0 && i < msg.size() && bits > 0) {
while (i >= 0 && i < msg_size && bits > 0) {
int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i*8;
int msb = (int)(sig.msb / 8) == i ? sig.msb : (i+1)*8 - 1;
int size = msb - lsb + 1;
Expand All @@ -33,23 +33,23 @@ int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
}


bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
bool MessageState::parse(uint64_t nanos, const uint8_t *msg, size_t msg_size) {
std::vector<double> tmp_vals(parse_sigs.size());
bool checksum_failed = false;
bool counter_failed = false;

for (int i = 0; i < parse_sigs.size(); i++) {
const auto &sig = parse_sigs[i];

int64_t tmp = get_raw_value(dat, sig);
int64_t tmp = get_raw_value(msg, msg_size, sig);
if (sig.is_signed) {
tmp -= ((tmp >> (sig.size-1)) & 0x1) ? (1ULL << sig.size) : 0;
}

//DEBUG("parse 0x%X %s -> %ld\n", address, sig.name, tmp);

if (!ignore_checksum) {
if (sig.calc_checksum != nullptr && sig.calc_checksum(address, sig, dat) != tmp) {
if (sig.calc_checksum != nullptr && sig.calc_checksum(address, sig, msg, msg_size) != tmp) {
checksum_failed = true;
}
}
Expand Down Expand Up @@ -238,9 +238,7 @@ void CANParser::UpdateCans(uint64_t nanos, const capnp::List<cereal::CanData>::R
// continue;
//}

std::vector<uint8_t> data(dat.size(), 0);
memcpy(data.data(), dat.begin(), dat.size());
state_it->second.parse(nanos, data);
state_it->second.parse(nanos, dat.begin(), dat.size());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we just pass the capnp::Data::Reader, or do we want to remove capnp as a dependency too?

}

// update bus timeout
Expand Down Expand Up @@ -268,9 +266,7 @@ void CANParser::UpdateCans(uint64_t nanos, const capnp::DynamicStruct::Reader& c

auto dat = cmsg.get("dat").as<capnp::Data>();
if (dat.size() > 64) return; // shouldn't ever happen
std::vector<uint8_t> data(dat.size(), 0);
memcpy(data.data(), dat.begin(), dat.size());
state_it->second.parse(nanos, data);
state_it->second.parse(nanos, dat.begin(), dat.size());
}

void CANParser::UpdateValid(uint64_t nanos) {
Expand Down