Create TactilityFreertos subproject (#440)

TactilityFreeRtos/Include/Tactility/Dispatcher.h

@@ -0,0 +1,132 @@
+/**
+* Dispatcher is a thread-safe code execution queue.
+*/
+#pragma once
+
+#include "EventGroup.h"
+#include "Mutex.h"
+#include "kernel/Kernel.h"
+
+#ifdef ESP_PLATFORM
+#include <esp_log.h>
+#endif
+
+#include <functional>
+#include <memory>
+#include <queue>
+
+namespace tt {
+
+/**
+ * A thread-safe way to defer code execution.
+ * Generally, one task would dispatch the execution,
+ * while the other thread consumes and executes the work.
+ */
+class Dispatcher final {
+
+    static constexpr auto TAG = "Dispatcher";
+    static constexpr EventBits_t BACKPRESSURE_WARNING_COUNT = 100U;
+    static constexpr EventBits_t WAIT_FLAG = 1U;
+
+public:
+
+    typedef std::function<void()> Function;
+
+private:
+
+    Mutex mutex;
+    std::queue<Function> queue = {};
+    EventGroup eventFlag;
+    bool shutdown = false;
+
+public:
+
+    explicit Dispatcher() = default;
+
+    ~Dispatcher() {
+        shutdown = true;
+        if (mutex.lock()) {
+		    mutex.unlock();
+        }
+    }
+
+    /**
+     * Queue a function to be consumed elsewhere.
+     * @param[in] function the function to execute elsewhere
+     * @param[in] timeout lock acquisition timeout
+     * @return true if dispatching was successful (timeout not reached)
+     */
+    bool dispatch(Function function, TickType_t timeout = kernel::MAX_TICKS) {
+        // Mutate
+        if (!mutex.lock(timeout)) {
+#ifdef ESP_PLATFORM
+            ESP_LOGE(TAG, "Mutex acquisition timeout");
+#endif
+            return false;
+        }
+
+        if (shutdown) {
+            return false;
+        }
+
+        queue.push(std::move(function));
+        if (queue.size() == BACKPRESSURE_WARNING_COUNT) {
+#ifdef ESP_PLATFORM
+            ESP_LOGW(TAG, "Backpressure: You're not consuming fast enough (100 queued)");
+#endif
+        }
+        mutex.unlock();
+        if (!eventFlag.set(WAIT_FLAG)) {
+#ifdef ESP_PLATFORM
+            ESP_LOGE(TAG, "Failed to set flag");
+#endif
+        }
+        return true;
+    }
+
+    /**
+     * Consume 1 or more dispatched function (if any) until the queue is empty.
+     * @warning The timeout is only the wait time before consuming the message! It is not a limit to the total execution time when calling this method.
+     * @param[in] timeout the ticks to wait for a message
+     * @return the amount of messages that were consumed
+     */
+    uint32_t consume(TickType_t timeout = kernel::MAX_TICKS) {
+        // Wait for signal
+        if (!eventFlag.wait(WAIT_FLAG, false, true, timeout)) {
+            return 0;
+        }
+
+        if (shutdown) {
+            return 0;
+        }
+
+        // Mutate
+        bool processing = true;
+        uint32_t consumed = 0;
+        do {
+            if (mutex.lock(10)) {
+                if (!queue.empty()) {
+                    auto function = queue.front();
+                    queue.pop();
+                    consumed++;
+                    processing = !queue.empty();
+                    // Don't keep lock as callback might be slow
+                    mutex.unlock();
+                    function();
+                } else {
+                    processing = false;
+                    mutex.unlock();
+                }
+            } else {
+#ifdef ESP_PLATFORM
+                ESP_LOGW(TAG, "Mutex acquisition timeout");
+#endif
+            }
+
+        } while (processing && !shutdown);
+
+        return consumed;
+    }
+};
+
+} // namespace

TactilityFreeRtos/Include/Tactility/DispatcherThread.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "Dispatcher.h"
+#include "Thread.h"
+
+namespace tt {
+
+/** Starts a Thread to process dispatched messages */
+class DispatcherThread final {
+
+    Dispatcher dispatcher;
+    std::unique_ptr<Thread> thread;
+    bool interruptThread = true;
+
+    int32_t threadMain() {
+        do {
+            /**
+             * If this value is too high (e.g. 1 second) then the dispatcher destroys too slowly when the simulator exits.
+             * This causes the problems with other services doing an update (e.g. Statusbar) and calling into destroyed mutex in the global scope.
+             */
+            dispatcher.consume(100 / portTICK_PERIOD_MS);
+        } while (!interruptThread);
+
+        return 0;
+    }
+
+public:
+
+    explicit DispatcherThread(const std::string& threadName, size_t threadStackSize = 4096) {
+        thread = std::make_unique<Thread>(
+            threadName,
+            threadStackSize,
+            [this] {
+                return threadMain();
+            }
+        );
+    }
+
+    ~DispatcherThread() {
+        if (thread->getState() != Thread::State::Stopped) {
+            stop();
+        }
+    }
+
+    /**
+     * Dispatch a message.
+     */
+    bool dispatch(const Dispatcher::Function& function, TickType_t timeout = kernel::MAX_TICKS) {
+        return dispatcher.dispatch(function, timeout);
+    }
+
+    /** Start the thread (blocking). */
+    void start() {
+        interruptThread = false;
+        thread->start();
+    }
+
+    /** Stop the thread (blocking). */
+    void stop() {
+        interruptThread = true;
+        thread->join();
+    }
+
+    /** @return true of the thread is started */
+    bool isStarted() const { return thread != nullptr && !interruptThread; }
+};
+
+}

TactilityFreeRtos/Include/Tactility/EventGroup.h

@@ -0,0 +1,165 @@
+#pragma once
+
+#include "freertoscompat/EventGroups.h"
+#include "freertoscompat/PortCompat.h"
+#include "kernel/Kernel.h"
+
+#include <cassert>
+#include <memory>
+
+namespace tt {
+
+/** Wrapper for FreeRTOS xEventGroup and related code */
+class EventGroup final {
+
+    struct EventGroupHandleDeleter {
+        static void operator()(EventGroupHandle_t handleToDelete) {
+            vEventGroupDelete(handleToDelete);
+        }
+    };
+
+    std::unique_ptr<std::remove_pointer_t<EventGroupHandle_t>, EventGroupHandleDeleter> handle = std::unique_ptr<std::remove_pointer_t<EventGroupHandle_t>, EventGroupHandleDeleter>(xEventGroupCreate());
+
+public:
+
+    EventGroup() {
+        assert(xPortInIsrContext() == pdFALSE);
+        assert(handle != nullptr);
+    }
+
+    ~EventGroup() {
+        assert(xPortInIsrContext() == pdFALSE);
+    }
+
+    enum class Error {
+        Unknown,
+        Timeout,
+        Resource,
+        Parameter,
+        IsrStatus
+    };
+
+    /**
+     * Set the flags.
+     * @param[in] flags the flags to set
+     * @param[out] outFlags optional resulting flags: this is set when the return value is true
+     * @param[out] outError optional error output: this is set when the return value is false
+     * @return true on success
+     */
+    bool set(uint32_t flags, uint32_t* outFlags = nullptr, Error* outError = nullptr) const {
+        assert(handle);
+        if (xPortInIsrContext() == pdTRUE) {
+            uint32_t result;
+            BaseType_t yield = pdFALSE;
+            if (xEventGroupSetBitsFromISR(handle.get(), flags, &yield) == pdFAIL) {
+                if (outError != nullptr) {
+                    *outError = Error::Resource;
+                }
+                return false;
+            } else {
+                if (outFlags != nullptr) {
+                    *outFlags = flags;
+                }
+                portYIELD_FROM_ISR(yield);
+                return true;
+            }
+        } else {
+            auto result = xEventGroupSetBits(handle.get(), flags);
+            if (outFlags != nullptr) {
+                *outFlags = result;
+            }
+            return true;
+        }
+    }
+
+    /**
+     * Clear flags
+     * @param[in] flags the flags to clear
+     * @param[out] outFlags optional resulting flags: this is set when the return value is true
+     * @param[out] outError optional error output: this is set when the return value is false
+     * @return true on success
+     */
+    bool clear(uint32_t flags, uint32_t* outFlags = nullptr, Error* outError = nullptr) const {
+        if (xPortInIsrContext() == pdTRUE) {
+            uint32_t result = xEventGroupGetBitsFromISR(handle.get());
+            if (xEventGroupClearBitsFromISR(handle.get(), flags) == pdFAIL) {
+                if (outError != nullptr) {
+                    *outError = Error::Resource;
+                }
+                return false;
+            }
+            if (outFlags != nullptr) {
+                *outFlags = result;
+            }
+            portYIELD_FROM_ISR(pdTRUE);
+            return true;
+        } else {
+            auto result = xEventGroupClearBits(handle.get(), flags);
+            if (outFlags != nullptr) {
+                *outFlags = result;
+            }
+            return true;
+        }
+    }
+
+    /**
+     * @return the current flags
+     */
+    uint32_t get() const {
+        if (xPortInIsrContext() == pdTRUE) {
+            return xEventGroupGetBitsFromISR(handle.get());
+        } else {
+            return xEventGroupGetBits(handle.get());
+        }
+    }
+
+    /**
+     * Wait for flags to be set
+     * @param[in] flags the flags to await
+     * @param[in] awaitAll If true, await for all bits to be set. Otherwise, await for any.
+     * @param[in] clearOnExit If true, clears all the bits on exit, otherwise don't clear.
+     * @param[in] timeout the maximum amount of ticks to wait for flags to be set
+     * @param[out] outFlags optional resulting flags: this is set when the return value is true
+     * @param[out] outError optional error output: this is set when the return value is false
+     */
+    bool wait(
+        uint32_t flags,
+        bool awaitAll = false,
+        bool clearOnExit = true,
+        TickType_t timeout = kernel::MAX_TICKS,
+        uint32_t* outFlags = nullptr,
+        Error* outError = nullptr
+    ) const {
+        assert(xPortInIsrContext() == pdFALSE);
+
+        uint32_t result_flags = xEventGroupWaitBits(
+            handle.get(),
+            flags,
+            clearOnExit ? pdTRUE : pdFALSE,
+            awaitAll ? pdTRUE : pdFALSE,
+            timeout
+        );
+
+        auto invalid_flags = awaitAll
+            ? ((flags & result_flags) != flags) // await all
+            : ((flags & result_flags) == 0U); // await any
+        if (invalid_flags) {
+            if (outError != nullptr) {
+                if (timeout > 0U) { // assume time-out
+                    *outError = Error::Timeout;
+                } else {
+                    *outError = Error::Resource;
+                }
+            }
+            return false;
+        }
+
+        if (outFlags != nullptr) {
+            *outFlags = result_flags;
+        }
+
+        return true;
+    }
+};
+
+} // namespace

TactilityFreeRtos/Include/Tactility/Lock.h

@@ -0,0 +1,83 @@
+#pragma once
+
+#include "kernel/Kernel.h"
+
+#include <Tactility/freertoscompat/RTOS.h>
+#include <functional>
+#include <memory>
+
+namespace tt {
+
+class ScopedLock;
+
+/** Represents a lock/mutex */
+class Lock {
+
+public:
+
+    virtual ~Lock() = default;
+
+    virtual bool lock(TickType_t timeout) const = 0;
+
+    bool lock() const { return lock(kernel::MAX_TICKS); }
+
+    virtual void unlock() const = 0;
+
+    void withLock(TickType_t timeout, const std::function<void()>& onLockAcquired) const {
+        if (lock(timeout)) {
+            onLockAcquired();
+            unlock();
+        }
+    }
+
+    void withLock(TickType_t timeout, const std::function<void()>& onLockAcquired, const std::function<void()>& onLockFailure) const {
+        if (lock(timeout)) {
+            onLockAcquired();
+            unlock();
+        } else {
+            onLockFailure();
+        }
+    }
+
+    void withLock(const std::function<void()>& onLockAcquired) const { withLock(kernel::MAX_TICKS, onLockAcquired); }
+
+    void withLock(const std::function<void()>& onLockAcquired, const std::function<void()>& onLockFailed) const { withLock(kernel::MAX_TICKS, onLockAcquired, onLockFailed); }
+
+	ScopedLock asScopedLock() const;
+};
+
+/**
+ * Represents a lockable instance that is scoped to a specific lifecycle.
+ * Once the ScopedLock is destroyed, unlock() is called automatically.
+ *
+ * In other words:
+ * You have to lock() this object manually, but unlock() happens automatically on destruction.
+ */
+class ScopedLock final : public Lock {
+
+    const Lock& lockable;
+
+public:
+
+    using Lock::lock;
+
+    explicit ScopedLock(const Lock& lockable) : lockable(lockable) {}
+
+    ~ScopedLock() override {
+        lockable.unlock(); // We don't care whether it succeeded or not
+    }
+
+    bool lock(TickType_t timeout) const override {
+        return lockable.lock(timeout);
+    }
+
+    void unlock() const override {
+        lockable.unlock();
+    }
+};
+
+inline ScopedLock Lock::asScopedLock() const {
+	return ScopedLock(*this);
+}
+
+}

TactilityFreeRtos/Include/Tactility/MessageQueue.h

@@ -0,0 +1,131 @@
+/**
+ * MessageQueue is a wrapper for FreeRTOS xQueue functionality.
+ * There is no additional thread-safety on top of the xQueue functionality,
+ * so make sure you create a lock if needed.
+ */
+#pragma once
+
+#include <memory>
+#include <cassert>
+
+#include "freertoscompat/PortCompat.h"
+#include "freertoscompat/Queue.h"
+
+namespace tt {
+
+/**
+ * Wraps xQueue functionality.
+ * Calls can be done from ISR context unless otherwise specified.
+ */
+class MessageQueue final {
+
+    static QueueHandle_t createQueue(uint32_t capacity, uint32_t messageSize) {
+        assert(capacity > 0U);
+        assert(messageSize > 0U);
+        return xQueueCreate(capacity, messageSize);
+    }
+
+    struct QueueHandleDeleter {
+        static void operator()(QueueHandle_t handleToDelete) {
+            vQueueDelete(handleToDelete);
+        }
+    };
+
+    std::unique_ptr<std::remove_pointer_t<QueueHandle_t>, QueueHandleDeleter> handle;
+
+public:
+    /**
+     * Allocate message queue
+     * @param[in] capacity Maximum messages in queue
+     * @param[in] messageSize The size in bytes of a single message
+     */
+    MessageQueue(uint32_t capacity, uint32_t messageSize) : handle(createQueue(capacity, messageSize)) {
+        assert(handle != nullptr);
+        assert(xPortInIsrContext() == pdFALSE);
+    }
+
+    /**
+     * @warning Don't call this from ISR context
+     */
+    ~MessageQueue() {
+        assert(xPortInIsrContext() == pdFALSE);
+    }
+
+    /**
+     * Post a message to the queue.
+     * The message is queued by copy, not by reference.
+     * @param[in] message A pointer to a message. The message will be copied into a buffer.
+     * @param[in] timeout
+     * @return success result
+     */
+    bool put(const void* message, TickType_t timeout) const {
+        assert(handle != nullptr);
+        assert(message != nullptr);
+
+        if (xPortInIsrContext() == pdTRUE) {
+            if (timeout != 0U) {
+                return false;
+            }
+
+            BaseType_t yield = pdFALSE;
+            if (xQueueSendToBackFromISR(handle.get(), message, &yield) != pdTRUE) {
+                return false;
+            }
+
+            portYIELD_FROM_ISR(yield);
+            return true;
+        } else {
+            return xQueueSendToBack(handle.get(), message, timeout) == pdPASS;
+        }
+    }
+
+    /**
+     * Get message from queue
+     * @param[out] message A pointer to an already allocated message object
+     * @param[in] timeout
+     * @return success result
+     */
+    bool get(void* message, TickType_t timeout) const {
+        assert(handle != nullptr);
+        assert(message != nullptr);
+
+        if (xPortInIsrContext() == pdTRUE) {
+            if (timeout != 0U) {
+                return false;
+            }
+
+            BaseType_t yield = pdFALSE;
+            if (xQueueReceiveFromISR(handle.get(), message, &yield) != pdPASS) {
+                return false;
+            }
+
+            portYIELD_FROM_ISR(yield);
+            return true;
+        } else {
+            return xQueueReceive(handle.get(), message, timeout) == pdPASS;
+        }
+    }
+
+    /**
+     * @return The amount of messages in the queue.
+     */
+    uint32_t getCount() const {
+        assert(handle != nullptr);
+        return (xPortInIsrContext() == pdTRUE)
+            ? uxQueueMessagesWaitingFromISR(handle.get())
+            : uxQueueMessagesWaiting(handle.get());
+    }
+
+    /**
+     * Reset queue
+     * @warning Don't call this from ISR context
+     * @return success result
+     */
+    void reset() const {
+        assert(handle != nullptr);
+        assert(xPortInIsrContext() == pdFALSE);
+        xQueueReset(handle.get());
+    }
+};
+
+} // namespace

TactilityFreeRtos/Include/Tactility/Mutex.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "Lock.h"
+#include "freertoscompat/PortCompat.h"
+#include "freertoscompat/Semaphore.h"
+
+#include <memory>
+#include <cassert>
+
+namespace tt {
+
+/**
+ * Wrapper for FreeRTOS xSemaphoreCreateMutex
+ * Cannot be used from ISR context
+ */
+class Mutex final : public Lock {
+
+    std::unique_ptr<std::remove_pointer_t<QueueHandle_t>, SemaphoreHandleDeleter> handle = std::unique_ptr<std::remove_pointer_t<QueueHandle_t>, SemaphoreHandleDeleter>(xSemaphoreCreateMutex());
+
+public:
+
+    using Lock::lock;
+
+    explicit Mutex() {
+        assert(handle != nullptr);
+    }
+
+    ~Mutex() override = default;
+
+    /** Attempt to lock the mutex. Blocks until timeout passes or lock is acquired.
+     * @param[in] timeout
+     * @return success result
+     */
+    bool lock(TickType_t timeout) const override {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xSemaphoreTake(handle.get(), timeout) == pdPASS;
+    }
+
+    /** Unlock the mutex */
+    void unlock() const override {
+        assert(xPortInIsrContext() == pdFALSE);
+        xSemaphoreGive(handle.get());
+    }
+
+    /** @return the task handle of the owning task */
+    TaskHandle_t getOwner() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xSemaphoreGetMutexHolder(handle.get());
+    }
+};
+
+} // namespace tt

