From a5f79f5eacd09f8f49bebbde2674d9504832552c Mon Sep 17 00:00:00 2001
From: Diogo Diniz <diogocruzdiniz@gmail.com>
Date: Tue, 30 Jun 2026 11:59:19 +0100
Subject: [PATCH] feat: Implemented do_message_dispatch

---
 app/src/incphub/src/incphub_shared.c | 29 +++++++++++++++++++++-------
 1 file changed, 22 insertions(+), 7 deletions(-)

diff --git a/app/src/incphub/src/incphub_shared.c b/app/src/incphub/src/incphub_shared.c
index 912612c..c32ff86 100644
--- a/app/src/incphub/src/incphub_shared.c
+++ b/app/src/incphub/src/incphub_shared.c
@@ -2,6 +2,7 @@
 
 #include <string.h>
 #include <stdbool.h>
+#include <stdint.h>
 
 #include "FreeRTOS.h"
 #include "semphr.h"
@@ -13,6 +14,13 @@
 #include "nil/nil.h"
 
 
+
+#if UINTPTR_MAX > UINT32_MAX
+#error "Pointers of 32-bits or less are required for incphub mailbox to function correctly."
+#endif
+
+
+
 // === Private variables ===
 
 // Global lock
@@ -207,16 +215,23 @@ int dequeue_message_ingest(uint8_t **buffer_ptr, TickType_t max_delay)
     return INCPHUB_OK;
 }
 
-int do_message_dispatch(incphub_cli_t *cli, uint8_t *buffer, size_t length, incphub_addr_t source)
+int do_message_dispatch(incphub_cli_t *cli, uint8_t *buffer)
 {
     //NOTE: Does not need global lock
 
-    (void)cli;
-    (void)buffer;
-    (void)length;
-    (void)source;
-    //TODO: Implement
-    return INCPHUB_ERR;
+    if (cli == NULL)
+        return INCPHUB_EINVAL;
+    if (buffer == NULL)
+        return INCPHUB_EINVAL;
+
+    // Try to notify
+    if (xTaskNotifyIndexed(cli->task, INCPHUB_TASKINDEX_MAILBOX, (uint32_t)buffer, eSetValueWithoutOverwrite) == pdFALSE)
+    {
+        // Failed because previous message has not yet been processed
+        return INCPHUB_EAGAIN;
+    }
+
+    return INCPHUB_OK;
 }
 
 int await_message_dispatch(uint8_t **buffer_ptr, size_t *length_ptr, incphub_addr_t *source_ptr)