RPC calls

components/constants/include/constants/app_comms.h

@@ -5,6 +5,12 @@
 #ifndef APP_COMMS_H
 #define APP_COMMS_H
 
+#define RPC_CALL_QUEUE_SIZE 10
+#define RPC_CALL_ENQUEUE_TIMEOUT_MS 250
+#define RPC_CALL_DEQUEUE_TIMEOUT_MS 3000
+#define RPC_CALL_THREAD_POOL_SIZE 3
+#define RPC_CALL_PARAMETERS_MAX_SIZE 512
+
 #define RX_QUEUE_SIZE 4
 #define MAX_RX_BUFFER_SIZE 512
 

components/flatbuffers/CMakeLists.txt

@@ -1,2 +1,2 @@
-idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" "SensorMessageBuilder.cpp" "TextControlMessageBuilder.cpp"
+idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" "SensorMessageBuilder.cpp" "TextControlMessageBuilder.cpp" "CallBuilder.cpp"
         INCLUDE_DIRS "include")

components/flatbuffers/CallBuilder.cpp

@@ -0,0 +1,23 @@
+#include "CallBuilder.h"
+#include "SerializedMessage.h"
+
+namespace Flatbuffers {
+SerializedMessage CallBuilder::build_return_call(uint8_t unique_id, const std::vector<uint8_t> &return_value) {
+  builder_.Clear();
+
+  const auto return_vector = builder_.CreateVector(return_value);
+
+  const auto message = Messaging::CreateReturnCall(
+      builder_, unique_id, static_cast<int>(return_value.size()),
+      return_vector);
+
+  builder_.Finish(message);
+
+  return {builder_.GetBufferPointer(), builder_.GetSize()};
+}
+
+const Messaging::SendCall *CallBuilder::parse_send_call(const uint8_t *buffer) {
+  return flatbuffers::GetRoot<Messaging::SendCall>(buffer);
+}
+
+} // namespace Flatbuffers

components/flatbuffers/include/CallBuilder.h

@@ -0,0 +1,27 @@
+
+#ifndef CALLBUILDER_H
+#define CALLBUILDER_H
+
+#include <vector>
+
+#include "SerializedMessage.h"
+#include "flatbuffers/flatbuffers.h"
+#include "flatbuffers_generated/ReturnCall_generated.h"
+#include "flatbuffers_generated/SendCall_generated.h"
+
+namespace Flatbuffers {
+
+class CallBuilder {
+public:
+  CallBuilder() : builder_(1024) {}
+
+  SerializedMessage build_return_call(uint8_t unique_id, const std::vector<uint8_t> &return_value);
+
+  static const Messaging::SendCall *parse_send_call(const uint8_t *buffer);
+
+private:
+  flatbuffers::FlatBufferBuilder builder_;
+};
+} // namespace Flatbuffers
+
+#endif // CALLBUILDER_H

components/flatbuffers/include/flatbuffers_generated/ReturnCall_generated.h