TactilityFreeRtos/Include/Tactility/PubSub.h

@@ -0,0 +1,109 @@
+#pragma once
+
+#include "Mutex.h"
+#include "kernel/Kernel.h"
+
+#include <functional>
+#include <list>
+
+#ifdef ESP_PLATFORM
+#include <esp_log.h>
+#endif
+
+#include <cassert>
+
+namespace tt {
+
+/** Publish and subscribe to messages in a thread-safe manner. */
+template<typename DataType>
+class PubSub final {
+
+    struct Subscription {
+        uint64_t id;
+        std::function<void(DataType)> callback;
+    };
+
+    typedef std::list<Subscription> Subscriptions;
+    uint64_t lastId = 0;
+    Subscriptions items;
+    Mutex mutex;
+
+public:
+
+    typedef void* SubscriptionHandle;
+
+    PubSub() = default;
+
+    ~PubSub() {
+        if (!items.empty()) {
+#ifdef ESP_PLATFORM
+            ESP_LOGW("PubSub", "Destroying with %d active subscriptions", items.size());
+#endif
+        }
+
+        // Wait for Mutex usage
+        if (mutex.lock(kernel::MAX_TICKS)) {
+            // TODO: Fix the case where the mutex might be immediately locked after this point and then crashes when deleted
+            mutex.unlock();
+        }
+    }
+
+    /**
+     * Start receiving messages at the specified handle (Re-entrable)
+     * @param[in] callback
+     * @return subscription instance
+     */
+    SubscriptionHandle subscribe(std::function<void(DataType)> callback) {
+        mutex.lock();
+
+        items.push_back({
+            .id = (++lastId),
+            .callback = std::move(callback)
+        });
+
+        mutex.unlock();
+
+        return reinterpret_cast<SubscriptionHandle>(lastId);
+    }
+
+    /**
+     * Stop receiving messages at the specified handle (Re-entrable)
+     * @param[in] subscription
+     */
+    void unsubscribe(SubscriptionHandle subscription) {
+        assert(subscription);
+
+        mutex.lock();
+
+        bool result = false;
+        auto id = reinterpret_cast<uint64_t>(subscription);
+        for (auto it = items.begin(); it != items.end(); ++it) {
+            if (it->id == id) {
+                items.erase(it);
+                result = true;
+                break;
+            }
+        }
+
+        mutex.unlock();
+        assert(result);
+    }
+
+    /**
+     * Publish something to all subscribers (Re-entrable)
+     * @param[in] data the data to publish
+     */
+    void publish(DataType data) {
+        mutex.lock();
+
+        // Iterate over subscribers
+        for (auto& it : items) {
+            it.callback(data);
+        }
+
+        mutex.unlock();
+    }
+};
+
+
+} // namespace

