mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-20 21:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

No more magic for the can chunks (#26861)

Browse Source 
* remove magic and add checksum

* add comms reset

* bump submodule
This commit is contained in:
Robbe Derks
2023-01-13 15:01:55 -08:00
committed by GitHub
parent 23bd5d6f5d
commit 3136985b95
4 changed files with 80 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda
+1 -1
Submodule panda updated: 0b3b906036...11d90f9e78
selfdrive/boardd/panda.cc
+45 -26
View File
@@ -26,6 +26,8 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
(hw_type == cereal::PandaState::PandaType::DOS) ||
(hw_type == cereal::PandaState::PandaType::TRES);
can_reset_communications();
return;
}
@@ -174,10 +176,6 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
int32_t pos = 0;
uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
uint32_t magic = CAN_TRANSACTION_MAGIC;
memcpy(&send_buf[0], &magic, sizeof(uint32_t));
pos += sizeof(uint32_t);
for (auto cmsg : can_data_list) {
// check if the message is intended for this panda
uint8_t bus = cmsg.getSrc();
@@ -194,20 +192,25 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
header.data_len_code = data_len_code;
header.bus = bus - bus_offset;
header.checksum = 0;
memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header));
pos += sizeof(can_header);
memcpy(&send_buf[pos], (uint8_t *)can_data.begin(), can_data.size());
pos += can_data.size();
memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size());
uint32_t msg_size = sizeof(can_header) + can_data.size();
// set checksum
((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size);
pos += msg_size;
if (pos >= USB_TX_SOFT_LIMIT) {
write_func(send_buf, pos);
pos = sizeof(uint32_t);
pos = 0;
}
}
// send remaining packets
if (pos > sizeof(uint32_t)) write_func(send_buf, pos);
if (pos > 0) write_func(send_buf, pos);
}
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
@@ -217,36 +220,35 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
}
bool Panda::can_receive(std::vector<can_frame>& out_vec) {
uint8_t data[RECV_SIZE];
int recv = handle->bulk_read(0x81, (uint8_t*)data, RECV_SIZE);
int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE);
if (!comms_healthy()) {
return false;
}
if (recv == RECV_SIZE) {
LOGW("Panda receive buffer full");
}
receive_buffer_size += recv;
return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec);
return (recv <= 0) ? true : unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec);
}
bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec) {
if (size < sizeof(uint32_t)) {
return true;
}
void Panda::can_reset_communications() {
handle->control_write(0xc0, 0, 0);
}
uint32_t magic;
memcpy(&magic, &data[0], sizeof(uint32_t));
if (magic != CAN_TRANSACTION_MAGIC) {
LOGE("CAN recv: buffer didn't start with magic");
handle->comms_healthy = false;
return false;
}
bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec) {
int pos = 0;
int pos = sizeof(uint32_t);
while (pos < size) {
while (pos <= size - sizeof(can_header)) {
can_header header;
memcpy(&header, &data[pos], sizeof(can_header));
const uint8_t data_len = dlc_to_len[header.data_len_code];
if (pos + sizeof(can_header) + data_len > size) {
// we don't have all the data for this message yet
break;
}
can_frame &canData = out_vec.emplace_back();
canData.busTime = 0;
canData.address = header.addr;
@@ -258,10 +260,27 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &o
canData.src += CAN_RETURNED_BUS_OFFSET;
}
const uint8_t data_len = dlc_to_len[header.data_len_code];
if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) {
LOGE("Panda CAN checksum failed");
return false;
}
canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len);
pos += sizeof(can_header) + data_len;
}
// move the overflowing data to the beginning of the buffer for the next round
memmove(data, &data[pos], size - pos);
size -= pos;
return true;
}
uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) {
uint8_t checksum = 0U;
for (uint32_t i = 0U; i < len; i++) {
checksum ^= data[i];
}
return checksum;
}
selfdrive/boardd/panda.h
+7 -1
View File
@@ -30,6 +30,7 @@ struct __attribute__((packed)) can_header {
uint8_t returned : 1;
uint8_t extended : 1;
uint32_t addr : 29;
uint8_t checksum : 8;
};
struct can_frame {
@@ -80,11 +81,16 @@ public:
void set_canfd_non_iso(uint16_t bus, bool non_iso);
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
bool can_receive(std::vector<can_frame>& out_vec);
void can_reset_communications();
protected:
// for unit tests
uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64];
uint32_t receive_buffer_size = 0;
Panda(uint32_t bus_offset) : bus_offset(bus_offset) {}
void pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
std::function<void(uint8_t *, size_t)> write_func);
bool unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec);
bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
uint8_t calculate_checksum(uint8_t *data, uint32_t len);
};
selfdrive/boardd/tests/test_boardd_usbprotocol.cc
+27 -10
View File
@@ -16,7 +16,8 @@ int random_int(int min, int max) {
struct PandaTest : public Panda {
PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type);
void test_can_send();
void test_can_recv();
void test_can_recv(uint32_t chunk_size = 0);
void test_chunked_can_recv();
std::map<int, std::string> test_data;
int can_list_size = 0;
@@ -58,11 +59,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState
void PandaTest::test_can_send() {
std::vector<uint8_t> unpacked_data;
this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
uint32_t magic;
memcpy(&magic, &chunk[0], sizeof(uint32_t));
REQUIRE(magic == CAN_TRANSACTION_MAGIC);
unpacked_data.insert(unpacked_data.end(), &chunk[sizeof(uint32_t)], &chunk[size]);
unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]);
});
REQUIRE(unpacked_data.size() == total_pakets_size);
@@ -77,16 +74,30 @@ void PandaTest::test_can_send() {
REQUIRE(header.addr == cnt);
REQUIRE(test_data.find(data_len) != test_data.end());
const std::string &dat = test_data[data_len];
REQUIRE(memcmp(dat.data(), &unpacked_data[pos + 5], dat.size()) == 0);
REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0);
++cnt;
}
REQUIRE(cnt == can_list_size);
}
void PandaTest::test_can_recv() {
void PandaTest::test_can_recv(uint32_t rx_chunk_size) {
std::vector<can_frame> frames;
this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) {
this->unpack_can_buffer(data, size, frames);
this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) {
if (rx_chunk_size == 0) {
REQUIRE(this->unpack_can_buffer(data, size, frames));
} else {
this->receive_buffer_size = 0;
uint32_t pos = 0;
while(pos < size) {
uint32_t chunk_size = std::min(rx_chunk_size, size - pos);
memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size);
this->receive_buffer_size += chunk_size;
pos += chunk_size;
REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames));
}
}
});
REQUIRE(frames.size() == can_list_size);
@@ -109,6 +120,9 @@ TEST_CASE("send/recv CAN 2.0 packets") {
SECTION("can_receive") {
test.test_can_recv();
}
SECTION("chunked_can_receive") {
test.test_can_recv(0x40);
}
}
TEST_CASE("send/recv CAN FD packets") {
@@ -122,4 +136,7 @@ TEST_CASE("send/recv CAN FD packets") {
SECTION("can_receive") {
test.test_can_recv();
}
SECTION("chunked_can_receive") {
test.test_can_recv(0x40);
}
}