@@ -0,0 +1,117 @@
+// automatically generated by the FlatBuffers compiler, do not modify
+
+#ifndef FLATBUFFERS_GENERATED_RETURNCALL_MESSAGING_H_
+#define FLATBUFFERS_GENERATED_RETURNCALL_MESSAGING_H_
+
+#include "flatbuffers/flatbuffers.h"
+
+// Ensure the included flatbuffers.h is the same version as when this file was
+// generated, otherwise it may not be compatible.
+// static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
+//               FLATBUFFERS_VERSION_MINOR == 2 &&
+//               FLATBUFFERS_VERSION_REVISION == 10,
+//              "Non-compatible flatbuffers version included");
+
+namespace Messaging {
+
+struct ReturnCall;
+struct ReturnCallBuilder;
+
+struct ReturnCall FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef ReturnCallBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_UNIQUE_ID = 4,
+    VT_LENGTH = 6,
+    VT_RETURN_VALUE = 8
+  };
+  uint8_t unique_id() const { return GetField<uint8_t>(VT_UNIQUE_ID, 0); }
+  uint16_t length() const { return GetField<uint16_t>(VT_LENGTH, 0); }
+  const ::flatbuffers::Vector<uint8_t> *return_value() const {
+    return GetPointer<const ::flatbuffers::Vector<uint8_t> *>(VT_RETURN_VALUE);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<uint8_t>(verifier, VT_UNIQUE_ID, 1) &&
+           VerifyField<uint16_t>(verifier, VT_LENGTH, 2) &&
+           VerifyOffset(verifier, VT_RETURN_VALUE) &&
+           verifier.VerifyVector(return_value()) && verifier.EndTable();
+  }
+};
+
+struct ReturnCallBuilder {
+  typedef ReturnCall Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_unique_id(uint8_t unique_id) {
+    fbb_.AddElement<uint8_t>(ReturnCall::VT_UNIQUE_ID, unique_id, 0);
+  }
+  void add_length(uint16_t length) {
+    fbb_.AddElement<uint16_t>(ReturnCall::VT_LENGTH, length, 0);
+  }
+  void add_return_value(
+      ::flatbuffers::Offset<::flatbuffers::Vector<uint8_t>> return_value) {
+    fbb_.AddOffset(ReturnCall::VT_RETURN_VALUE, return_value);
+  }
+  explicit ReturnCallBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+      : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<ReturnCall> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<ReturnCall>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<ReturnCall> CreateReturnCall(
+    ::flatbuffers::FlatBufferBuilder &_fbb, uint8_t unique_id = 0,
+    uint16_t length = 0,
+    ::flatbuffers::Offset<::flatbuffers::Vector<uint8_t>> return_value = 0) {
+  ReturnCallBuilder builder_(_fbb);
+  builder_.add_return_value(return_value);
+  builder_.add_length(length);
+  builder_.add_unique_id(unique_id);
+  return builder_.Finish();
+}
+
+inline ::flatbuffers::Offset<ReturnCall>
+CreateReturnCallDirect(::flatbuffers::FlatBufferBuilder &_fbb,
+                       uint8_t unique_id = 0, uint16_t length = 0,
+                       const std::vector<uint8_t> *return_value = nullptr) {
+  auto return_value__ =
+      return_value ? _fbb.CreateVector<uint8_t>(*return_value) : 0;
+  return Messaging::CreateReturnCall(_fbb, unique_id, length, return_value__);
+}
+
+inline const Messaging::ReturnCall *GetReturnCall(const void *buf) {
+  return ::flatbuffers::GetRoot<Messaging::ReturnCall>(buf);
+}
+
+inline const Messaging::ReturnCall *GetSizePrefixedReturnCall(const void *buf) {
+  return ::flatbuffers::GetSizePrefixedRoot<Messaging::ReturnCall>(buf);
+}
+
+inline bool VerifyReturnCallBuffer(::flatbuffers::Verifier &verifier) {
+  return verifier.VerifyBuffer<Messaging::ReturnCall>(nullptr);
+}
+
+inline bool
+VerifySizePrefixedReturnCallBuffer(::flatbuffers::Verifier &verifier) {
+  return verifier.VerifySizePrefixedBuffer<Messaging::ReturnCall>(nullptr);
+}
+
+inline void
+FinishReturnCallBuffer(::flatbuffers::FlatBufferBuilder &fbb,
+                       ::flatbuffers::Offset<Messaging::ReturnCall> root) {
+  fbb.Finish(root);
+}
+
+inline void FinishSizePrefixedReturnCallBuffer(
+    ::flatbuffers::FlatBufferBuilder &fbb,
+    ::flatbuffers::Offset<Messaging::ReturnCall> root) {
+  fbb.FinishSizePrefixed(root);
+}
+
+} // namespace Messaging
+
+#endif // FLATBUFFERS_GENERATED_RETURNCALL_MESSAGING_H_

components/flatbuffers/include/flatbuffers_generated/SendCall_generated.h