TactilityFreeRtos/Include/Tactility/RecursiveMutex.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "Lock.h"
+#include "freertoscompat/PortCompat.h"
+#include "freertoscompat/Semaphore.h"
+
+#include <memory>
+#include <cassert>
+
+namespace tt {
+
+/**
+ * Wrapper for FreeRTOS xSemaphoreCreateRecursiveMutex
+ * Cannot be used from ISR context
+ */
+class RecursiveMutex final : public Lock {
+
+    std::unique_ptr<std::remove_pointer_t<QueueHandle_t>, SemaphoreHandleDeleter> handle = std::unique_ptr<std::remove_pointer_t<QueueHandle_t>, SemaphoreHandleDeleter>(xSemaphoreCreateRecursiveMutex());
+
+public:
+
+    using Lock::lock;
+
+    explicit RecursiveMutex() {
+        assert(handle != nullptr);
+    }
+
+    ~RecursiveMutex() override = default;
+
+    /**
+     * Attempt to lock the mutex. Blocks until timeout passes or lock is acquired.
+     * @param[in] timeout
+     * @return success result
+     */
+    bool lock(TickType_t timeout) const override {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xSemaphoreTakeRecursive(handle.get(), timeout) == pdPASS;
+    }
+
+    /** Unlock the mutex */
+    void unlock() const override {
+        assert(xPortInIsrContext() == pdFALSE);
+        xSemaphoreGiveRecursive(handle.get());
+    }
+
+    /** @return the owner of the thread */
+     TaskHandle_t getOwner() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xSemaphoreGetMutexHolder(handle.get());
+    }
+};
+
+} // namespace

