From 433add3a02f81f9cc44c8ef44126697b8190b29d Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Thu, 18 Nov 2021 09:54:14 +0800
Subject: [PATCH] boardd/can_recv_thread: improve caching for can_frame
 (#22944)

* better cache

* use emplace to remove copy&realloc

* rebase master
old-commit-hash: 841fb93def3b52cc597d6e60a51f676002e6f3ac
---
 selfdrive/boardd/boardd.cc         | 3 ++-
 selfdrive/boardd/panda.cc          | 8 ++------
 selfdrive/boardd/tests/test_boardd | 3 +++
 3 files changed, 7 insertions(+), 7 deletions(-)
 create mode 100755 selfdrive/boardd/tests/test_boardd

diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 50749a1061..d15dc1bbe9 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -237,6 +237,7 @@ void can_recv_thread(std::vector<Panda *> pandas) {
   // run at 100hz
   const uint64_t dt = 10000000ULL;
   uint64_t next_frame_time = nanos_since_boot() + dt;
+  std::vector<can_frame> raw_can_data;
 
   while (!do_exit) {
     if (!check_all_connected(pandas)){
@@ -244,8 +245,8 @@ void can_recv_thread(std::vector<Panda *> pandas) {
       break;
     }
 
-    std::vector<can_frame> raw_can_data;
     bool comms_healthy = true;
+    raw_can_data.clear();
     for (const auto& panda : pandas) {
       comms_healthy &= panda->can_receive(raw_can_data);
     }
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index f101b2e3b2..01c9f9fa8f 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -430,8 +430,6 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
     return false;
   }
 
-  out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE));
-
   static uint8_t tail[CANPACKET_MAX_SIZE];
   uint8_t tail_size = 0;
   uint8_t counter = 0;
@@ -453,9 +451,10 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
       uint8_t data_len = dlc_to_len[(chunk[pos] >> 4)];
       uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len;
       if (pckt_len <= (chunk_len - pos)) {
-        can_frame canData;
         can_header header;
         memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE);
+
+        can_frame &canData = out_vec.emplace_back();
         canData.busTime = 0;
         canData.address = header.addr;
         canData.src = header.bus + bus_offset;
@@ -465,9 +464,6 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
         canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len);
 
         pos += pckt_len;
-
-        // add to vector
-        out_vec.push_back(canData);
       } else {
         // Keep partial CAN packet until next USB packet
         tail_size = (chunk_len - pos);
diff --git a/selfdrive/boardd/tests/test_boardd b/selfdrive/boardd/tests/test_boardd
new file mode 100755
index 0000000000..105bd5a96c
--- /dev/null
+++ b/selfdrive/boardd/tests/test_boardd
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b5c0b301b3fc7c0f4718409ed68033eb870f127489e9d834aa9dbd23ed7eb91b
+size 13919544