From 7ef09a45e422392a2b4db7e5a1e8fdf7ab5a9cea Mon Sep 17 00:00:00 2001
From: deanlee <deanlee3@gmail.com>
Date: Mon, 25 Mar 2024 16:09:20 +0800
Subject: [PATCH] parse can strings in a c++ helper function

---
 can/SConscript            |  3 ++-
 can/parser_pyx.pyx        | 32 ++++++++------------------------
 can/strings_to_candata.cc | 37 +++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+), 25 deletions(-)
 create mode 100644 can/strings_to_candata.cc

diff --git a/can/SConscript b/can/SConscript
index 3923bf3035..1a1f79cd4c 100644
--- a/can/SConscript
+++ b/can/SConscript
@@ -16,8 +16,9 @@ envDBC.Library('libdbc_static', src, LIBS=libs)
 
 # Build packer and parser
 lenv = envCython.Clone()
+lenv.Library('strings_to_candata', ['strings_to_candata.cc'])
 lenv["LINKFLAGS"] += [libdbc[0].get_labspath()]
-parser = lenv.Program('parser_pyx.so', 'parser_pyx.pyx')
+parser = lenv.Program('parser_pyx.so', 'parser_pyx.pyx', LIBS=['strings_to_candata', 'capnp', 'kj'] + lenv["LIBS"])
 packer = lenv.Program('packer_pyx.so', 'packer_pyx.pyx')
 
 lenv.Depends(parser, libdbc)
diff --git a/can/parser_pyx.pyx b/can/parser_pyx.pyx
index 7d0869355b..cd1c1216f1 100644
--- a/can/parser_pyx.pyx
+++ b/can/parser_pyx.pyx
@@ -5,17 +5,18 @@ from cython.operator cimport dereference as deref, preincrement as preinc
 from libcpp.pair cimport pair
 from libcpp.string cimport string
 from libcpp.vector cimport vector
+from libcpp cimport bool
 from libcpp.unordered_set cimport unordered_set
 from libc.stdint cimport uint32_t
 
 from .common cimport CANParser as cpp_CANParser
-from .common cimport dbc_lookup, SignalValue, DBC, CanData, CanFrame
+from .common cimport dbc_lookup, SignalValue, DBC, CanData
 
 import numbers
-import capnp
-from cereal import log
 from collections import defaultdict
 
+cdef extern from "strings_to_candata.cc":
+  void strings_to_candata(const vector[string] &strings, vector[CanData] &candata, bool sendcan) except +
 
 cdef class CANParser:
   cdef:
@@ -77,30 +78,13 @@ cdef class CANParser:
       for l in v.values():  # no-cython-lint
         l.clear()
 
-    cdef CanFrame* frame
-    cdef CanData* can_data
-    cdef vector[CanData] can_data_array
-
-    can_data_array.reserve(len(strings))
-    try:
-      for s in strings:
-        with log.Event.from_bytes(s) as msg:
-          can_event = msg.sendCan if sendcan else msg.can
-          can_data = &(can_data_array.emplace_back())
-          can_data.nanos = msg.logMonoTime
-          can_data.frames.reserve(len(can_event))
-          for c in can_event:
-            frame = &(can_data.frames.emplace_back())
-            frame.src = c.src
-            frame.address = c.address
-            frame.dat = c.dat
-    except capnp.lib.capnp.KjException as ex:
-      raise RuntimeError(str(ex))
-
     cdef vector[SignalValue] new_vals
     cdef unordered_set[uint32_t] updated_addrs
+    cdef vector[CanData] can_data
+
+    strings_to_candata(strings, can_data, sendcan)
+    self.can.update(can_data, new_vals)
 
-    self.can.update(can_data_array, new_vals)
     cdef vector[SignalValue].iterator it = new_vals.begin()
     cdef SignalValue* cv
     while it != new_vals.end():
diff --git a/can/strings_to_candata.cc b/can/strings_to_candata.cc
new file mode 100644
index 0000000000..46ce918602
--- /dev/null
+++ b/can/strings_to_candata.cc
@@ -0,0 +1,37 @@
+#include <capnp/serialize.h>
+
+#include <string>
+#include <vector>
+
+#include "cereal/gen/cpp/log.capnp.h"
+#include "opendbc/can/common.h"
+
+void strings_to_candata(const std::vector<std::string> &strings, std::vector<CanData> &can_data, bool sendcan) {
+  kj::Array<capnp::word> aligned_buf;
+  can_data.reserve(strings.size());
+  for (const auto &s : strings) {
+    const size_t buf_size = (s.length() / sizeof(capnp::word)) + 1;
+    if (aligned_buf.size() < buf_size) {
+      aligned_buf = kj::heapArray<capnp::word>(buf_size);
+    }
+    memcpy(aligned_buf.begin(), s.data(), s.length());
+
+    // extract the messages
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.slice(0, buf_size));
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    auto &can = can_data.emplace_back();
+    can.nanos = event.getLogMonoTime();
+
+    auto cans = sendcan ? event.getSendcan() : event.getCan();
+    can.frames.reserve(cans.size());
+    for (const auto cmsg : cans) {
+      auto &frame = can.frames.emplace_back();
+      frame.src = cmsg.getSrc();
+      frame.address = cmsg.getAddress();
+      auto dat = cmsg.getDat();
+      frame.dat.resize(dat.size());
+      memcpy(frame.dat.data(), dat.begin(), dat.size());
+    }
+  }
+}