TactilityFreeRtos/Include/Tactility/Semaphore.h

@@ -0,0 +1,128 @@
+#pragma once
+
+#include "Lock.h"
+#include "freertoscompat/PortCompat.h"
+#include "freertoscompat/Semaphore.h"
+
+#include <cassert>
+#include <memory>
+
+#ifdef ESP_PLATFORM
+#include <esp_log.h>
+#endif
+
+namespace tt {
+
+/**
+ * Wrapper for xSemaphoreCreateBinary (max count == 1) and xSemaphoreCreateCounting (max count > 1)
+ * Can be used from ISR context, but cannot be created/destroyed from such a context.
+ */
+class Semaphore final : public Lock {
+
+    static QueueHandle_t createHandle(uint32_t maxCount, uint32_t initialAvailable) {
+        assert(maxCount > 0U);
+        assert(initialAvailable <= maxCount);
+
+        if (maxCount == 1U) {
+            auto result = xSemaphoreCreateBinary();
+            if (initialAvailable != 0U) {
+                auto give_result = xSemaphoreGive(result);
+                assert(give_result == pdPASS);
+            }
+            return result;
+        } else {
+            return xSemaphoreCreateCounting(maxCount, initialAvailable);
+        }
+    }
+
+    std::unique_ptr<std::remove_pointer_t<QueueHandle_t>, SemaphoreHandleDeleter> handle;
+
+public:
+
+    using Lock::lock;
+
+    /**
+     * Cannot be called from ISR context.
+     * @param[in] maxAvailable The maximum count
+     * @param[in] initialAvailable The initial count
+     */
+    Semaphore(uint32_t maxAvailable, uint32_t initialAvailable) : handle(createHandle(maxAvailable, initialAvailable)) {
+        assert(xPortInIsrContext() == pdFALSE);
+        assert(handle != nullptr);
+    }
+
+    /**
+     * Cannot be called from IRQ/ISR mode.
+     * @param[in] maxAvailable The maximum count
+     */
+    explicit Semaphore(uint32_t maxAvailable) : Semaphore(maxAvailable, maxAvailable) {}
+
+    /** Cannot be called from IRQ/ISR mode. */
+    ~Semaphore() override {
+        assert(xPortInIsrContext() == pdFALSE);
+    }
+
+    Semaphore(Semaphore& other) : handle(std::move(other.handle)) {}
+
+    /**
+     * Acquire the semaphore
+     * @param[in] timeout
+     * @return true on success
+     */
+    bool acquire(TickType_t timeout) const {
+        if (xPortInIsrContext() == pdTRUE) {
+            if (timeout != 0U) {
+                return false;
+            }
+
+            BaseType_t yield = pdFALSE;
+            if (xSemaphoreTakeFromISR(handle.get(), &yield) != pdPASS) {
+                return false;
+            }
+
+            portYIELD_FROM_ISR(yield);
+            return true;
+        } else {
+            return xSemaphoreTake(handle.get(), timeout) == pdPASS;
+        }
+    }
+
+    /**
+     * Release the semaphore
+     * @return true on success
+     */
+    bool release() const {
+        if (xPortInIsrContext() == pdTRUE) {
+            BaseType_t yield = pdFALSE;
+            if (xSemaphoreGiveFromISR(handle.get(), &yield) != pdTRUE) {
+                return false;
+            }
+
+            portYIELD_FROM_ISR(yield);
+            return true;
+        } else {
+            return xSemaphoreGive(handle.get()) == pdPASS;
+        }
+    }
+
+    /** @return the acquisition count */
+    uint32_t getAvailable() const {
+        if (xPortInIsrContext() == pdTRUE) {
+            return uxSemaphoreGetCountFromISR(handle.get());
+        } else {
+            return uxSemaphoreGetCount(handle.get());
+        }
+    }
+
+    // region Lock
+
+    /** Calls acquire() */
+    bool lock(TickType_t timeout) const override { return acquire(timeout); }
+
+    /** Calls release() */
+    void unlock() const override { release(); }
+
+    // endregion
+};
+
+} // namespace

