From 7aa379e123aba70a4c36f95e69786a558892dc2d Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Tue, 16 Mar 2021 18:13:15 +0800
Subject: [PATCH] use class AlignedBuffer from cereal (#20343)

* AlignedBuffer

* move AlignedBuffer to cereal/messaging.hpp

* rename function aligned to get

* use overloaded function AlignedBuffer:: get(Message *)
---
 selfdrive/boardd/boardd.cc         | 15 +++------------
 selfdrive/locationd/ubloxd_main.cc |  9 ++-------
 selfdrive/loggerd/loggerd.cc       | 12 ++----------
 3 files changed, 7 insertions(+), 29 deletions(-)

diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 5060d81baf..0d356bbf92 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -99,11 +99,7 @@ void safety_setter_thread() {
   }
   LOGW("got %d bytes CarParams", params.size());
 
-  // format for board, make copy due to alignment issues, will be freed on out of scope
-  auto amsg = kj::heapArray<capnp::word>((params.size() / sizeof(capnp::word)) + 1);
-  memcpy(amsg.begin(), params.data(), params.size());
-
-  capnp::FlatArrayMessageReader cmsg(amsg);
+  capnp::FlatArrayMessageReader cmsg(AlignedBuffer(params.data(), params.size()));
   cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
   cereal::CarParams::SafetyModel safety_model = car_params.getSafetyModel();
 
@@ -201,7 +197,7 @@ void can_recv(PubMaster &pm) {
 void can_send_thread(bool fake_send) {
   LOGD("start send thread");
 
-  kj::Array<capnp::word> buf = kj::heapArray<capnp::word>(1024);
+  AlignedBuffer aligned_buf;
   Context * context = Context::create();
   SubSocket * subscriber = SubSocket::create(context, "sendcan");
   assert(subscriber != NULL);
@@ -217,13 +213,8 @@ void can_send_thread(bool fake_send) {
       }
       continue;
     }
-    const size_t size = (msg->getSize() / sizeof(capnp::word)) + 1;
-    if (buf.size() < size) {
-      buf = kj::heapArray<capnp::word>(size);
-    }
-    memcpy(buf.begin(), msg->getData(), msg->getSize());
 
-    capnp::FlatArrayMessageReader cmsg(buf.slice(0, size));
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.get(msg));
     cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
 
     //Dont send if older than 1 second
diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc
index 3ebc437a1d..310648efc9 100644
--- a/selfdrive/locationd/ubloxd_main.cc
+++ b/selfdrive/locationd/ubloxd_main.cc
@@ -23,7 +23,7 @@ ExitHandler do_exit;
 using namespace ublox;
 int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
   LOGW("starting ubloxd");
-  kj::Array<capnp::word> buf = kj::heapArray<capnp::word>(1024);
+  AlignedBuffer aligned_buf;
   UbloxMsgParser parser;
 
   Context * context = Context::create();
@@ -41,13 +41,8 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
       }
       continue;
     }
-    const size_t size = (msg->getSize() / sizeof(capnp::word)) + 1;
-    if (buf.size() < size) {
-      buf = kj::heapArray<capnp::word>(size);
-    }
-    memcpy(buf.begin(), msg->getData(), msg->getSize());
 
-    capnp::FlatArrayMessageReader cmsg(buf.slice(0, size));
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.get(msg));
     cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
     auto ubloxRaw = event.getUbloxRaw();
 
diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc
index cad256e9fc..a2758de4f3 100644
--- a/selfdrive/loggerd/loggerd.cc
+++ b/selfdrive/loggerd/loggerd.cc
@@ -383,7 +383,7 @@ int main(int argc, char** argv) {
 
   uint64_t msg_count = 0;
   uint64_t bytes_count = 0;
-  kj::Array<capnp::word> buf = kj::heapArray<capnp::word>(1024);
+  AlignedBuffer aligned_buf;
 
   double start_ts = seconds_since_boot();
   double last_rotate_tms = millis_since_boot();
@@ -427,15 +427,7 @@ int main(int argc, char** argv) {
         if (fpkt_id >= 0) {
           // track camera frames to sync to encoder
           // only process last frame
-          const uint8_t* data = (uint8_t*)last_msg->getData();
-          const size_t len = last_msg->getSize();
-          const size_t size = len / sizeof(capnp::word) + 1;
-          if (buf.size() < size) {
-            buf = kj::heapArray<capnp::word>(size);
-          }
-          memcpy(buf.begin(), data, len);
-
-          capnp::FlatArrayMessageReader cmsg(buf);
+          capnp::FlatArrayMessageReader cmsg(aligned_buf.get(last_msg));
           cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
 
           if (fpkt_id == LOG_CAMERA_ID_FCAMERA) {