@@ -0,0 +1,123 @@
+// automatically generated by the FlatBuffers compiler, do not modify
+
+#ifndef FLATBUFFERS_GENERATED_SENDCALL_MESSAGING_H_
+#define FLATBUFFERS_GENERATED_SENDCALL_MESSAGING_H_
+
+#include "flatbuffers/flatbuffers.h"
+
+// Ensure the included flatbuffers.h is the same version as when this file was
+// generated, otherwise it may not be compatible.
+// static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
+//               FLATBUFFERS_VERSION_MINOR == 2 &&
+//               FLATBUFFERS_VERSION_REVISION == 10,
+//              "Non-compatible flatbuffers version included");
+
+namespace Messaging {
+
+struct SendCall;
+struct SendCallBuilder;
+
+struct SendCall FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef SendCallBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_TAG = 4,
+    VT_UNIQUE_ID = 6,
+    VT_LENGTH = 8,
+    VT_PARAMETERS = 10
+  };
+  uint8_t tag() const { return GetField<uint8_t>(VT_TAG, 0); }
+  uint8_t unique_id() const { return GetField<uint8_t>(VT_UNIQUE_ID, 0); }
+  uint16_t length() const { return GetField<uint16_t>(VT_LENGTH, 0); }
+  const ::flatbuffers::Vector<uint8_t> *parameters() const {
+    return GetPointer<const ::flatbuffers::Vector<uint8_t> *>(VT_PARAMETERS);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<uint8_t>(verifier, VT_TAG, 1) &&
+           VerifyField<uint8_t>(verifier, VT_UNIQUE_ID, 1) &&
+           VerifyField<uint16_t>(verifier, VT_LENGTH, 2) &&
+           VerifyOffset(verifier, VT_PARAMETERS) &&
+           verifier.VerifyVector(parameters()) && verifier.EndTable();
+  }
+};
+
+struct SendCallBuilder {
+  typedef SendCall Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_tag(uint8_t tag) {
+    fbb_.AddElement<uint8_t>(SendCall::VT_TAG, tag, 0);
+  }
+  void add_unique_id(uint8_t unique_id) {
+    fbb_.AddElement<uint8_t>(SendCall::VT_UNIQUE_ID, unique_id, 0);
+  }
+  void add_length(uint16_t length) {
+    fbb_.AddElement<uint16_t>(SendCall::VT_LENGTH, length, 0);
+  }
+  void add_parameters(
+      ::flatbuffers::Offset<::flatbuffers::Vector<uint8_t>> parameters) {
+    fbb_.AddOffset(SendCall::VT_PARAMETERS, parameters);
+  }
+  explicit SendCallBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+      : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<SendCall> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<SendCall>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<SendCall> CreateSendCall(
+    ::flatbuffers::FlatBufferBuilder &_fbb, uint8_t tag = 0,
+    uint8_t unique_id = 0, uint16_t length = 0,
+    ::flatbuffers::Offset<::flatbuffers::Vector<uint8_t>> parameters = 0) {
+  SendCallBuilder builder_(_fbb);
+  builder_.add_parameters(parameters);
+  builder_.add_length(length);
+  builder_.add_unique_id(unique_id);
+  builder_.add_tag(tag);
+  return builder_.Finish();
+}
+
+inline ::flatbuffers::Offset<SendCall>
+CreateSendCallDirect(::flatbuffers::FlatBufferBuilder &_fbb, uint8_t tag = 0,
+                     uint8_t unique_id = 0, uint16_t length = 0,
+                     const std::vector<uint8_t> *parameters = nullptr) {
+  auto parameters__ = parameters ? _fbb.CreateVector<uint8_t>(*parameters) : 0;
+  return Messaging::CreateSendCall(_fbb, tag, unique_id, length, parameters__);
+}
+
+inline const Messaging::SendCall *GetSendCall(const void *buf) {
+  return ::flatbuffers::GetRoot<Messaging::SendCall>(buf);
+}
+
+inline const Messaging::SendCall *GetSizePrefixedSendCall(const void *buf) {
+  return ::flatbuffers::GetSizePrefixedRoot<Messaging::SendCall>(buf);
+}
+
+inline bool VerifySendCallBuffer(::flatbuffers::Verifier &verifier) {
+  return verifier.VerifyBuffer<Messaging::SendCall>(nullptr);
+}
+
+inline bool
+VerifySizePrefixedSendCallBuffer(::flatbuffers::Verifier &verifier) {
+  return verifier.VerifySizePrefixedBuffer<Messaging::SendCall>(nullptr);
+}
+
+inline void
+FinishSendCallBuffer(::flatbuffers::FlatBufferBuilder &fbb,
+                     ::flatbuffers::Offset<Messaging::SendCall> root) {
+  fbb.Finish(root);
+}
+
+inline void FinishSizePrefixedSendCallBuffer(
+    ::flatbuffers::FlatBufferBuilder &fbb,
+    ::flatbuffers::Offset<Messaging::SendCall> root) {
+  fbb.FinishSizePrefixed(root);
+}
+
+} // namespace Messaging
+
+#endif // FLATBUFFERS_GENERATED_SENDCALL_MESSAGING_H_