TactilityFreeRtos/Include/Tactility/Thread.h

@@ -0,0 +1,263 @@
+#pragma once
+
+#include "freertoscompat/Task.h"
+#include "kernel/Kernel.h"
+#include "Mutex.h"
+
+#include <cassert>
+#include <functional>
+#include <memory>
+#include <string>
+
+#ifdef ESP_PLATFORM
+#include <esp_log.h>
+#endif
+
+namespace tt {
+
+class Thread final {
+
+    static constexpr size_t LOCAL_STORAGE_SELF_POINTER_INDEX = 0;
+
+public:
+
+    enum class State{
+        Stopped,
+        Starting,
+        Running,
+    };
+
+    /** ThreadPriority */
+    enum class Priority : UBaseType_t {
+        None = 0U,
+        Idle = 1U,
+        Lower = 2U,
+        Low = 3U,
+        Normal = 4U,
+        High = 5U,
+        Higher = 6U,
+        Critical = 7U
+    };
+
+    typedef std::function<int32_t()> MainFunction;
+
+    typedef void (*StateCallback)(State state, void* context);
+
+private:
+
+    static constexpr auto TAG = "Thread";
+
+    static_assert(static_cast<UBaseType_t>(Priority::Critical) < configMAX_PRIORITIES, "Highest thread priority is higher than max priority");
+
+    static void mainBody(void* context) {
+        assert(context != nullptr);
+        auto* thread = static_cast<Thread*>(context);
+
+        // Save Thread instance pointer to task local storage
+        assert(pvTaskGetThreadLocalStoragePointer(nullptr, LOCAL_STORAGE_SELF_POINTER_INDEX) == nullptr);
+        vTaskSetThreadLocalStoragePointer(nullptr, LOCAL_STORAGE_SELF_POINTER_INDEX, thread);
+
+#ifdef ESP_PLATFORM
+        ESP_LOGI(TAG, "Starting %s", thread->name.c_str());
+#endif
+        assert(thread->state == State::Starting);
+        thread->setState(State::Running);
+        thread->callbackResult = thread->mainFunction();
+        assert(thread->state == State::Running);
+        thread->setState(State::Stopped);
+#ifdef ESP_PLATFORM
+        ESP_LOGI(TAG, "Stopped %s", thread->name.c_str());
+#endif
+
+        vTaskSetThreadLocalStoragePointer(nullptr, 0, nullptr);
+        thread->taskHandle = nullptr;
+
+        vTaskDelete(nullptr);
+    }
+
+    TaskHandle_t taskHandle = nullptr;
+    State state = State::Stopped;
+    MainFunction mainFunction;
+    int32_t callbackResult = 0;
+    StateCallback stateCallback = nullptr;
+    void* stateCallbackContext = nullptr;
+    std::string name = {};
+    Priority priority = Priority::Normal;
+    Mutex mutex;
+    configSTACK_DEPTH_TYPE stackSize = 0;
+    portBASE_TYPE affinity = -1;
+
+    void setState(State newState) {
+        mutex.lock();
+        state = newState;
+        if (stateCallback) {
+            stateCallback(state, stateCallbackContext);
+        }
+        mutex.unlock();
+    }
+
+public:
+
+    Thread() = default;
+
+    Thread(
+        std::string name,
+        configSTACK_DEPTH_TYPE stackSize,
+        MainFunction function,
+        portBASE_TYPE affinity = -1
+    ) :
+        mainFunction(function),
+        name(std::move(name)),
+        stackSize(stackSize),
+        affinity(affinity)
+    {}
+
+    /** @warning If thread is running, you just call join() first */
+    ~Thread() {
+        assert(state == State::Stopped);
+        assert(taskHandle == nullptr);
+    }
+
+    void setName(std::string newName) {
+        mutex.lock();
+        assert(state == State::Stopped);
+        name = std::move(newName);
+        mutex.unlock();
+    }
+
+    void setStackSize(size_t newStackSize) {
+        mutex.lock();
+        assert(state == State::Stopped);
+        assert(newStackSize % 4 == 0);
+        stackSize = newStackSize;
+        mutex.unlock();
+    }
+
+    void setAffinity(portBASE_TYPE newAffinity) {
+        mutex.lock();
+        assert(state == State::Stopped);
+        affinity = newAffinity;
+        mutex.unlock();
+    }
+
+    void setMainFunction(MainFunction function) {
+        mutex.lock();
+        assert(state == State::Stopped);
+        mainFunction = function;
+        mutex.unlock();
+    }
+
+    void setPriority(Priority newPriority) {
+        mutex.lock();
+        assert(state == State::Stopped);
+        priority = newPriority;
+        mutex.unlock();
+    }
+
+    void setStateCallback(StateCallback callback, _Nullable void* callbackContext = nullptr) {
+        mutex.lock();
+        assert(state == State::Stopped);
+        stateCallback = callback;
+        stateCallbackContext = callbackContext;
+        mutex.unlock();
+    }
+
+    State getState() const {
+        auto lock = mutex.asScopedLock();
+        lock.lock();
+        return state;
+    }
+
+    void start() {
+        mutex.lock();
+        assert(mainFunction);
+        assert(state == State::Stopped);
+        assert(stackSize > 0 && stackSize < (UINT16_MAX * sizeof(StackType_t)));
+        mutex.unlock();
+
+        setState(State::Starting);
+
+        mutex.lock();
+        uint32_t stack_depth = stackSize / sizeof(StackType_t);
+        mutex.unlock();
+
+        BaseType_t result;
+        if (affinity != -1) {
+#ifdef ESP_PLATFORM
+            result = xTaskCreatePinnedToCore(
+                mainBody,
+                name.c_str(),
+                stack_depth,
+                this,
+                static_cast<UBaseType_t>(priority),
+                &taskHandle,
+                affinity
+            );
+#else
+            // Pinned tasks are not supported by current FreeRTOS platform - creating regular one
+            result = xTaskCreate(
+                mainBody,
+                name.c_str(),
+                stack_depth,
+                this,
+                static_cast<UBaseType_t>(priority),
+                &taskHandle
+            );
+#endif
+        } else {
+            result = xTaskCreate(
+                mainBody,
+                name.c_str(),
+                stack_depth,
+                this,
+                static_cast<UBaseType_t>(priority),
+                &taskHandle
+            );
+        }
+
+        assert(result == pdPASS);
+        assert(taskHandle != nullptr || getState() == State::Stopped);
+    }
+
+    /**
+     * @warning If this blocks forever, it might be because of the Thread, but it could also be because another task is blocking the CPU.
+     */
+    bool join(TickType_t timeout = kernel::MAX_TICKS, TickType_t pollInterval = 10) {
+        assert(getCurrent() != this);
+
+        TickType_t start_ticks = kernel::getTicks();
+        while (getTaskHandle()) {
+            kernel::delayTicks(pollInterval);
+            if (kernel::getTicks() - start_ticks > timeout) {
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    TaskHandle_t getTaskHandle() const { return taskHandle; }
+
+    int32_t getReturnCode() const {
+        assert(getState() == State::Stopped);
+        return callbackResult;
+    }
+
+    uint32_t getStackSpace() const {
+        if (xPortInIsrContext() == pdTRUE || getTaskHandle() == nullptr) {
+            return 0;
+        } else {
+            return uxTaskGetStackHighWaterMark(taskHandle) * sizeof(StackType_t);
+        }
+    }
+
+    static Thread* getCurrent() {
+        return static_cast<Thread*>(pvTaskGetThreadLocalStoragePointer(nullptr, LOCAL_STORAGE_SELF_POINTER_INDEX));
+    }
+};
+
+constexpr auto THREAD_PRIORITY_SERVICE = Thread::Priority::High;
+constexpr auto THREAD_PRIORITY_RENDER = Thread::Priority::Higher;
+constexpr auto THREAD_PRIORITY_ISR = Thread::Priority::Critical;
+
+} // namespace

TactilityFreeRtos/Include/Tactility/Timer.h

@@ -0,0 +1,151 @@
+#pragma once
+
+#include "Thread.h"
+#include "freertoscompat/Timers.h"
+
+#include <functional>
+#include <memory>
+
+namespace tt {
+
+/**
+ * Wrapper class for xTimer functions.
+ * @warning Cannot be called from an ISR context except for ::setPendingCallback()
+ */
+class Timer final {
+
+public:
+
+    enum class Type {
+        Once = 0, // Timer triggers once after time has passed
+        Periodic = 1 // Timer triggers repeatedly after time has passed
+    };
+
+    typedef std::function<void()> Callback;
+    typedef void (*PendingCallback)(void* context, uint32_t arg);
+
+private:
+
+    struct TimerHandleDeleter {
+        void operator()(TimerHandle_t handleToDelete) const {
+            xTimerDelete(handleToDelete, kernel::MAX_TICKS);
+        }
+    };
+
+    Callback callback;
+    std::unique_ptr<std::remove_pointer_t<TimerHandle_t>, TimerHandleDeleter> handle;
+
+    static TimerHandle_t createTimer(Type type, TickType_t ticks, void* timerId, TimerCallbackFunction_t callback) {
+        assert(timerId != nullptr);
+        assert(callback != nullptr);
+        BaseType_t auto_reload = (type == Type::Once) ? pdFALSE : pdTRUE;
+        return xTimerCreate(nullptr, ticks, auto_reload, timerId, callback);
+    }
+
+
+    static void onCallback(TimerHandle_t hTimer) {
+        auto* timer = static_cast<Timer*>(pvTimerGetTimerID(hTimer));
+        if (timer != nullptr) {
+            timer->callback();
+        }
+    }
+
+public:
+
+    /**
+     * @param[in] type The timer type
+     * @param[in] callback The callback function
+     */
+    Timer(Type type, TickType_t ticks, Callback callback) :
+        callback(callback),
+        handle(createTimer(type, ticks, this, onCallback))
+    {
+        assert(xPortInIsrContext() == pdFALSE);
+        assert(handle != nullptr);
+    }
+
+    ~Timer() {
+        assert(xPortInIsrContext() == pdFALSE);
+    }
+
+    /**
+     * Start the timer
+     * @return success result
+     */
+    bool start() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xTimerStart(handle.get(), kernel::MAX_TICKS) == pdPASS;
+    }
+
+    /** Stop the timer
+     * @warning If the timer was just triggered, the callback might still be going on after stop() was called
+     * @return success result
+     */
+    bool stop() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xTimerStop(handle.get(), kernel::MAX_TICKS) == pdPASS;
+    }
+
+    /**
+     * Set a new interval and reset the timer
+     * @param[in] interval The new timer interval
+     * @return success result
+     */
+    bool reset(TickType_t interval) const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xTimerChangePeriod(handle.get(), interval, kernel::MAX_TICKS) == pdPASS &&
+            xTimerReset(handle.get(), kernel::MAX_TICKS) == pdPASS;
+    }
+
+    /**
+     * Reset the timer
+     * @return success result
+     */
+    bool reset() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xTimerReset(handle.get(), kernel::MAX_TICKS) == pdPASS;
+    }
+
+    /** @return true when the timer is running */
+    bool isRunning() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xTimerIsTimerActive(handle.get()) == pdTRUE;
+    }
+
+    /** @return the expiry time in ticks */
+    TickType_t getExpiryTime() const {
+        assert(xPortInIsrContext() == pdFALSE);
+        return xTimerGetExpiryTime(handle.get());
+    }
+
+    /**
+     * Calls xTimerPendFunctionCall internally.
+     * @param[in] callback the function to call
+     * @param[in] callbackContext the first function argument
+     * @param[in] callbackArg the second function argument
+     * @param[in] timeout the function timeout (must set to 0 in ISR mode)
+     * @return true on success
+     */
+    bool setPendingCallback(PendingCallback newCallback, void* callbackContext, uint32_t callbackArg, TickType_t timeout) const {
+        if (xPortInIsrContext() == pdTRUE) {
+            assert(timeout == 0);
+            return xTimerPendFunctionCallFromISR(newCallback, callbackContext, callbackArg, nullptr) == pdPASS;
+        } else {
+            return xTimerPendFunctionCall(newCallback, callbackContext, callbackArg, timeout) == pdPASS;
+        }
+    }
+
+    /** Set callback priority
+     * @param[in] priority The priority
+     */
+    void setCallbackPriority(Thread::Priority priority) const {
+        assert(xPortInIsrContext() == pdFALSE);
+
+        TaskHandle_t task_handle = xTimerGetTimerDaemonTaskHandle();
+        assert(task_handle); // Don't call this method before timer task start
+
+        vTaskPrioritySet(task_handle, static_cast<UBaseType_t>(priority));
+    }
+};
+
+} // namespace

