TactilityKernel improvements and more (#459)

TactilityKernel/Source/concurrent/Dispatcher.cpp

@@ -0,0 +1,142 @@
+// SPDX-License-Identifier: Apache-2.0
+
+#include <queue>
+
+#include <Tactility/concurrent/Dispatcher.h>
+
+#include "Tactility/Error.h"
+
+#include <Tactility/Log.h>
+#include <Tactility/concurrent/EventGroup.h>
+#include <Tactility/concurrent/Mutex.h>
+#include <atomic>
+
+#define TAG LOG_TAG("Dispatcher")
+
+static constexpr EventBits_t BACKPRESSURE_WARNING_COUNT = 100U;
+static constexpr EventBits_t WAIT_FLAG = 1U;
+
+struct QueuedItem {
+    DispatcherCallback callback;
+    void* context;
+};
+
+struct DispatcherData {
+    Mutex mutex = { 0 };
+    std::queue<QueuedItem> queue = {};
+    EventGroupHandle_t eventGroup = nullptr;
+    std::atomic<bool> shutdown{false}; // TODO: Use EventGroup
+
+    DispatcherData() {
+        event_group_construct(&eventGroup);
+        mutex_construct(&mutex);
+    }
+
+    ~DispatcherData() {
+        event_group_destruct(&eventGroup);
+        mutex_destruct(&mutex);
+    }
+};
+
+#define dispatcher_data(handle) static_cast<DispatcherData*>(handle)
+
+extern "C" {
+
+DispatcherHandle_t dispatcher_alloc(void) {
+    return new DispatcherData();
+}
+
+void dispatcher_free(DispatcherHandle_t dispatcher) {
+    auto* data = dispatcher_data(dispatcher);
+    data->shutdown.store(true, std::memory_order_release);
+    mutex_lock(&data->mutex);
+    mutex_unlock(&data->mutex);
+    delete data;
+}
+
+error_t dispatcher_dispatch_timed(DispatcherHandle_t dispatcher, void* callbackContext, DispatcherCallback callback, TickType_t timeout) {
+    auto* data = dispatcher_data(dispatcher);
+
+    // Mutate
+    if (!mutex_try_lock_timed(&data->mutex, timeout)) {
+#ifdef ESP_PLATFORM
+        LOG_E(TAG, "Mutex acquisition timeout");
+#endif
+        return ERROR_TIMEOUT;
+    }
+
+    if (data->shutdown.load(std::memory_order_acquire)) {
+        mutex_unlock(&data->mutex);
+        return ERROR_INVALID_STATE;
+    }
+
+    data->queue.push({
+        .callback = callback,
+        .context = callbackContext
+    });
+
+    if (data->queue.size() == BACKPRESSURE_WARNING_COUNT) {
+#ifdef ESP_PLATFORM
+        LOG_W(TAG, "Backpressure: You're not consuming fast enough (100 queued)");
+#endif
+    }
+
+    mutex_unlock(&data->mutex);
+
+    if (event_group_set(data->eventGroup, WAIT_FLAG) != ERROR_NONE) {
+#ifdef ESP_PLATFORM
+        LOG_E(TAG, "Failed to set flag");
+#endif
+        return ERROR_RESOURCE;
+    }
+
+    return ERROR_NONE;
+}
+
+error_t dispatcher_consume_timed(DispatcherHandle_t dispatcher, TickType_t timeout) {
+    auto* data = dispatcher_data(dispatcher);
+
+    // TODO: keep track of time and consider the timeout input as total timeout
+
+    // Wait for signal
+    error_t error = event_group_wait(data->eventGroup, WAIT_FLAG, false, true, nullptr, timeout);
+    if (error != ERROR_NONE) {
+        if (error == ERROR_TIMEOUT) {
+            return ERROR_TIMEOUT;
+        } else {
+            return ERROR_RESOURCE;
+        }
+    }
+
+    if (data->shutdown.load(std::memory_order_acquire)) {
+        return ERROR_INVALID_STATE;
+    }
+
+    // Mutate
+    bool processing = true;
+    do {
+        if (mutex_try_lock_timed(&data->mutex, 10)) {
+            if (!data->queue.empty()) {
+                // Make a copy, so it's thread-safe when we unlock
+                auto entry = data->queue.front();
+                data->queue.pop();
+                processing = !data->queue.empty();
+                // Don't keep lock as callback might be slow and we want to allow dispatch in the meanwhile
+                mutex_unlock(&data->mutex);
+                entry.callback(entry.context);
+            } else {
+                processing = false;
+                mutex_unlock(&data->mutex);
+            }
+        } else {
+#ifdef ESP_PLATFORM
+            LOG_W(TAG, "Mutex acquisition timeout");
+#endif
+        }
+
+    } while (processing && !data->shutdown.load(std::memory_order_acquire));
+
+    return ERROR_NONE;
+}
+
+}

TactilityKernel/Source/concurrent/EventGroup.cpp

@@ -0,0 +1,84 @@
+// SPDX-License-Identifier: Apache-2.0
+
+#include <Tactility/concurrent/EventGroup.h>
+#include <Tactility/Error.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+error_t event_group_set(EventGroupHandle_t eventGroup, uint32_t inFlags) {
+    if (xPortInIsrContext() == pdTRUE) {
+        BaseType_t yield = pdFALSE;
+        if (xEventGroupSetBitsFromISR(eventGroup, inFlags, &yield) == pdFAIL) {
+            return ERROR_RESOURCE;
+        }
+        portYIELD_FROM_ISR(yield);
+    } else {
+        xEventGroupSetBits(eventGroup, inFlags);
+    }
+    return ERROR_NONE;
+}
+
+error_t event_group_clear(EventGroupHandle_t eventGroup, uint32_t flags) {
+    if (xPortInIsrContext() == pdTRUE) {
+        if (xEventGroupClearBitsFromISR(eventGroup, flags) == pdFAIL) {
+            return ERROR_RESOURCE;
+        }
+        portYIELD_FROM_ISR(pdTRUE);
+    } else {
+        xEventGroupClearBits(eventGroup, flags);
+    }
+    return ERROR_NONE;
+}
+
+uint32_t event_group_get(EventGroupHandle_t eventGroup) {
+    if (xPortInIsrContext() == pdTRUE) {
+        return xEventGroupGetBitsFromISR(eventGroup);
+    } else {
+        return xEventGroupGetBits(eventGroup);
+    }
+}
+
+error_t event_group_wait(
+    EventGroupHandle_t eventGroup,
+    uint32_t inFlags,
+    bool awaitAll,
+    bool clearOnExit,
+    uint32_t* outFlags,
+    TickType_t timeout
+) {
+    if (xPortInIsrContext()) {
+        return ERROR_ISR_STATUS;
+    }
+
+    uint32_t result_flags = xEventGroupWaitBits(
+        eventGroup,
+        inFlags,
+        clearOnExit ? pdTRUE : pdFALSE,
+        awaitAll ? pdTRUE : pdFALSE,
+        timeout
+    );
+
+    auto invalid_flags = awaitAll
+        ? ((inFlags & result_flags) != inFlags) // await all
+        : ((inFlags & result_flags) == 0U); // await any
+
+    if (invalid_flags) {
+        const uint32_t matched = inFlags & result_flags;
+        if (matched == 0U) {
+            return ERROR_TIMEOUT;
+        }
+        return ERROR_RESOURCE;
+    }
+
+    if (outFlags != nullptr) {
+        *outFlags = result_flags;
+    }
+
+    return ERROR_NONE;
+}
+
+#ifdef __cplusplus
+}
+#endif