Break out Package read/write

1 files changed, 55 insertions, 0 deletions

diff --git a/src/package.cc b/src/package.cc
new file mode 100644
index 0000000..8a7ef24
--- /dev/null
+++ b/src/package.cc
@@ -0,0 +1,55 @@
+// -*- mode: c++; c-basic-offset: 2; -*-
+
+#include "common.hh"
+
+#include <cstring>
+
+#include "data.hh"
+#include "package.hh"
+
+size_t read_package(Package* pkg, uint8_t const* data, size_t max) {
+  if (max < 26) return 0;
+  size_t offset = 0;
+  pkg->id = read_u32(data + offset);
+  offset += 4;
+  pkg->timestamp.tv_sec = read_u64(data + offset);
+  offset += 8;
+  pkg->timestamp.tv_nsec = read_u32(data + offset);
+  offset += 4;
+  pkg->flags = read_u16(data + offset);
+  offset += 2;
+  pkg->source_port = read_u16(data + offset);
+  offset += 2;
+  pkg->target_port = read_u16(data + offset);
+  offset += 2;
+  auto len = read_u16(data + offset);
+  offset += 2;
+  if (offset + len + 2 > max) return 0;
+  pkg->source_host.assign(reinterpret_cast<char const*>(data) + offset, len);
+  offset += len;
+  len = read_u16(data + offset);
+  offset += 2;
+  if (offset + len > max) return 0;
+  pkg->target_host.assign(reinterpret_cast<char const*>(data) + offset, len);
+  offset += len;
+  return offset;
+}
+
+size_t write_package(Package const& pkg, uint8_t* data, size_t max) {
+  auto len = 26 + pkg.source_host.size() + pkg.target_host.size();
+  if (!data || max < len) {
+    return len;
+  }
+  write_u32(data, pkg.id);
+  write_u64(data + 4, pkg.timestamp.tv_sec);
+  write_u32(data + 12, pkg.timestamp.tv_nsec);
+  write_u16(data + 16, pkg.flags);
+  write_u16(data + 18, pkg.source_port);
+  write_u16(data + 20, pkg.target_port);
+  write_u16(data + 22, pkg.source_host.size());
+  memcpy(data + 24, pkg.source_host.data(), pkg.source_host.size());
+  write_u16(data + 24 + pkg.source_host.size(), pkg.target_host.size());
+  memcpy(data + 26 + pkg.source_host.size(),
+         pkg.target_host.data(), pkg.target_host.size());
+  return len;
+}