TactilityFreeRtos/Include/Tactility/freertoscompat/EventGroups.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#ifdef ESP_PLATFORM
+#include <freertos/FreeRTOS.h>
+#include <freertos/event_groups.h>
+#else
+#include <FreeRTOS.h>
+#include <event_groups.h>
+#endif
+

TactilityFreeRtos/Include/Tactility/freertoscompat/PortCompat.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include "RTOS.h"
+
+#ifndef ESP_PLATFORM
+#define xPortInIsrContext(x) (false)
+#endif

TactilityFreeRtos/Include/Tactility/freertoscompat/Queue.h

@@ -0,0 +1,8 @@
+#ifdef ESP_PLATFORM
+#include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
+#else
+#include <FreeRTOS.h>
+#include <queue.h>
+#endif
+

TactilityFreeRtos/Include/Tactility/freertoscompat/README.md

@@ -0,0 +1,3 @@
+Compatibility include files for FreeRTOS.
+Custom FreeRTOS from ESP-IDF prefixes paths with "freertos/",
+but this isn't the normal behaviour for the regular FreeRTOS project.

TactilityFreeRtos/Include/Tactility/freertoscompat/RTOS.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#ifdef ESP_PLATFORM
+#include <freertos/FreeRTOS.h>
+#else
+#include <FreeRTOS.h>
+#endif

