From 97bfe99980a3ef58e07eb8006ac5e59bf83ad986 Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Tue, 16 Mar 2021 00:27:20 +0800
Subject: [PATCH] Added new util class AlignedBuffer (#125)

* AlignedBuffer

* rename function aligned to get

* add overloaded function for convenience
---
 messaging/messaging.hpp   | 19 +++++++++++++++++++
 messaging/socketmaster.cc | 15 ++++-----------
 2 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/messaging/messaging.hpp b/messaging/messaging.hpp
index 6531de6..5490a63 100644
--- a/messaging/messaging.hpp
+++ b/messaging/messaging.hpp
@@ -120,3 +120,22 @@ public:
 private:
   std::map<std::string, PubSocket *> sockets_;
 };
+
+class AlignedBuffer {
+public:
+  AlignedBuffer() = default;
+  AlignedBuffer(const char *data, const size_t size) { get(data, size); }
+  kj::ArrayPtr<const capnp::word> get(const char *data, const size_t size) {
+    words_size = size / sizeof(capnp::word) + 1;
+    if (aligned_buf.size() < words_size) {
+      aligned_buf = kj::heapArray<capnp::word>(words_size < 512 ? 512 : words_size);
+    }
+    memcpy(aligned_buf.begin(), data, size);
+    return aligned_buf.slice(0, words_size);
+  }
+  inline kj::ArrayPtr<const capnp::word> get(Message *m) { return get(m->getData(), m->getSize()); }
+  inline operator kj::ArrayPtr<const capnp::word>() { return aligned_buf.slice(0, words_size); }
+private:
+  kj::Array<capnp::word> aligned_buf;
+  size_t words_size;
+};
diff --git a/messaging/socketmaster.cc b/messaging/socketmaster.cc
index 53821aa..b53f7e5 100644
--- a/messaging/socketmaster.cc
+++ b/messaging/socketmaster.cc
@@ -39,7 +39,7 @@ struct SubMaster::SubMessage {
   uint64_t rcv_time = 0, rcv_frame = 0;
   void *allocated_msg_reader = nullptr;
   capnp::FlatArrayMessageReader *msg_reader = nullptr;
-  kj::Array<capnp::word> buf;
+  AlignedBuffer aligned_buf;
   cereal::Event::Reader event;
 };
 
@@ -56,8 +56,7 @@ SubMaster::SubMaster(const std::initializer_list<const char *> &service_list, co
       .socket = socket,
       .freq = serv->frequency,
       .ignore_alive = inList(ignore_alive, name),
-      .allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)),
-      .buf = kj::heapArray<capnp::word>(1024)};
+      .allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader))};
     messages_[socket] = m;
     services_[name] = m;
   }
@@ -75,17 +74,11 @@ int SubMaster::update(int timeout) {
     if (msg == nullptr) continue;
 
     SubMessage *m = messages_.at(s);
-    const size_t size = (msg->getSize() / sizeof(capnp::word)) + 1;
-    if (m->buf.size() < size) {
-      m->buf = kj::heapArray<capnp::word>(size);
-    }
-    memcpy(m->buf.begin(), msg->getData(), msg->getSize());
-    delete msg;
-
     if (m->msg_reader) {
       m->msg_reader->~FlatArrayMessageReader();
     }
-    m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(kj::ArrayPtr<capnp::word>(m->buf.begin(), size));
+    m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.get(msg));
+    delete msg;
     m->event = m->msg_reader->getRoot<cereal::Event>();
     m->updated = true;
     m->rcv_time = current_time;