AlignedBuffer: avoid unnecessary data copying when already aligned

cereal/messaging/messaging.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <algorithm>
 #include <cstddef>
 #include <map>
 #include <string>
@@ -86,17 +87,25 @@ class PubMaster {
 class AlignedBuffer {
 public:
   kj::ArrayPtr<const capnp::word> align(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);
+    const size_t word_count = size / sizeof(capnp::word);
+
+    // Check if data is already aligned
+    if (reinterpret_cast<uintptr_t>(data) % alignof(capnp::word) == 0) {
+      return kj::arrayPtr(reinterpret_cast<const capnp::word *>(data), word_count);
+    }
+
+    // Data is not aligned, perform alignment
+    if (aligned_buf.size() < word_count) {
+      aligned_buf = kj::heapArray<capnp::word>(std::max(word_count, size_t(512)));
     }
-    memcpy(aligned_buf.begin(), data, size);
-    return aligned_buf.slice(0, words_size);
+    memcpy(aligned_buf.begin(), data, word_count * sizeof(capnp::word));
+    return aligned_buf.slice(0, word_count);
   }
+
   inline kj::ArrayPtr<const capnp::word> align(Message *m) {
     return align(m->getData(), m->getSize());
   }
+
 private:
   kj::Array<capnp::word> aligned_buf;
-  size_t words_size;
 };

cereal/messaging/socketmaster.cc

@@ -1,6 +1,7 @@
 #include <assert.h>
 #include <stdlib.h>
 #include <string>
+#include <memory>
 #include <mutex>
 
 #include "cereal/services.h"
@@ -40,6 +41,7 @@ struct SubMaster::SubMessage {
   bool is_polled = false;
   capnp::FlatArrayMessageReader *msg_reader = nullptr;
   AlignedBuffer aligned_buf;
+  std::unique_ptr<Message> message;
   cereal::Event::Reader event;
 };
 
@@ -92,8 +94,10 @@ void SubMaster::update(int timeout) {
     m->msg_reader->~FlatArrayMessageReader();
     capnp::ReaderOptions options;
     options.traversalLimitInWords = kj::maxValue; // Don't limit
+
+    // Reset the message buffer to the new one. Keep it for the message reader's lifetime.
+    m->message.reset(msg);
     m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg), options);
-    delete msg;
     messages.push_back({m->name, m->msg_reader->getRoot<cereal::Event>()});
   }