RPC calls

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