WIP working buffer copy with simple locking

commons.hpp

@@ -0,0 +1,122 @@
+#pragma once
+
+#include <array>
+#include <span>
+#include <optional>
+
+#include "logging.hpp"
+
+namespace commons {
+
+template<typename LockType>
+class lock
+{
+public:
+  ~lock() { locker.unlock(); }
+
+  static std::optional<lock> get(LockType& l)
+  {
+    if(!l.lock())
+    {
+      log::debug("Could not acquire lock.");
+      return std::nullopt;
+    }
+    return lock<LockType>{l};
+  }
+
+private:
+  explicit lock(LockType& l) : locker{l} {}
+  LockType& locker;
+};
+
+class mutex
+{
+public:
+  bool lock()
+  {
+    if(locked)
+    {
+      return false;
+    }
+
+    locked = true;
+    return true;
+  }
+
+  void unlock()
+  {
+    locked = false;
+  }
+
+  bool is_locked() const {
+    return locked;
+  }
+
+protected:
+  bool locked = false;
+};
+
+}
+
+template <typename T, size_t S>
+struct buffer {
+  constexpr buffer() {}
+  constexpr auto size() { return S; }
+
+  bool copy_from(const std::span<T>& input, uint16_t amount)
+  {
+    const auto lk = commons::lock<commons::mutex>::get(m);
+    if(!lk)
+    {
+      return false;
+    }
+
+    if(amount > max_size)
+    {
+      return false;
+    }
+
+    old_pos = new_pos;
+
+    if (old_pos + amount > max_size) {
+      const auto size_to_copy = max_size - old_pos;
+      std::copy(input.begin(),
+                std::next(input.begin(), size_to_copy),
+                std::next(buf.begin(), old_pos));
+      old_pos = 0;
+
+      //TODO: this only works if amount is not double the size of main_buf
+      std::copy(std::next(input.begin(), size_to_copy),
+                input.begin() + amount, buf.begin());
+      new_pos = amount - size_to_copy;
+    } else {
+      std::copy(input.begin(),
+                input.begin() + amount,
+                std::next(buf.begin(), old_pos));
+      new_pos = old_pos + amount;
+    }
+
+    return true;
+  }
+
+  bool print() const
+  {
+    const auto lk = commons::lock<commons::mutex>::get(m);
+    if(!lk)
+    {
+      return false;
+    }
+
+    uart_interface::write(
+        {reinterpret_cast<const char *>(buf.data()), new_pos});
+    return true;
+  }
+
+private:
+  static constexpr auto max_size = S;
+  std::array<T, S> buf{};
+  size_t old_pos = 0;
+  size_t new_pos = 0;
+  mutable commons::mutex m;
+};
+