components/rpc/CommunicationRouter.cpp

@@ -50,7 +50,7 @@ CommunicationRouter::~CommunicationRouter() {
     }
 }
 
-int CommunicationRouter::send_msg(char *buffer, const size_t length) const {
+int CommunicationRouter::send_msg(uint8_t *buffer, const size_t length) const {
     route(reinterpret_cast<uint8_t *>(buffer), length);
     return 0;
 }

components/rpc/MessagingInterface.cpp

@@ -2,67 +2,90 @@
 // Created by Johnathon Slightham on 2025-05-25.
 //
 
+#include <chrono>
+#include <cstddef>
 #include <ranges>
 
 #include "MessagingInterface.h"
 #include "AngleControlMessageBuilder.h"
+#include "CallBuilder.h"
 #include "ConfigManager.h"
+#include "constants/module.h"
+#include "flatbuffers_generated/SendCall_generated.h"
 #include "freertos/FreeRTOS.h"
+#include "freertos/idf_additions.h"
 #include "freertos/queue.h"
 #include "freertos/semphr.h"
 #include "MPIMessageBuilder.h"
 
-MessagingInterface::~MessagingInterface() {
-    vSemaphoreDelete(m_map_semaphore);
+#define TAG "MessagingInterface"
+#define MAX_RX_WAIT_TIME_MS 3000
+#define MAX_TX_WAIT_TIME_MS 200
+#define THREAD_POOL_SLEEP_AFTER_FAIL_MS 10
 
-    for (const auto queue: m_tag_to_queue | std::views::values) {
-        vQueueDelete(queue);
+bool Writer::write(std::vector<uint8_t>& return_value) {
+    if (m_called) { // Only allow returning once
+        return false;
     }
+    m_called = true;
+
+    auto [send_data, send_size] = m_builder.build_return_call(m_unique_id, return_value);
+    m_messaging_interface.send((uint8_t*)send_data, send_size, PC_ADDR, FN_CALL_TAG, true);
+
+    return true;
 }
 
-int MessagingInterface::send(char* buffer, const int size, const int destination, const int tag, const bool durable) {
+MessagingInterface::~MessagingInterface() {
+    vSemaphoreDelete(m_map_semaphore);
+}
+
+int MessagingInterface::send(uint8_t* buffer, const size_t size, const uint8_t destination, const uint8_t tag, const bool durable) {
     Flatbuffers::MPIMessageBuilder builder;
     const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, m_config_manager.get_module_id(), destination, m_sequence_number++, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
 
-    m_router->send_msg(static_cast<char *>(mpi_buffer), mpi_size);
+    m_router->send_msg((uint8_t *)mpi_buffer, mpi_size);
     return 0;
 }
 
-int MessagingInterface::broadcast(char* buffer, int size, int root, bool durable) {
+int MessagingInterface::broadcast(uint8_t* buffer, size_t size, bool durable) {
     // todo: impl
     return 0;
 }
 
-int MessagingInterface::recv(char* buffer, int size, int source, const int tag) {
+std::optional<SizeAndSource> MessagingInterface::recv(uint8_t* buffer, size_t size, const uint8_t tag) {
     checkOrInsertTag(tag);
 
-    // todo: the buffer needs to be large enough, this copies into the buffer...
-    // todo: handle the source
-    xQueueReceive(m_tag_to_queue.at(tag), buffer, portMAX_DELAY);
+    // The message should already be validated if added to message queue
+    auto maybe_message = m_tag_to_queue.at(tag)->dequeue(std::chrono::milliseconds(MAX_RX_WAIT_TIME_MS));
+    if (!maybe_message) { return std::nullopt; }
+    const auto message = std::move(*maybe_message);
+    const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(message->data());
 
-    return 0;
+    const auto bytes_written = std::min(size, (size_t)mpi_message->length());
+    std::memcpy(buffer, mpi_message->payload()->data(), bytes_written);
+
+    return SizeAndSource{(size_t)bytes_written, (uint8_t)mpi_message->sender()};
 }
 
-int MessagingInterface::sendrecv(char* send_buffer, const int send_size, const int dest, const int send_tag, char* recv_buffer, const int recv_size, const int recv_tag, const bool durable) {
+int MessagingInterface::sendrecv(uint8_t* send_buffer, const size_t send_size, const uint8_t dest, const uint8_t send_tag, uint8_t* recv_buffer, const size_t recv_size, const uint8_t recv_tag, const bool durable) {
     send(send_buffer, send_size, dest, send_tag, durable);
-    recv(recv_buffer, recv_size, dest, recv_tag);
+    recv(recv_buffer, recv_size, recv_tag);
 
     return 0;
 }
 
-// todo: when handleRecv returns, remove from queue (from router)
 void MessagingInterface::handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer) {
     const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
 
     checkOrInsertTag(mpi_message->tag());
 
-    xQueueSendToBack(m_tag_to_queue.at(mpi_message->tag()), mpi_message->payload()->data(), 0);
+    m_tag_to_queue.at(mpi_message->tag())->enqueue(std::move(buffer), std::chrono::milliseconds(MAX_TX_WAIT_TIME_MS));
 }
 
 void MessagingInterface::checkOrInsertTag(const uint8_t tag) {
     xSemaphoreTake(m_map_semaphore, portMAX_DELAY);
     if (!m_tag_to_queue.contains(tag)) {
-        m_tag_to_queue[tag] = xQueueCreate(MPI_QUEUE_SIZE, MAX_MPI_BUFFER_SIZE);
+        m_tag_to_queue[tag] = std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(MPI_QUEUE_SIZE);
     }
     xSemaphoreGive(m_map_semaphore);
 }
@@ -81,3 +104,57 @@ Messaging::ConnectionType MessagingInterface::get_connection_type() const {
 uint8_t MessagingInterface::get_leader() const {
     return this->m_router->get_leader();
 }
+
+void MessagingInterface::handleRecvCall(void *args) {
+    const auto that = (MessagingInterface *)args;
+
+    while (!that->m_stop_thread) {
+        auto buffer = std::make_unique<std::vector<uint8_t>>();
+        buffer->resize(RPC_CALL_PARAMETERS_MAX_SIZE);
+        if (const auto rx_info = that->recv(buffer->data(), RPC_CALL_PARAMETERS_MAX_SIZE, FN_CALL_TAG)) {
+            buffer->resize((*rx_info).bytes_written);
+
+            flatbuffers::Verifier verifier(buffer->data(), buffer->size());
+            if (!Messaging::VerifySendCallBuffer(verifier)) {
+                continue;
+                ESP_LOGW(TAG, "Got invalid rpc call flatbuffer");
+            }
+
+            if (!that->m_rpc_call_queue->enqueue(std::move(buffer), std::chrono::milliseconds(RPC_CALL_ENQUEUE_TIMEOUT_MS))) {
+                ESP_LOGW(TAG, "Dropping RPC calls since queue is filled, and passed timeout");
+            }
+        }
+    }
+}
+
+void MessagingInterface::processRPCCall(void *args) {
+    const auto that = (MessagingInterface *)args;
+
+    Flatbuffers::CallBuilder builder{};
+
+    while (!that->m_stop_thread) {
+        if(auto maybe_rpc_call = that->m_rpc_call_queue->dequeue(std::chrono::milliseconds(RPC_CALL_DEQUEUE_TIMEOUT_MS))) {
+            auto rpc_call = *std::move(maybe_rpc_call);
+            auto parsed_rpc_call = builder.parse_send_call(rpc_call->data());
+
+            if (!that->m_tag_to_fn[parsed_rpc_call->tag()]) {
+                ESP_LOGW(TAG, "RPC call failed, tried to call unregistered tag %d", parsed_rpc_call->tag());
+                vTaskDelay(pdMS_TO_TICKS(THREAD_POOL_SLEEP_AFTER_FAIL_MS));
+                continue;
+            }
+
+            Writer w{parsed_rpc_call->unique_id(), *that};
+            that->m_tag_to_fn[parsed_rpc_call->tag()](parsed_rpc_call->parameters()->data(), (size_t)parsed_rpc_call->length(), w);
+        }
+    }
+}
+
+bool MessagingInterface::register_function(uint8_t function_tag, std::function<void(const uint8_t *, size_t, Writer&)> f) {
+    if (m_tag_to_fn[function_tag]) {
+        ESP_LOGW(TAG, "Attempt to register a function that was already registered");
+        return false;
+    }
+
+    m_tag_to_fn[function_tag] = f;
+    return true;
+}

components/rpc/include/CommunicationRouter.h

@@ -56,7 +56,7 @@ public:
 
   [[noreturn]] static void router_thread(void *args);
   [[noreturn]] static void link_layer_thread(void *args);
-  int send_msg(char *buffer, size_t length) const;
+  int send_msg(uint8_t *buffer, size_t length) const;
   void update_leader();
   void route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const;
   void route(uint8_t* buffer, size_t size) const;

components/rpc/include/MessagingInterface.h

@@ -7,41 +7,83 @@
 
 #include <memory>
 #include <unordered_map>
-#include <flatbuffers_generated/TopologyMessage_generated.h>
+#include <vector>
 
+#include "flatbuffers_generated/TopologyMessage_generated.h"
+#include "CallBuilder.h"
 #include "BlockingQueue.h"
 #include "constants/app_comms.h"
 #include "CommunicationRouter.h"
 
+#define FN_CALL_TAG 100  // reserved tag for RPC functionality
+
+struct SizeAndSource {
+    size_t bytes_written;
+    uint8_t sender;
+};
+
+class MessagingInterface;  // fwd def
+
+class Writer {
+public:
+    Writer(uint8_t unique_id, MessagingInterface &messaging_interface) : m_unique_id(unique_id), m_messaging_interface(messaging_interface) {}
+
+    // Return a value from an RPC call.
+    bool write(std::vector<uint8_t>& return_value);
+private:
+    uint8_t m_unique_id;
+    MessagingInterface &m_messaging_interface;
+    Flatbuffers::CallBuilder m_builder{};  // todo: this may be expensive to keep allocating, potentially move to MI & guard with lock
+    bool m_called = false;
+};
+
 class MessagingInterface {
 public:
     explicit MessagingInterface()
         : m_config_manager(ConfigManager::get_instance()),
             m_mpi_rx_queue(std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(RX_QUEUE_SIZE)),
+            m_rpc_call_queue(std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(RPC_CALL_QUEUE_SIZE)),
             m_router(std::make_unique<CommunicationRouter>([this](std::unique_ptr<std::vector<uint8_t>>&& buffer) { handleRecv(std::move(buffer)); })),
-            m_map_semaphore(xSemaphoreCreateMutex()) {};
+            m_map_semaphore(xSemaphoreCreateMutex()) {
+                xTaskCreate(handleRecvCall, "rpc_rx", 3072, this, 5, &m_handle_recv_call_task);
+                m_thread_pool.resize(RPC_CALL_THREAD_POOL_SIZE);
+                for (int i = 0; i < RPC_CALL_THREAD_POOL_SIZE; i++) {
+                     xTaskCreate(processRPCCall, "rpc_thread", 3072, this, 5, &m_thread_pool[i]);
+                }
+            };
 
     ~MessagingInterface();
 
-    int send(char* buffer, int size, int destination, int tag, bool durable);
-    int broadcast(char* buffer, int size, int root, bool durable);
-    int recv(char* buffer, int size, int source, int tag);
-    int sendrecv(char *send_buffer, int send_size, int dest, int send_tag, char *recv_buffer, int recv_size, int recv_tag, bool durable);
+    int send(uint8_t* buffer, size_t size, uint8_t destination, uint8_t tag, bool durable);
+    int broadcast(uint8_t* buffer, size_t size, bool durable);
+    std::optional<SizeAndSource> recv(uint8_t * buffer, size_t size, uint8_t tag);
+    int sendrecv(uint8_t *send_buffer, size_t send_size, uint8_t dest, uint8_t send_tag, uint8_t *recv_buffer, size_t recv_size, uint8_t recv_tag, bool durable);
     std::pair<std::vector<uint8_t>, std::vector<Orientation>> get_physically_connected_modules() const;
     Messaging::ConnectionType get_connection_type() const;
     uint8_t get_leader() const;
 
+    // Register a function for RPC (to be executed in a threadpool). Similar interface to GRPC with the writer.
+    bool register_function(uint8_t function_tag, std::function<void(const uint8_t *, size_t, Writer&)>);
+
 private:
     void handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer);
+    static void handleRecvCall(void *args);
+    static void processRPCCall(void *args);
 
     void checkOrInsertTag(uint8_t tag);
 
     ConfigManager& m_config_manager;
     uint16_t m_sequence_number = 0;
     std::unique_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_mpi_rx_queue;
+    std::unique_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rpc_call_queue;
     std::unique_ptr<CommunicationRouter> m_router;
     SemaphoreHandle_t m_map_semaphore;
-    std::unordered_map<uint8_t, QueueHandle_t> m_tag_to_queue;
+    std::unordered_map<uint8_t, std::unique_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>> m_tag_to_queue;
+    std::unordered_map<uint8_t, std::function<void(const uint8_t *, size_t, Writer&)>> m_tag_to_fn;
+
+    std::atomic<bool> m_stop_thread{false};
+    TaskHandle_t m_handle_recv_call_task;
+    std::vector<TaskHandle_t> m_thread_pool;
 };
 
 #endif //MESSAGINGINTERFACE_H

main/LoopManager.cpp

@@ -19,11 +19,12 @@
 [[noreturn]] void LoopManager::control_loop() const {
     uint8_t buffer[512];
     while (true) {
-        m_messaging_interface->recv(reinterpret_cast<char *>(buffer), 512, PC_ADDR,
-                                    ACTUATOR_CMD_TAG);
+        if (m_messaging_interface->recv(buffer, 512,
+                                    ACTUATOR_CMD_TAG)) {
                                         m_actuator->actuate(buffer);
                                         send_sensor_reading(false);
                                     }
+            }
 }
 
 [[noreturn]] void LoopManager::sensor_loop(char *args) {
@@ -53,7 +54,7 @@
             that->m_config_manager.get_module_id(), that->m_config_manager.get_module_type(),
             module_ids, casted_orientations, that->m_messaging_interface->get_connection_type(),
             that->m_messaging_interface->get_leader());
-        that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR,
+        that->m_messaging_interface->send((uint8_t *)data, size, PC_ADDR,
                                           TOPOLOGY_CMD_TAG, false);
         vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
     }
@@ -61,10 +62,10 @@
 
 [[noreturn]] void LoopManager::metadata_rx_loop(char *args) {
     const auto that = reinterpret_cast<LoopManager *>(args);
-    const auto buffer = std::make_unique<std::vector<char>>();
+    const auto buffer = std::make_unique<std::vector<uint8_t>>();
     buffer->resize(512);
     while (true) {
-        that->m_messaging_interface->recv(buffer->data(), 512, PC_ADDR, METADATA_RX_TAG);
+        that->m_messaging_interface->recv(buffer->data(), 512, METADATA_RX_TAG);
     }
 }
 
@@ -74,6 +75,6 @@ void LoopManager::send_sensor_reading(bool durable) const {
     if (m_actuator) {
         auto data = m_actuator->get_sensor_data();
         const auto [ptr, size] = smb.build_sensor_message(data);
-        m_messaging_interface->send(reinterpret_cast<char *>(ptr), size, PC_ADDR, SENSOR_TAG, durable);
+        m_messaging_interface->send((uint8_t *)ptr, size, PC_ADDR, SENSOR_TAG, durable);
     }
 }

main/include/LoopManager.h

@@ -6,6 +6,8 @@
 #define LOOPMANAGER_H
 
 #include <memory>
+#include <vector>
+#include <cstdint>
 
 #include "MessagingInterface.h"
 #include "control/ActuatorFactory.h"
@@ -15,7 +17,18 @@ class LoopManager {
 public:
     LoopManager() : m_config_manager(ConfigManager::get_instance()),
         m_messaging_interface(std::make_unique<MessagingInterface>()),
-       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())) {}
+       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())) {
+
+           // todo: temporary demo register_function call.
+           const auto function_tag = 100;
+           m_messaging_interface->register_function(function_tag, [](const uint8_t *parameters, size_t parameter_size, Writer& writer) {
+               ESP_LOGI("TMP", "%s", (char*)parameters);
+               std::string msg = "Hello from board!";
+               std::vector<uint8_t> return_value(msg.begin(), msg.end());
+               writer.write(return_value);
+           });
+       }
+
     [[noreturn]] void control_loop() const;                     // gets control commands
     [[noreturn]] static void sensor_loop(char * args);          // sends sensor data commands continually
     [[noreturn]] static void metadata_tx_loop(char * args);     // sends metadata continually (low duty cycle)