TactilityFreeRtos/Include/Tactility/freertoscompat/Semaphore.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#ifdef ESP_PLATFORM
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#else
+#include <FreeRTOS.h>
+#include <semphr.h>
+#endif
+
+#include <cassert>
+
+struct SemaphoreHandleDeleter {
+  static void operator()(QueueHandle_t handleToDelete) {
+    assert(xPortInIsrContext() == pdFALSE);
+    vSemaphoreDelete(handleToDelete);
+  }
+};
+

TactilityFreeRtos/Include/Tactility/freertoscompat/Task.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#ifdef ESP_PLATFORM
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+#else
+#include <FreeRTOS.h>
+#include <task.h>
+#endif
+

TactilityFreeRtos/Include/Tactility/freertoscompat/Timers.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#ifdef ESP_PLATFORM
+#include <freertos/FreeRTOS.h>
+#include <freertos/timers.h>
+#else
+#include <FreeRTOS.h>
+#include <timers.h>
+#endif

TactilityFreeRtos/Include/Tactility/kernel/Kernel.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include "../freertoscompat/PortCompat.h"
+#include "../freertoscompat/Task.h"
+
+#ifdef ESP_PLATFORM
+#include <esp_timer.h>
+#include <rom/ets_sys.h>
+#else
+#include <sys/time.h>
+#include <cstdint>
+#include <unistd.h>
+#endif
+
+#include <cassert>
+
+namespace tt::kernel {
+
+constexpr TickType_t MAX_TICKS = ~static_cast<TickType_t>(0);
+
+/** @return the frequency at which the kernel task schedulers operate */
+constexpr uint32_t getTickFrequency() {
+    return configTICK_RATE_HZ;
+}
+
+/** @return the amount of ticks that has passed in the main kernel task */
+inline TickType_t getTicks() {
+    if (xPortInIsrContext() == pdTRUE) {
+        return xTaskGetTickCountFromISR();
+    } else {
+        return xTaskGetTickCount();
+    }
+}
+
+
+/** @return the amount of milliseconds that has passed in the main kernel tasks */
+inline size_t getMillis() {
+    return getTicks() * portTICK_PERIOD_MS;
+}
+
+/** @return the microseconds that have passed since boot */
+inline int64_t getMicrosSinceBoot() {
+#ifdef ESP_PLATFORM
+    return esp_timer_get_time();
+#else
+    timeval tv;
+    gettimeofday(&tv, nullptr);
+    return 1000000 * tv.tv_sec + tv.tv_usec;
+#endif
+}
+
+/** Convert seconds to ticks */
+inline TickType_t secondsToTicks(uint32_t seconds) {
+    return static_cast<uint64_t>(seconds) * 1000U / portTICK_PERIOD_MS;
+}
+
+/** Convert milliseconds to ticks */
+inline TickType_t millisToTicks(uint32_t milliSeconds) {
+#if configTICK_RATE_HZ == 1000
+    return static_cast<TickType_t>(milliSeconds);
+#else
+    return static_cast<TickType_t>(((float)configTICK_RATE_HZ) / 1000.0f * (float)milliSeconds);
+#endif
+}
+
+/**
+ * Delay the current task for the specified amount of ticks
+ * @warning Does not work in ISR context
+ */
+inline void delayTicks(TickType_t ticks) {
+    assert(xPortInIsrContext() == pdFALSE);
+    if (ticks == 0U) {
+        taskYIELD();
+    } else {
+        vTaskDelay(ticks);
+    }
+}
+
+/**
+ * Delay the current task for the specified amount of milliseconds
+ * @warning Does not work in ISR context
+ */
+inline void delayMillis(uint32_t milliSeconds) {
+    delayTicks(millisToTicks(milliSeconds));
+}
+
+/**
+ * Stall the currently active CPU core for the specified amount of microseconds.
+ * This does not allow other tasks to run on the stalled CPU core.
+ */
+inline void delayMicros(uint32_t microseconds) {
+#ifdef ESP_PLATFORM
+    ets_delay_us(microseconds);
+#else
+    usleep(microseconds);
+#endif
+}
+
+} // namespace