C++ conversion (#80)

Converted project to C++

TactilityCore/Source/ApiLock.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "EventFlag.h"
+
+typedef tt::EventFlag* ApiLock;
+
+#define TT_API_LOCK_EVENT (1U << 0)
+
+#define tt_api_lock_alloc_locked() tt::event_flag_alloc()
+
+#define tt_api_lock_wait_unlock(_lock) \
+    tt::event_flag_wait(_lock, TT_API_LOCK_EVENT, tt::TtFlagWaitAny, tt::TtWaitForever)
+
+#define tt_api_lock_free(_lock) tt::event_flag_free(_lock)
+
+#define tt_api_lock_unlock(_lock) tt::event_flag_set(_lock, TT_API_LOCK_EVENT)
+
+#define tt_api_lock_wait_unlock_and_free(_lock) \
+    tt_api_lock_wait_unlock(_lock);          \
+    tt_api_lock_free(_lock);

TactilityCore/Source/Bundle.cpp

@@ -0,0 +1,83 @@
+#include "Bundle.h"
+
+namespace tt {
+
+bool Bundle::getBool(const std::string& key) const {
+    return this->entries.find(key)->second.value_bool;
+}
+
+int32_t Bundle::getInt32(const std::string& key) const {
+    return this->entries.find(key)->second.value_int32;
+}
+
+std::string Bundle::getString(const std::string& key) const {
+    return this->entries.find(key)->second.value_string;
+}
+
+bool Bundle::hasBool(const std::string& key) const {
+    auto entry = this->entries.find(key);
+    return entry != std::end(this->entries) && entry->second.type == TypeBool;
+}
+
+bool Bundle::hasInt32(const std::string& key) const {
+    auto entry = this->entries.find(key);
+    return entry != std::end(this->entries) && entry->second.type == TypeInt32;
+}
+
+bool Bundle::hasString(const std::string& key) const {
+    auto entry = this->entries.find(key);
+    return entry != std::end(this->entries) && entry->second.type == TypeString;
+}
+
+bool Bundle::optBool(const std::string& key, bool& out) const {
+    auto entry = this->entries.find(key);
+    if (entry != std::end(this->entries) && entry->second.type == TypeBool) {
+        out = entry->second.value_bool;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool Bundle::optInt32(const std::string& key, int32_t& out) const {
+    auto entry = this->entries.find(key);
+    if (entry != std::end(this->entries) && entry->second.type == TypeInt32) {
+        out = entry->second.value_int32;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool Bundle::optString(const std::string& key, std::string& out) const {
+    auto entry = this->entries.find(key);
+    if (entry != std::end(this->entries) && entry->second.type == TypeString) {
+        out = entry->second.value_string;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void Bundle::putBool(const std::string& key, bool value) {
+    this->entries[key] = {
+        .type = TypeBool,
+        .value_bool = value
+    };
+}
+
+void Bundle::putInt32(const std::string& key, int32_t value) {
+    this->entries[key] = {
+        .type = TypeInt32,
+        .value_int32 = value
+    };
+}
+
+void Bundle::putString(const std::string& key, const std::string& value) {
+    this->entries[key] = {
+        .type = TypeString,
+        .value_string = value
+    };
+}
+
+} // namespace

TactilityCore/Source/Bundle.h

@@ -0,0 +1,63 @@
+/**
+ * @brief key-value storage for general purpose.
+ * Maps strings on a fixed set of data types.
+ */
+#pragma once
+
+#include <cstdint>
+#include <cstdio>
+#include <string>
+#include <unordered_map>
+
+namespace tt {
+
+class Bundle {
+
+private:
+
+    typedef uint32_t Hash;
+
+    typedef enum {
+        TypeBool,
+        TypeInt32,
+        TypeString,
+    } Type;
+
+    typedef struct {
+        const char* key;
+        Type type;
+        union {
+            bool value_bool;
+            int32_t value_int32;
+        };
+        std::string value_string;
+    } Value;
+
+    std::unordered_map<std::string, Value> entries;
+
+public:
+
+    Bundle() = default;
+
+    Bundle(const Bundle& bundle) {
+        this->entries = bundle.entries;
+    }
+
+    bool getBool(const std::string& key) const;
+    int32_t getInt32(const std::string& key) const;
+    std::string getString(const std::string& key) const;
+
+    bool hasBool(const std::string& key) const;
+    bool hasInt32(const std::string& key) const;
+    bool hasString(const std::string& key) const;
+
+    bool optBool(const std::string& key, bool& out) const;
+    bool optInt32(const std::string& key, int32_t& out) const;
+    bool optString(const std::string& key, std::string& out) const;
+
+    void putBool(const std::string& key, bool value);
+    void putInt32(const std::string& key, int32_t value);
+    void putString(const std::string& key, const std::string& value);
+};
+
+} // namespace

TactilityCore/Source/Check.cpp

@@ -0,0 +1,44 @@
+#include "Check.h"
+
+#include "CoreDefines.h"
+#include "Log.h"
+
+#ifdef ESP_TARGET
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#else
+#include "FreeRTOS.h"
+#include "task.h"
+#endif
+
+#define TAG "kernel"
+
+static void tt_print_memory_info() {
+#ifdef ESP_PLATFORM
+    TT_LOG_E(TAG, "default caps:");
+    TT_LOG_E(TAG, "  total: %u", heap_caps_get_total_size(MALLOC_CAP_DEFAULT));
+    TT_LOG_E(TAG, "  free: %u", heap_caps_get_free_size(MALLOC_CAP_DEFAULT));
+    TT_LOG_E(TAG, "  min free: %u", heap_caps_get_minimum_free_size(MALLOC_CAP_DEFAULT));
+    TT_LOG_E(TAG, "internal caps:");
+    TT_LOG_E(TAG, "  total: %u", heap_caps_get_total_size(MALLOC_CAP_INTERNAL));
+    TT_LOG_E(TAG, "  free: %u", heap_caps_get_free_size(MALLOC_CAP_INTERNAL));
+    TT_LOG_E(TAG, "  min free: %u", heap_caps_get_minimum_free_size(MALLOC_CAP_INTERNAL));
+#endif
+}
+
+static void tt_print_task_info() {
+    const char* name = pcTaskGetName(nullptr);
+    const char* safe_name = name ? name : "main";
+    TT_LOG_E(TAG, "Task: %s", safe_name);
+    TT_LOG_E(TAG, "Stack watermark: %u", uxTaskGetStackHighWaterMark(NULL) * 4);
+}
+
+TT_NORETURN void tt_crash_implementation() {
+    tt_print_task_info();
+    tt_print_memory_info();
+    // TODO: Add breakpoint when debugger is attached.
+#ifdef ESP_TARGET
+    esp_system_abort("System halted. Connect debugger for more info.");
+#endif
+    __builtin_unreachable();
+}

TactilityCore/Source/Check.h

@@ -0,0 +1,90 @@
+/**
+ * @file check.h
+ * 
+ * Tactility crash and assert functions.
+ * 
+ * The main problem with crashing is that you can't do anything without disturbing registers,
+ * and if you disturb registers, you won't be able to see the correct register values in the debugger.
+ * 
+ * Current solution works around it by passing the message through r12 and doing some magic with registers in crash function.
+ * r0-r10 are stored in the ram2 on crash routine start and restored at the end.
+ * The only register that is going to be lost is r11.
+ * 
+ */
+#pragma once
+
+#include "Log.h"
+#include <cassert>
+
+#define TT_NORETURN [[noreturn]]
+
+/** Crash system */
+TT_NORETURN void tt_crash_implementation();
+
+/** Crash system with message. */
+#define tt_crash(message)                                                                    \
+    do {                                                                                       \
+        TT_LOG_E("crash", "%s\n\tat %s:%d", ((message) ? (message) : ""), __FILE__, __LINE__); \
+        tt_crash_implementation();                                                             \
+    } while (0)
+
+/** Halt system
+ *
+ * @param      optional  message (const char*)
+ */
+#define tt_halt(...) M_APPLY(__tt_halt, M_IF_EMPTY(__VA_ARGS__)((NULL), (__VA_ARGS__)))
+
+/** Check condition and crash if check failed */
+#define tt_check_internal(__e, __m)        \
+    do {                                   \
+        if (!(__e)) {                      \
+            TT_LOG_E("check", "%s", #__e); \
+            if (__m) {                     \
+                tt_crash_internal(#__m);   \
+            } else {                       \
+                tt_crash_internal("");     \
+            }                              \
+        }                                  \
+    } while (0)
+
+/** Check condition and crash if failed
+ *
+ * @param      condition to check
+ * @param      optional  message (const char*)
+ */
+
+#ifdef NDEBUG
+#define tt_check(x, ...) if (!(x)) { TT_LOG_E("check", "check failed: %s", #x); }
+#else
+#define tt_check(x, ...) assert(x)
+#endif
+
+/** Only in debug build: Assert condition and crash if assert failed  */
+#ifdef TT_DEBUG
+#define tt_assert_internal(__e, __m)               \
+    do {                                    \
+        if (!(__e)) {                       \
+            TT_LOG_E("assert", "%s", #__e); \
+            if (__m) {                      \
+                __tt_crash(#__m);           \
+            } else {                        \
+                __tt_crash("");             \
+            }                               \
+        }                                   \
+    } while (0)
+#else
+#define __tt_assert(__e, __m) \
+    do {                      \
+        ((void)(__e));        \
+        ((void)(__m));        \
+    } while (0)
+#endif
+
+/** Assert condition and crash if failed
+ *
+ * @warning    only will do check if firmware compiled in debug mode
+ *
+ * @param      condition to check
+ * @param      optional  message (const char*)
+ */
+#define tt_assert(expression) assert(expression)

TactilityCore/Source/CoreDefines.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "CoreExtraDefines.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/portmacro.h"
+#else
+#include "portmacro.h"
+#endif
+
+#define TT_RETURNS_NONNULL __attribute__((returns_nonnull))
+
+#define TT_WARN_UNUSED __attribute__((warn_unused_result))
+
+#define TT_UNUSED __attribute__((unused))
+
+#define TT_WEAK __attribute__((weak))
+
+#define TT_PACKED __attribute__((packed))
+
+#define TT_PLACE_IN_SECTION(x) __attribute__((section(x)))
+
+#define TT_ALIGN(n) __attribute__((aligned(n)))
+
+// Used by portENABLE_INTERRUPTS and portDISABLE_INTERRUPTS?
+#ifdef ESP_TARGET
+#define TT_IS_IRQ_MODE() (xPortInIsrContext() == pdTRUE)
+#else
+#define TT_IS_IRQ_MODE() false
+#endif
+
+#define TT_IS_ISR() (TT_IS_IRQ_MODE())
+
+#define TT_CHECK_RETURN __attribute__((__warn_unused_result__))

TactilityCore/Source/CoreExtraDefines.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#define TT_MAX(a, b)            \
+    ({                          \
+        __typeof__(a) _a = (a); \
+        __typeof__(b) _b = (b); \
+        _a > _b ? _a : _b;      \
+    })
+
+#define TT_MIN(a, b)            \
+    ({                          \
+        __typeof__(a) _a = (a); \
+        __typeof__(b) _b = (b); \
+        _a < _b ? _a : _b;      \
+    })
+
+#define TT_ABS(a) ({ (a) < 0 ? -(a) : (a); })
+
+#define TT_ROUND_UP_TO(a, b)    \
+    ({                          \
+        __typeof__(a) _a = (a); \
+        __typeof__(b) _b = (b); \
+        _a / _b + !!(_a % _b);  \
+    })
+
+#define TT_CLAMP(x, upper, lower) (TT_MIN(upper, TT_MAX(x, lower)))
+
+#define TT_COUNT_OF(x) (sizeof(x) / sizeof(x[0]))
+
+#define TT_SWAP(x, y)       \
+    do {                    \
+        typeof(x) SWAP = x; \
+        x = y;              \
+        y = SWAP;           \
+    } while (0)
+
+#define TT_STRINGIFY(x) #x
+
+#define TT_TOSTRING(x) TT_STRINGIFY(x)
+
+#define TT_CONCATENATE(a, b) CONCATENATE_(a, b)
+#define TT_CONCATENATE_(a, b) a##b
+
+#define TT_REVERSE_BYTES_U32(x)                                                           \
+    ((((x) & 0x000000FF) << 24) | (((x) & 0x0000FF00) << 8) | (((x) & 0x00FF0000) >> 8) | \
+     (((x) & 0xFF000000) >> 24))
+
+#define TT_BIT(x, n) (((x) >> (n)) & 1)
+
+#define TT_BIT_SET(x, n)        \
+    ({                          \
+        __typeof__(x) _x = (1); \
+        (x) |= (_x << (n));     \
+    })
+
+#define TT_BIT_CLEAR(x, n)      \
+    ({                          \
+        __typeof__(x) _x = (1); \
+        (x) &= ~(_x << (n));    \
+    })
+
+#define TT_SW_MEMBARRIER() asm volatile("" : : : "memory")

TactilityCore/Source/CoreTypes.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <cstdint>
+#include "TactilityCoreConfig.h"
+
+namespace tt {
+
+typedef enum {
+    TtWaitForever = 0xFFFFFFFFU,
+} TtWait;
+
+typedef enum {
+    TtFlagWaitAny = 0x00000000U, ///< Wait for any flag (default).
+    TtFlagWaitAll = 0x00000001U, ///< Wait for all flags.
+    TtFlagNoClear = 0x00000002U, ///< Do not clear flags which have been specified to wait for.
+
+    TtFlagError = 0x80000000U,          ///< Error indicator.
+    TtFlagErrorUnknown = 0xFFFFFFFFU,   ///< TtStatusError (-1).
+    TtFlagErrorTimeout = 0xFFFFFFFEU,   ///< TtStatusErrorTimeout (-2).
+    TtFlagErrorResource = 0xFFFFFFFDU,  ///< TtStatusErrorResource (-3).
+    TtFlagErrorParameter = 0xFFFFFFFCU, ///< TtStatusErrorParameter (-4).
+    TtFlagErrorISR = 0xFFFFFFFAU,       ///< TtStatusErrorISR (-6).
+} TtFlag;
+
+typedef enum {
+    TtStatusOk = 0, ///< Operation completed successfully.
+    TtStatusError =
+        -1,                        ///< Unspecified RTOS error: run-time error but no other error message fits.
+    TtStatusErrorTimeout = -2,   ///< Operation not completed within the timeout period.
+    TtStatusErrorResource = -3,  ///< Resource not available.
+    TtStatusErrorParameter = -4, ///< Parameter error.
+    TtStatusErrorNoMemory =
+        -5, ///< System is out of memory: it was impossible to allocate or reserve memory for the operation.
+    TtStatusErrorISR =
+        -6,                         ///< Not allowed in ISR context: the function cannot be called from interrupt service routines.
+    TtStatusReserved = 0x7FFFFFFF ///< Prevents enum down-size compiler optimization.
+} TtStatus;
+
+} // namespace

TactilityCore/Source/Critical.cpp

@@ -0,0 +1,51 @@
+#include "Critical.h"
+#include "CoreDefines.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "freertos/portmacro.h"
+#else
+#include "FreeRTOS.h"
+#include "task.h"
+#include "portmacro.h"
+#endif
+
+#ifdef ESP_PLATFORM
+static portMUX_TYPE critical_mutex;
+#define TT_ENTER_CRITICAL() taskENTER_CRITICAL(&critical_mutex)
+#else
+#define TT_ENTER_CRITICAL() taskENTER_CRITICAL()
+#endif
+
+namespace tt::critical {
+
+TtCriticalInfo enter() {
+    TtCriticalInfo info;
+
+    info.isrm = 0;
+    info.from_isr = TT_IS_ISR();
+    info.kernel_running = (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING);
+
+    if (info.from_isr) {
+        info.isrm = taskENTER_CRITICAL_FROM_ISR();
+    } else if (info.kernel_running) {
+        TT_ENTER_CRITICAL();
+    } else {
+        portDISABLE_INTERRUPTS();
+    }
+
+    return info;
+}
+
+void exit(TtCriticalInfo info) {
+    if (info.from_isr) {
+        taskEXIT_CRITICAL_FROM_ISR(info.isrm);
+    } else if (info.kernel_running) {
+        TT_ENTER_CRITICAL();
+    } else {
+        portENABLE_INTERRUPTS();
+    }
+}
+
+}

TactilityCore/Source/Critical.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <cstdint>
+
+#ifndef TT_CRITICAL_ENTER
+#define TT_CRITICAL_ENTER() __TtCriticalInfo __tt_critical_info = __tt_critical_enter();
+#endif
+
+#ifndef TT_CRITICAL_EXIT
+#define TT_CRITICAL_EXIT() __tt_critical_exit(__tt_critical_info);
+#endif
+
+namespace tt::critical {
+
+typedef struct {
+    uint32_t isrm;
+    bool from_isr;
+    bool kernel_running;
+} TtCriticalInfo;
+
+TtCriticalInfo enter();
+
+void exit(TtCriticalInfo info);
+
+} // namespace

TactilityCore/Source/Crypt.cpp

@@ -0,0 +1,190 @@
+#include "Crypt.h"
+
+#include "Check.h"
+#include "Log.h"
+#include "mbedtls/aes.h"
+#include <cstring>
+
+#ifdef ESP_PLATFORM
+#include "esp_cpu.h"
+#include "esp_mac.h"
+#include "nvs_flash.h"
+#endif
+
+namespace tt::crypt {
+
+#define TAG "secure"
+#define TT_NVS_NAMESPACE "tt_secure"
+
+#ifdef ESP_PLATFORM
+/**
+ * Get a key based on hardware parameters.
+ * @param[out] key the output key
+ */
+static void get_hardware_key(uint8_t key[32]) {
+    uint8_t mac[8];
+    // MAC can be 6 or 8 bytes
+    size_t mac_length = esp_mac_addr_len_get(ESP_MAC_EFUSE_FACTORY);
+    TT_LOG_I(TAG, "Using MAC with length %u", mac_length);
+    tt_check(mac_length <= 8);
+    ESP_ERROR_CHECK(esp_read_mac(mac, ESP_MAC_EFUSE_FACTORY));
+
+    // Fill buffer with repeating MAC
+    for (size_t i = 0; i < 32; ++i) {
+        key[i] = mac[i % mac_length];
+    }
+}
+#endif
+
+#ifdef ESP_PLATFORM
+/**
+ * The key is built up as follows:
+ * - Fetch 32 bytes from NVS storage and store as key data
+ * - Fetch 6-8 MAC bytes and overwrite the first 6-8 bytes of the key with this info
+ *
+ * When flash encryption is disabled:
+ *   Without the MAC data, an attack would look like this:
+ *   - Retrieve all the partitions from the ESP32
+ *   - Read the key from NVS flash
+ *   - Use the key to decrypt
+ *   With the MAC data added, an attacker would have to do much more:
+ *   - Retrieve all the partitions from the ESP32 (copy app)
+ *   - Upload custom app to retrieve internal MAC
+ *   - Read the key from NVS flash
+ *   - Re-flash original app and combine it with the MAC
+ *   - Use the key to decrypt
+ *   - Re-flash the device with original firmware.
+ *
+ * Adding the MAC doesn't add a lot of extra security, but I think it's worth it.
+ *
+ * @param[out] key the output key
+ */
+static void get_nvs_key(uint8_t key[32]) {
+    nvs_handle_t handle;
+    esp_err_t result = nvs_open(TT_NVS_NAMESPACE, NVS_READWRITE, &handle);
+
+    if (result != ESP_OK) {
+        TT_LOG_E(TAG, "Failed to get key from NVS (%s)", esp_err_to_name(result));
+        tt_crash("NVS error");
+    }
+
+    size_t length = 32;
+    if (nvs_get_blob(handle, "key", key, &length) == ESP_OK) {
+        TT_LOG_I(TAG, "Fetched key from NVS (%d bytes)", length);
+        tt_check(length == 32);
+    } else {
+        // TODO: Improved randomness
+        esp_cpu_cycle_count_t cycle_count = esp_cpu_get_cycle_count();
+        auto seed = cycle_count;
+        srand(seed);
+        for (int i = 0; i < 32; ++i) {
+            key[i] = (uint8_t)(rand());
+        }
+        ESP_ERROR_CHECK(nvs_set_blob(handle, "key", key, 32));
+        TT_LOG_I(TAG, "Stored new key in NVS");
+    }
+
+    nvs_close(handle);
+}
+#endif
+
+/**
+ * Performs XOR on 2 memory regions and stores it in a third
+ * @param[in] in_left input buffer for XOR
+ * @param[in] in_right second input buffer for XOR
+ * @param[out] out output buffer for result of XOR
+ * @param[in] length data length (all buffers must be at least this size)
+ */
+static void xor_key(const uint8_t* in_left, const uint8_t* in_right, uint8_t* out, size_t length) {
+    for (int i = 0; i < length; ++i) {
+        out[i] = in_left[i] ^ in_right[i];
+    }
+}
+
+/**
+ * Combines a stored key and a hardware key into a single reliable key value.
+ * @param[out] key the key output
+ */
+static void get_key(uint8_t key[32]) {
+#if !defined(CONFIG_SECURE_BOOT) || !defined(CONFIG_SECURE_FLASH_ENC_ENABLED)
+    TT_LOG_W(TAG, "Using tt_secure_* code with secure boot and/or flash encryption disabled.");
+    TT_LOG_W(TAG, "An attacker with physical access to your ESP32 can decrypt your secure data.");
+#endif
+
+    uint8_t hardware_key[32];
+    uint8_t nvs_key[32];
+
+#ifdef ESP_PLATFORM
+    get_hardware_key(hardware_key);
+    get_nvs_key(nvs_key);
+    xor_key(hardware_key, nvs_key, key, 32);
+#else
+    TT_LOG_W(TAG, "Using unsafe key for debugging purposes.");
+    memset(key, 0, 32);
+#endif
+}
+
+void get_iv_from_data(const void* data, size_t data_length, uint8_t iv[16]) {
+    memset((void*)iv, 0, 16);
+    uint8_t* data_bytes = (uint8_t*)data;
+    for (int i = 0; i < data_length; ++i) {
+        size_t safe_index = i % 16;
+        iv[safe_index] %= data_bytes[i];
+    }
+}
+
+void get_iv_from_string(const char* input, uint8_t iv[16]) {
+    get_iv_from_data((const void*)input, strlen(input), iv);
+}
+
+static int aes256_crypt_cbc(
+    const uint8_t key[32],
+    int mode,
+    size_t length,
+    unsigned char iv[16],
+    const unsigned char* input,
+    unsigned char* output
+) {
+    tt_check(key && iv && input && output);
+
+    if ((length % 16) || (length == 0)) {
+        return -1; // TODO: Proper error code from mbed lib?
+    }
+
+    mbedtls_aes_context master;
+    mbedtls_aes_init(&master);
+    if (mode == MBEDTLS_AES_ENCRYPT) {
+        mbedtls_aes_setkey_enc(&master, key, 256);
+    } else {
+        mbedtls_aes_setkey_dec(&master, key, 256);
+    }
+    int result = mbedtls_aes_crypt_cbc(&master, mode, length, iv, input, output);
+    mbedtls_aes_free(&master);
+    return result;
+}
+
+int encrypt(const uint8_t iv[16], uint8_t* in_data, uint8_t* out_data, size_t length) {
+    tt_check(length % 16 == 0, "Length is not a multiple of 16 bytes (for AES 256");
+    uint8_t key[32];
+    get_key(key);
+
+    // TODO: Is this still needed after switching to regular AES functions?
+    uint8_t iv_copy[16];
+    memcpy(iv_copy, iv, sizeof(iv_copy));
+
+    return aes256_crypt_cbc(key, MBEDTLS_AES_ENCRYPT, length, iv_copy, in_data, out_data);
+}
+
+int decrypt(const uint8_t iv[16], uint8_t* in_data, uint8_t* out_data, size_t length) {
+    tt_check(length % 16 == 0, "Length is not a multiple of 16 bytes (for AES 256");
+    uint8_t key[32];
+    get_key(key);
+
+    // TODO: Is this still needed after switching to regular AES functions?
+    uint8_t iv_copy[16];
+    memcpy(iv_copy, iv, sizeof(iv_copy));
+
+    return aes256_crypt_cbc(key, MBEDTLS_AES_DECRYPT, length, iv_copy, in_data, out_data);
+}
+
+} // namespace

TactilityCore/Source/Crypt.h

@@ -0,0 +1,69 @@
+/** @file secure.h
+ *
+ * @brief Hardware-bound encryption methods.
+ * @warning Enable secure boot and flash encryption to increase security.
+ *
+ * Offers AES 256 CBC encryption with built-in key.
+ * The key is built from data including:
+ *  - the internal factory MAC address
+ *  - random data stored in NVS
+ *
+ * It's important to use flash encryption to avoid an attacker to get
+ * access to your encrypted data. If flash encryption is disabled,
+ * someone can fetch the key from the partitions.
+ *
+ * See:
+ * https://docs.espressif.com/projects/esp-idf/en/latest/esp32/security/secure-boot-v2.html
+ * https://docs.espressif.com/projects/esp-idf/en/latest/esp32/security/flash-encryption.html
+ */
+#pragma once
+
+#include <cstdio>
+#include <cstdint>
+
+namespace tt::crypt {
+
+/**
+ * @brief Fills the IV with zeros and then creates an IV based on the input data.
+ * @param data input data
+ * @param data_length input data length
+ * @param iv output IV
+ */
+void get_iv_from_data(const void* data, size_t data_length, uint8_t iv[16]);
+
+/**
+* @brief Fills the IV with zeros and then creates an IV based on the input data.
+ * @param input input text
+ * @param iv output IV
+ */
+void get_iv_from_string(const char* input, uint8_t iv[16]);
+
+/**
+ * @brief Encrypt data.
+ *
+ * Important: Use flash encryption to increase security.
+ * Important: input and output data must be aligned to 16 bytes.
+ *
+ * @param iv the AES IV
+ * @param data_in input data
+ * @param data_out output data
+ * @param length data length, a multiple of 16
+ * @return the result of esp_aes_crypt_cbc() (MBEDTLS_ERR_*)
+ */
+int encrypt(const uint8_t iv[16], uint8_t* in_data, uint8_t* out_data, size_t length);
+
+/**
+ * @brief Decrypt data.
+ *
+ * Important: Use flash encryption to increase security.
+ * Important: input and output data must be aligned to 16 bytes.
+ *
+ * @param iv AES IV
+ * @param data_in input data
+ * @param data_out output data
+ * @param length data length, a multiple of 16
+ * @return the result of esp_aes_crypt_cbc() (MBEDTLS_ERR_*)
+ */
+int decrypt(const uint8_t iv[16], uint8_t* in_data, uint8_t* out_data, size_t length);
+
+} // namespace

TactilityCore/Source/Dispatcher.cpp

@@ -0,0 +1,39 @@
+#include "Dispatcher.h"
+
+namespace tt {
+
+Dispatcher::Dispatcher(size_t queueLimit) :
+    queue(queueLimit, sizeof(DispatcherMessage)),
+    mutex(MutexTypeNormal),
+    buffer({ .callback = nullptr, .context = nullptr }) { }
+
+Dispatcher::~Dispatcher() {
+    queue.reset();
+    // Wait for Mutex usage
+    mutex.acquire(TtWaitForever);
+    mutex.release();
+}
+
+void Dispatcher::dispatch(Callback callback, void* context) {
+    DispatcherMessage message = {
+        .callback = callback,
+        .context = context
+    };
+    mutex.acquire(TtWaitForever);
+    queue.put(&message, TtWaitForever);
+    mutex.release();
+}
+
+bool Dispatcher::consume(uint32_t timeout_ticks) {
+    mutex.acquire(TtWaitForever);
+    if (queue.get(&buffer, timeout_ticks) == TtStatusOk) {
+        buffer.callback(buffer.context);
+        mutex.release();
+        return true;
+    } else {
+        mutex.release();
+        return false;
+    }
+}
+
+} // namespace

TactilityCore/Source/Dispatcher.h

@@ -0,0 +1,35 @@
+/**
+* @file Dispatcher.h
+*
+* Dispatcher is a thread-safe code execution queue.
+*/
+#pragma once
+
+#include "MessageQueue.h"
+#include "Mutex.h"
+
+namespace tt {
+
+typedef void (*Callback)(void* data);
+
+class Dispatcher {
+private:
+    typedef struct {
+        Callback callback;
+        void* context;
+    } DispatcherMessage;
+
+    MessageQueue queue;
+    Mutex mutex;
+    DispatcherMessage buffer; // Buffer for consuming a message
+
+public:
+
+    explicit Dispatcher(size_t queueLimit = 8);
+    ~Dispatcher();
+
+    void dispatch(Callback callback, void* context);
+    bool consume(uint32_t timeout_ticks);
+};
+
+} // namespace

TactilityCore/Source/EventFlag.cpp

@@ -0,0 +1,158 @@
+#include "EventFlag.h"
+
+#include "Check.h"
+#include "CoreDefines.h"
+
+#ifdef ESP_TARGET
+#include "freertos/FreeRTOS.h"
+#include "freertos/event_groups.h"
+#include "freertos/portmacro.h"
+#else
+#include "FreeRTOS.h"
+#include "event_groups.h"
+#include "portmacro.h"
+#endif
+
+#define TT_EVENT_FLAG_MAX_BITS_EVENT_GROUPS 24U
+#define TT_EVENT_FLAG_INVALID_BITS (~((1UL << TT_EVENT_FLAG_MAX_BITS_EVENT_GROUPS) - 1U))
+
+namespace tt {
+
+EventFlag* event_flag_alloc() {
+    tt_assert(!TT_IS_IRQ_MODE());
+
+    EventGroupHandle_t handle = xEventGroupCreate();
+    tt_check(handle);
+
+    return static_cast<EventFlag*>(handle);
+}
+
+void event_flag_free(EventFlag* instance) {
+    tt_assert(!TT_IS_IRQ_MODE());
+    vEventGroupDelete((EventGroupHandle_t)instance);
+}
+
+uint32_t event_flag_set(EventFlag* instance, uint32_t flags) {
+    tt_assert(instance);
+    tt_assert((flags & TT_EVENT_FLAG_INVALID_BITS) == 0U);
+
+    auto hEventGroup = static_cast<EventGroupHandle_t>(instance);
+    uint32_t rflags;
+    BaseType_t yield;
+
+    if (TT_IS_IRQ_MODE()) {
+        yield = pdFALSE;
+        if (xEventGroupSetBitsFromISR(hEventGroup, (EventBits_t)flags, &yield) == pdFAIL) {
+            rflags = (uint32_t)TtFlagErrorResource;
+        } else {
+            rflags = flags;
+            portYIELD_FROM_ISR(yield);
+        }
+    } else {
+        rflags = xEventGroupSetBits(hEventGroup, (EventBits_t)flags);
+    }
+
+    /* Return event flags after setting */
+    return (rflags);
+}
+
+uint32_t event_flag_clear(EventFlag* instance, uint32_t flags) {
+    tt_assert(instance);
+    tt_assert((flags & TT_EVENT_FLAG_INVALID_BITS) == 0U);
+
+    auto hEventGroup = static_cast<EventGroupHandle_t>(instance);
+    uint32_t rflags;
+
+    if (TT_IS_IRQ_MODE()) {
+        rflags = xEventGroupGetBitsFromISR(hEventGroup);
+
+        if (xEventGroupClearBitsFromISR(hEventGroup, (EventBits_t)flags) == pdFAIL) {
+            rflags = (uint32_t)TtStatusErrorResource;
+        } else {
+            /* xEventGroupClearBitsFromISR only registers clear operation in the timer command queue. */
+            /* Yield is required here otherwise clear operation might not execute in the right order. */
+            /* See https://github.com/FreeRTOS/FreeRTOS-Kernel/issues/93 for more info.               */
+            portYIELD_FROM_ISR(pdTRUE);
+        }
+    } else {
+        rflags = xEventGroupClearBits(hEventGroup, (EventBits_t)flags);
+    }
+
+    /* Return event flags before clearing */
+    return (rflags);
+}
+
+uint32_t event_flag_get(EventFlag* instance) {
+    tt_assert(instance);
+
+    auto hEventGroup = static_cast<EventGroupHandle_t>(instance);
+    uint32_t rflags;
+
+    if (TT_IS_IRQ_MODE()) {
+        rflags = xEventGroupGetBitsFromISR(hEventGroup);
+    } else {
+        rflags = xEventGroupGetBits(hEventGroup);
+    }
+
+    /* Return current event flags */
+    return (rflags);
+}
+
+uint32_t event_flag_wait(
+    EventFlag* instance,
+    uint32_t flags,
+    uint32_t options,
+    uint32_t timeout
+) {
+    tt_assert(!TT_IS_IRQ_MODE());
+    tt_assert(instance);
+    tt_assert((flags & TT_EVENT_FLAG_INVALID_BITS) == 0U);
+
+    auto hEventGroup = static_cast<EventGroupHandle_t>(instance);
+    BaseType_t wait_all;
+    BaseType_t exit_clr;
+    uint32_t rflags;
+
+    if (options & TtFlagWaitAll) {
+        wait_all = pdTRUE;
+    } else {
+        wait_all = pdFAIL;
+    }
+
+    if (options & TtFlagNoClear) {
+        exit_clr = pdFAIL;
+    } else {
+        exit_clr = pdTRUE;
+    }
+
+    rflags = xEventGroupWaitBits(
+        hEventGroup,
+        (EventBits_t)flags,
+        exit_clr,
+        wait_all,
+        (TickType_t)timeout
+    );
+
+    if (options & TtFlagWaitAll) {
+        if ((flags & rflags) != flags) {
+            if (timeout > 0U) {
+                rflags = (uint32_t)TtStatusErrorTimeout;
+            } else {
+                rflags = (uint32_t)TtStatusErrorResource;
+            }
+        }
+    } else {
+        if ((flags & rflags) == 0U) {
+            if (timeout > 0U) {
+                rflags = (uint32_t)TtStatusErrorTimeout;
+            } else {
+                rflags = (uint32_t)TtStatusErrorResource;
+            }
+        }
+    }
+
+    /* Return event flags before clearing */
+    return (rflags);
+}
+
+} // namespace

TactilityCore/Source/EventFlag.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "CoreTypes.h"
+
+namespace tt {
+
+typedef void EventFlag;
+
+/** Allocate EventFlag
+ *
+ * @return     pointer to EventFlag
+ */
+EventFlag* event_flag_alloc();
+
+/** Deallocate EventFlag
+ *
+ * @param      instance  pointer to EventFlag
+ */
+void event_flag_free(EventFlag* instance);
+
+/** Set flags
+ *
+ * @param      instance  pointer to EventFlag
+ * @param[in]  flags     The flags
+ *
+ * @return     Resulting flags or error (TtStatus)
+ */
+uint32_t event_flag_set(EventFlag* instance, uint32_t flags);
+
+/** Clear flags
+ *
+ * @param      instance  pointer to EventFlag
+ * @param[in]  flags     The flags
+ *
+ * @return     Resulting flags or error (TtStatus)
+ */
+uint32_t event_flag_clear(EventFlag* instance, uint32_t flags);
+
+/** Get flags
+ *
+ * @param      instance  pointer to EventFlag
+ *
+ * @return     Resulting flags
+ */
+uint32_t event_flag_get(EventFlag* instance);
+
+/** Wait flags
+ *
+ * @param      instance  pointer to EventFlag
+ * @param[in]  flags     The flags
+ * @param[in]  options   The option flags
+ * @param[in]  timeout   The timeout
+ *
+ * @return     Resulting flags or error (TtStatus)
+ */
+uint32_t event_flag_wait(
+    EventFlag* instance,
+    uint32_t flags,
+    uint32_t options,
+    uint32_t timeout
+);
+
+} // namespace

TactilityCore/Source/Hash.cpp

@@ -0,0 +1,28 @@
+#include "Hash.h"
+
+namespace tt::hash {
+
+uint32_t djb2(const char* str) {
+    uint32_t hash = 5381;
+    char c = (char)*str++;
+    while (c != 0) {
+        hash = ((hash << 5) + hash) + (uint32_t)c; // hash * 33 + c
+        c = (char)*str++;
+    }
+    return hash;
+}
+
+uint32_t djb2(const void* data, size_t length) {
+    uint32_t hash = 5381;
+    auto* data_bytes = static_cast<const uint8_t*>(data);
+    uint8_t c = *data_bytes++;
+    size_t index = 0;
+    while (index < length) {
+        hash = ((hash << 5) + hash) + (uint32_t)c; // hash * 33 + c
+        c = *data_bytes++;
+        index++;
+    }
+    return hash;
+}
+
+} // namespace

TactilityCore/Source/Hash.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+
+namespace tt::hash {
+
+/**
+ * Implementation of DJB2 hashing algorithm.
+ * @param[in] str the string to calculate the hash for
+ * @return the hash
+ */
+uint32_t djb2(const char* str);
+
+/**
+ * Implementation of DJB2 hashing algorithm.
+ * @param[in] data the bytes to calculate the hash for
+ * @return the hash
+ */
+uint32_t djb2(const void* data, size_t length);
+
+} // namespace

TactilityCore/Source/Kernel.cpp

@@ -0,0 +1,208 @@
+#include "Kernel.h"
+#include "Check.h"
+#include "CoreDefines.h"
+#include "CoreTypes.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#else
+#include "FreeRTOS.h"
+#include "task.h"
+#endif
+
+#ifdef ESP_PLATFORM
+#include "rom/ets_sys.h"
+#else
+#include <unistd.h>
+#endif
+
+namespace tt {
+
+bool kernel_is_irq() {
+    return TT_IS_IRQ_MODE();
+}
+
+bool kernel_is_running() {
+    return xTaskGetSchedulerState() != taskSCHEDULER_RUNNING;
+}
+
+int32_t kernel_lock() {
+    tt_assert(!kernel_is_irq());
+
+    int32_t lock;
+
+    switch (xTaskGetSchedulerState()) {
+        case taskSCHEDULER_SUSPENDED:
+            lock = 1;
+            break;
+
+        case taskSCHEDULER_RUNNING:
+            vTaskSuspendAll();
+            lock = 0;
+            break;
+
+        case taskSCHEDULER_NOT_STARTED:
+        default:
+            lock = (int32_t)TtStatusError;
+            break;
+    }
+
+    /* Return previous lock state */
+    return (lock);
+}
+
+int32_t kernel_unlock() {
+    tt_assert(!kernel_is_irq());
+
+    int32_t lock;
+
+    switch (xTaskGetSchedulerState()) {
+        case taskSCHEDULER_SUSPENDED:
+            lock = 1;
+
+            if (xTaskResumeAll() != pdTRUE) {
+                if (xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED) {
+                    lock = (int32_t)TtStatusError;
+                }
+            }
+            break;
+
+        case taskSCHEDULER_RUNNING:
+            lock = 0;
+            break;
+
+        case taskSCHEDULER_NOT_STARTED:
+        default:
+            lock = (int32_t)TtStatusError;
+            break;
+    }
+
+    /* Return previous lock state */
+    return (lock);
+}
+
+int32_t kernel_restore_lock(int32_t lock) {
+    tt_assert(!kernel_is_irq());
+
+    switch (xTaskGetSchedulerState()) {
+        case taskSCHEDULER_SUSPENDED:
+        case taskSCHEDULER_RUNNING:
+            if (lock == 1) {
+                vTaskSuspendAll();
+            } else {
+                if (lock != 0) {
+                    lock = (int32_t)TtStatusError;
+                } else {
+                    if (xTaskResumeAll() != pdTRUE) {
+                        if (xTaskGetSchedulerState() != taskSCHEDULER_RUNNING) {
+                            lock = (int32_t)TtStatusError;
+                        }
+                    }
+                }
+            }
+            break;
+
+        case taskSCHEDULER_NOT_STARTED:
+        default:
+            lock = (int32_t)TtStatusError;
+            break;
+    }
+
+    /* Return new lock state */
+    return (lock);
+}
+
+uint32_t kernel_get_tick_frequency() {
+    /* Return frequency in hertz */
+    return (configTICK_RATE_HZ);
+}
+
+void delay_tick(uint32_t ticks) {
+    tt_assert(!kernel_is_irq());
+    if (ticks == 0U) {
+        taskYIELD();
+    } else {
+        vTaskDelay(ticks);
+    }
+}
+
+TtStatus delay_until_tick(uint32_t tick) {
+    tt_assert(!kernel_is_irq());
+
+    TickType_t tcnt, delay;
+    TtStatus stat;
+
+    stat = TtStatusOk;
+    tcnt = xTaskGetTickCount();
+
+    /* Determine remaining number of tick to delay */
+    delay = (TickType_t)tick - tcnt;
+
+    /* Check if target tick has not expired */
+    if ((delay != 0U) && (0 == (delay >> (8 * sizeof(TickType_t) - 1)))) {
+        if (xTaskDelayUntil(&tcnt, delay) == pdFALSE) {
+            /* Did not delay */
+            stat = TtStatusError;
+        }
+    } else {
+        /* No delay or already expired */
+        stat = TtStatusErrorParameter;
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+uint32_t get_tick() {
+    TickType_t ticks;
+
+    if (kernel_is_irq() != 0U) {
+        ticks = xTaskGetTickCountFromISR();
+    } else {
+        ticks = xTaskGetTickCount();
+    }
+
+    return ticks;
+}
+
+uint32_t ms_to_ticks(uint32_t milliseconds) {
+#if configTICK_RATE_HZ == 1000
+    return milliseconds;
+#else
+    return (uint32_t)((float)configTICK_RATE_HZ) / 1000.0f * (float)milliseconds;
+#endif
+}
+
+void delay_ms(uint32_t milliseconds) {
+    if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING) {
+        if (milliseconds > 0 && milliseconds < portMAX_DELAY - 1) {
+            milliseconds += 1;
+        }
+#if configTICK_RATE_HZ_RAW == 1000
+        tt_delay_tick(milliseconds);
+#else
+        delay_tick(ms_to_ticks(milliseconds));
+#endif
+    } else if (milliseconds > 0) {
+        delay_us(milliseconds * 1000);
+    }
+}
+
+void delay_us(uint32_t microseconds) {
+#ifdef ESP_PLATFORM
+    ets_delay_us(microseconds);
+#else
+    usleep(microseconds);
+#endif
+}
+
+Platform get_platform() {
+#ifdef ESP_PLATFORM
+    return PlatformEsp;
+#else
+    return PlatformPc;
+#endif
+}
+
+} // namespace

TactilityCore/Source/Kernel.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#include "CoreTypes.h"
+
+namespace tt {
+
+typedef enum {
+    PlatformEsp,
+    PlatformPc
+} Platform;
+
+/** Check if CPU is in IRQ or kernel running and IRQ is masked
+ * 
+ * Originally this primitive was born as a workaround for FreeRTOS kernel primitives shenanigans with PRIMASK.
+ * 
+ * Meaningful use cases are:
+ * 
+ * - When kernel is started and you want to ensure that you are not in IRQ or IRQ is not masked(like in critical section)
+ * - When kernel is not started and you want to make sure that you are not in IRQ mode, ignoring PRIMASK.
+ * 
+ * As you can see there will be edge case when kernel is not started and PRIMASK is not 0 that may cause some funky behavior.
+ * Most likely it will happen after kernel primitives being used, but control not yet passed to kernel.
+ * It's up to you to figure out if it is safe for your code or not.
+ * 
+ * @return     true if CPU is in IRQ or kernel running and IRQ is masked
+ */
+bool kernel_is_irq();
+
+/** Check if kernel is running
+ *
+ * @return     true if running, false otherwise
+ */
+bool kernel_is_running();
+
+/** Lock kernel, pause process scheduling
+ *
+ * @warning This should never be called in interrupt request context.
+ *
+ * @return     previous lock state(0 - unlocked, 1 - locked)
+ */
+int32_t kernel_lock();
+
+/** Unlock kernel, resume process scheduling
+ *
+ * @warning This should never be called in interrupt request context.
+ *
+ * @return     previous lock state(0 - unlocked, 1 - locked)
+ */
+int32_t kernel_unlock();
+
+/** Restore kernel lock state
+ *
+ * @warning This should never be called in interrupt request context.
+ *
+ * @param[in]  lock  The lock state
+ *
+ * @return     new lock state or error
+ */
+int32_t kernel_restore_lock(int32_t lock);
+
+/** Get kernel systick frequency
+ *
+ * @return     systick counts per second
+ */
+uint32_t kernel_get_tick_frequency();
+
+/** Delay execution
+ *
+ * @warning This should never be called in interrupt request context.
+ *
+ * Also keep in mind delay is aliased to scheduler timer intervals.
+ *
+ * @param[in]  ticks  The ticks count to pause
+ */
+void delay_tick(uint32_t ticks);
+
+/** Delay until tick
+ *
+ * @warning This should never be called in interrupt request context.
+ *
+ * @param[in]  ticks  The tick until which kerel should delay task execution
+ *
+ * @return     The status.
+ */
+TtStatus delay_until_tick(uint32_t tick);
+
+/** Convert milliseconds to ticks
+ *
+ * @param[in]   milliseconds    time in milliseconds
+ * @return      time in ticks
+ */
+uint32_t ms_to_ticks(uint32_t milliseconds);
+
+/** Delay in milliseconds
+ * 
+ * This method uses kernel ticks on the inside, which causes delay to be aliased to scheduler timer intervals.
+ * Real wait time will be between X+ milliseconds.
+ * Special value: 0, will cause task yield.
+ * Also if used when kernel is not running will fall back to `tt_delay_us`.
+ * 
+ * @warning    Cannot be used from ISR
+ *
+ * @param[in]  milliseconds  milliseconds to wait
+ */
+void delay_ms(uint32_t milliseconds);
+
+/** Delay in microseconds
+ *
+ * Implemented using Cortex DWT counter. Blocking and non aliased.
+ *
+ * @param[in]  microseconds  microseconds to wait
+ */
+void delay_us(uint32_t microseconds);
+
+Platform get_platform();
+
+} // namespace

TactilityCore/Source/Log.cpp

@@ -0,0 +1,92 @@
+#ifndef ESP_PLATFORM
+
+#include "Log.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#else
+#include <cstdint>
+#include <sys/time.h>
+#endif
+
+namespace tt {
+
+static char loglevel_to_prefix(LogLevel level) {
+    switch (level) {
+        case LogLevelError:
+            return 'E';
+        case LogLevelWarning:
+            return 'W';
+        case LogLevelInfo:
+            return 'I';
+        case LogLevelDebug:
+            return 'D';
+        case LogLevelTrace:
+            return 'T';
+        default:
+            return '?';
+    }
+}
+
+static const char* loglevel_to_colour(LogLevel level) {
+    switch (level) {
+        case LogLevelError:
+            return "\033[1;31m";
+        case LogLevelWarning:
+            return "\033[33m";
+        case LogLevelInfo:
+            return "\033[32m";
+        case LogLevelDebug:
+            return "\033[1;37m";
+        case LogLevelTrace:
+            return "\033[37m";
+        default:
+            return "";
+    }
+}
+
+uint64_t log_timestamp() {
+#ifdef ESP_PLATFORM
+    if (xTaskGetSchedulerState() == taskSCHEDULER_NOT_STARTED) {
+        return clock() / CLOCKS_PER_SEC * 1000;
+    }
+    static uint32_t base = 0;
+    if (base == 0 && xPortGetCoreID() == 0) {
+        base = clock() / CLOCKS_PER_SEC * 1000;
+    }
+    TickType_t tick_count = xPortInIsrContext() ? xTaskGetTickCountFromISR() : xTaskGetTickCount();
+    return base + tick_count * (1000 / configTICK_RATE_HZ);
+#else
+    static uint64_t base = 0;
+
+    struct timeval time {};
+    gettimeofday(&time, nullptr);
+    uint64_t now = ((uint64_t)time.tv_sec * 1000) + (time.tv_usec / 1000);
+    if (base == 0) {
+        base = now;
+    }
+    return now - base;
+#endif
+}
+
+void log(LogLevel level, const char* tag, const char* format, ...) {
+    printf(
+        "%s%c (%lu) %s: ",
+        loglevel_to_colour(level),
+        loglevel_to_prefix(level),
+        log_timestamp(),
+        tag
+    );
+
+    va_list args;
+    va_start(args, format);
+    vprintf(format, args);
+    va_end(args);
+
+    printf("\033[0m\n");
+}
+
+} // namespace
+
+#endif

TactilityCore/Source/Log.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#ifdef ESP_TARGET
+#include "esp_log.h"
+#else
+#include <cstdarg>
+#include <cstdio>
+#endif
+
+#ifdef ESP_TARGET
+
+#define TT_LOG_E(tag, format, ...) \
+    ESP_LOGE(tag, format, ##__VA_ARGS__)
+#define TT_LOG_W(tag, format, ...) \
+    ESP_LOGW(tag, format, ##__VA_ARGS__)
+#define TT_LOG_I(tag, format, ...) \
+    ESP_LOGI(tag, format, ##__VA_ARGS__)
+#define TT_LOG_D(tag, format, ...) \
+    ESP_LOGD(tag, format, ##__VA_ARGS__)
+#define TT_LOG_T(tag, format, ...) \
+    ESP_LOGV(tag, format, ##__VA_ARGS__)
+
+#else
+
+namespace tt {
+
+typedef enum {
+    LogLevelError,
+    LogLevelWarning,
+    LogLevelInfo,
+    LogLevelDebug,
+    LogLevelTrace
+} LogLevel;
+
+void log(LogLevel level, const char* tag, const char* format, ...);
+
+} // namespace
+
+#define TT_LOG_E(tag, format, ...) \
+    tt::log(tt::LogLevelError, tag, format, ##__VA_ARGS__)
+#define TT_LOG_W(tag, format, ...) \
+    tt::log(tt::LogLevelWarning, tag, format, ##__VA_ARGS__)
+#define TT_LOG_I(tag, format, ...) \
+    tt::log(tt::LogLevelInfo, tag, format, ##__VA_ARGS__)
+#define TT_LOG_D(tag, format, ...) \
+    tt::log(tt::LogLevelDebug, tag, format, ##__VA_ARGS__)
+#define TT_LOG_T(tag, format, ...) \
+    tt::log(tt::LogLevelTrace, tag, format, ##__VA_ARGS__)
+
+#endif // ESP_TARGET

TactilityCore/Source/MessageQueue.cpp

@@ -0,0 +1,173 @@
+#include "MessageQueue.h"
+#include "Check.h"
+#include "Kernel.h"
+
+namespace tt {
+
+MessageQueue::MessageQueue(uint32_t msg_count, uint32_t msg_size) {
+    tt_assert((kernel_is_irq() == 0U) && (msg_count > 0U) && (msg_size > 0U));
+    queue_handle = xQueueCreate(msg_count, msg_size);
+    tt_check(queue_handle);
+}
+
+MessageQueue::~MessageQueue() {
+    tt_assert(kernel_is_irq() == 0U);
+    vQueueDelete(queue_handle);
+}
+
+TtStatus MessageQueue::put(const void* msg_ptr, uint32_t timeout) {
+    TtStatus stat;
+    BaseType_t yield;
+
+    stat = TtStatusOk;
+
+    if (kernel_is_irq() != 0U) {
+        if ((queue_handle == nullptr) || (msg_ptr == nullptr) || (timeout != 0U)) {
+            stat = TtStatusErrorParameter;
+        } else {
+            yield = pdFALSE;
+
+            if (xQueueSendToBackFromISR(queue_handle, msg_ptr, &yield) != pdTRUE) {
+                stat = TtStatusErrorResource;
+            } else {
+                portYIELD_FROM_ISR(yield);
+            }
+        }
+    } else {
+        if ((queue_handle == nullptr) || (msg_ptr == nullptr)) {
+            stat = TtStatusErrorParameter;
+        } else {
+            if (xQueueSendToBack(queue_handle, msg_ptr, (TickType_t)timeout) != pdPASS) {
+                if (timeout != 0U) {
+                    stat = TtStatusErrorTimeout;
+                } else {
+                    stat = TtStatusErrorResource;
+                }
+            }
+        }
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+TtStatus MessageQueue::get(void* msg_ptr, uint32_t timeout_ticks) {
+    TtStatus stat;
+    BaseType_t yield;
+
+    stat = TtStatusOk;
+
+    if (kernel_is_irq() != 0U) {
+        if ((queue_handle == nullptr) || (msg_ptr == nullptr) || (timeout_ticks != 0U)) {
+            stat = TtStatusErrorParameter;
+        } else {
+            yield = pdFALSE;
+
+            if (xQueueReceiveFromISR(queue_handle, msg_ptr, &yield) != pdPASS) {
+                stat = TtStatusErrorResource;
+            } else {
+                portYIELD_FROM_ISR(yield);
+            }
+        }
+    } else {
+        if ((queue_handle == nullptr) || (msg_ptr == nullptr)) {
+            stat = TtStatusErrorParameter;
+        } else {
+            if (xQueueReceive(queue_handle, msg_ptr, (TickType_t)timeout_ticks) != pdPASS) {
+                if (timeout_ticks != 0U) {
+                    stat = TtStatusErrorTimeout;
+                } else {
+                    stat = TtStatusErrorResource;
+                }
+            }
+        }
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+uint32_t MessageQueue::getCapacity() const {
+    auto* mq = (StaticQueue_t*)(queue_handle);
+    uint32_t capacity;
+
+    if (mq == nullptr) {
+        capacity = 0U;
+    } else {
+        /* capacity = pxQueue->uxLength */
+        capacity = mq->uxDummy4[1];
+    }
+
+    /* Return maximum number of messages */
+    return (capacity);
+}
+
+uint32_t MessageQueue::getMessageSize() const {
+    auto* mq = (StaticQueue_t*)(queue_handle);
+    uint32_t size;
+
+    if (mq == nullptr) {
+        size = 0U;
+    } else {
+        /* size = pxQueue->uxItemSize */
+        size = mq->uxDummy4[2];
+    }
+
+    /* Return maximum message size */
+    return (size);
+}
+
+uint32_t MessageQueue::getCount() const {
+    UBaseType_t count;
+
+    if (queue_handle == nullptr) {
+        count = 0U;
+    } else if (kernel_is_irq() != 0U) {
+        count = uxQueueMessagesWaitingFromISR(queue_handle);
+    } else {
+        count = uxQueueMessagesWaiting(queue_handle);
+    }
+
+    /* Return number of queued messages */
+    return ((uint32_t)count);
+}
+
+uint32_t MessageQueue::getSpace() const {
+    auto* mq = (StaticQueue_t*)(queue_handle);
+    uint32_t space;
+    uint32_t isrm;
+
+    if (mq == nullptr) {
+        space = 0U;
+    } else if (kernel_is_irq() != 0U) {
+        isrm = taskENTER_CRITICAL_FROM_ISR();
+
+        /* space = pxQueue->uxLength - pxQueue->uxMessagesWaiting; */
+        space = mq->uxDummy4[1] - mq->uxDummy4[0];
+
+        taskEXIT_CRITICAL_FROM_ISR(isrm);
+    } else {
+        space = (uint32_t)uxQueueSpacesAvailable((QueueHandle_t)mq);
+    }
+
+    /* Return number of available slots */
+    return (space);
+}
+
+TtStatus MessageQueue::reset() {
+    TtStatus stat;
+
+    if (kernel_is_irq() != 0U) {
+        stat = TtStatusErrorISR;
+    } else if (queue_handle == nullptr) {
+        stat = TtStatusErrorParameter;
+    } else {
+        stat = TtStatusOk;
+        (void)xQueueReset(queue_handle);
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+} // namespace

TactilityCore/Source/MessageQueue.h

@@ -0,0 +1,99 @@
+/**
+ * @file MessageQueue.h
+ *
+ * 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 "CoreTypes.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#else
+#include "FreeRTOS.h"
+#include "queue.h"
+#endif
+
+namespace tt {
+
+class MessageQueue {
+private:
+    QueueHandle_t queue_handle;
+
+public:
+    /** Allocate message queue
+     *
+     * @param[in]  msg_count  The message count
+     * @param[in]  msg_size   The message size
+     */
+    MessageQueue(uint32_t msg_count, uint32_t msg_size);
+
+    ~MessageQueue();
+
+    /** Put message into queue
+     *
+     * @param      instance  pointer to MessageQueue instance
+     * @param[in]  msg_ptr   The message pointer
+     * @param[in]  timeout   The timeout
+     * @param[in]  msg_prio  The message prio
+     *
+     * @return     The status.
+     */
+    TtStatus put(const void* msg_ptr, uint32_t timeout);
+
+    /** Get message from queue
+     *
+     * @param      instance  pointer to MessageQueue instance
+     * @param      msg_ptr   The message pointer
+     * @param      msg_prio  The message prioority
+     * @param[in]  timeout_ticks   The timeout
+     *
+     * @return     The status.
+     */
+    TtStatus get(void* msg_ptr, uint32_t timeout_ticks);
+
+    /** Get queue capacity
+     *
+     * @param      instance  pointer to MessageQueue instance
+     *
+     * @return     capacity in object count
+     */
+    uint32_t getCapacity() const;
+
+    /** Get message size
+     *
+     * @param      instance  pointer to MessageQueue instance
+     *
+     * @return     Message size in bytes
+     */
+    uint32_t getMessageSize() const;
+
+    /** Get message count in queue
+     *
+     * @param      instance  pointer to MessageQueue instance
+     *
+     * @return     Message count
+     */
+    uint32_t getCount() const;
+
+    /** Get queue available space
+     *
+     * @param      instance  pointer to MessageQueue instance
+     *
+     * @return     Message count
+     */
+    uint32_t getSpace() const;
+
+    /** Reset queue
+     *
+     * @param      instance  pointer to MessageQueue instance
+     *
+     * @return     The status.
+     */
+    TtStatus reset();
+};
+
+} // namespace

TactilityCore/Source/Mutex.cpp

@@ -0,0 +1,135 @@
+#include "Mutex.h"
+
+#include "Check.h"
+#include "CoreDefines.h"
+#include "Log.h"
+
+namespace tt {
+
+#define MUTEX_DEBUGGING false
+
+#if MUTEX_DEBUGGING
+#define TAG "mutex"
+    void tt_mutex_info(Mutex mutex, const char* label) {
+        MutexData* data = (MutexData*)mutex;
+        if (data == NULL) {
+            TT_LOG_I(TAG, "mutex %s: is NULL", label);
+        } else {
+            TT_LOG_I(TAG, "mutex %s: handle=%0X type=%d owner=%0x", label, data->handle, data->type, tt_mutex_get_owner(mutex));
+        }
+    }
+#else
+#define tt_mutex_info(mutex, text)
+#endif
+
+Mutex::Mutex(MutexType type) : type(type) {
+    tt_mutex_info(data, "alloc");
+    switch (type) {
+        case MutexTypeNormal:
+            semaphore = xSemaphoreCreateMutex();
+            break;
+        case MutexTypeRecursive:
+            semaphore = xSemaphoreCreateRecursiveMutex();
+            break;
+        default:
+            tt_crash("Mutex type unknown/corrupted");
+    }
+
+    tt_check(semaphore != nullptr);
+
+}
+
+Mutex::~Mutex() {
+    tt_assert(!TT_IS_IRQ_MODE());
+    vSemaphoreDelete(semaphore);
+    semaphore = nullptr; // If the mutex is used after release, this might help debugging
+}
+
+TtStatus Mutex::acquire(uint32_t timeout) const {
+    tt_assert(!TT_IS_IRQ_MODE());
+    tt_assert(semaphore);
+    TtStatus status = TtStatusOk;
+
+    tt_mutex_info(mutex, "acquire");
+
+    switch (type) {
+        case MutexTypeNormal:
+            if (xSemaphoreTake(semaphore, timeout) != pdPASS) {
+                if (timeout != 0U) {
+                    status = TtStatusErrorTimeout;
+                } else {
+                    status = TtStatusErrorResource;
+                }
+            }
+            break;
+        case MutexTypeRecursive:
+            if (xSemaphoreTakeRecursive(semaphore, timeout) != pdPASS) {
+                if (timeout != 0U) {
+                    status = TtStatusErrorTimeout;
+                } else {
+                    status = TtStatusErrorResource;
+                }
+            }
+            break;
+        default:
+            tt_crash("mutex type unknown/corrupted");
+    }
+
+    return status;
+}
+
+TtStatus Mutex::release() const {
+    assert(!TT_IS_IRQ_MODE());
+    tt_assert(semaphore);
+    TtStatus status = TtStatusOk;
+
+    tt_mutex_info(mutex, "release");
+
+    switch (type) {
+        case MutexTypeNormal: {
+            if (xSemaphoreGive(semaphore) != pdPASS) {
+                status = TtStatusErrorResource;
+            }
+            break;
+        }
+        case MutexTypeRecursive:
+            if (xSemaphoreGiveRecursive(semaphore) != pdPASS) {
+                status = TtStatusErrorResource;
+            }
+            break;
+        default:
+            tt_crash("mutex type unknown/corrupted");
+    }
+
+    return status;
+}
+
+ThreadId Mutex::getOwner() const {
+    tt_assert(!TT_IS_IRQ_MODE());
+    tt_assert(semaphore);
+    return (ThreadId)xSemaphoreGetMutexHolder(semaphore);
+}
+
+
+Mutex* tt_mutex_alloc(MutexType type) {
+    return new Mutex(type);
+}
+
+void tt_mutex_free(Mutex* mutex) {
+    delete mutex;
+}
+
+TtStatus tt_mutex_acquire(Mutex* mutex, uint32_t timeout) {
+    return mutex-> acquire(timeout);
+}
+
+TtStatus tt_mutex_release(Mutex* mutex) {
+    return mutex->release();
+
+}
+
+ThreadId tt_mutex_get_owner(Mutex* mutex) {
+    return mutex->getOwner();
+}
+
+} // namespace

TactilityCore/Source/Mutex.h

@@ -0,0 +1,83 @@
+/**
+ * @file mutex.h
+ * Mutex
+ */
+#pragma once
+
+#include "CoreTypes.h"
+#include "Thread.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/semphr.h"
+#else
+#include "FreeRTOS.h"
+#include "semphr.h"
+#endif
+
+namespace tt {
+
+typedef enum {
+    MutexTypeNormal,
+    MutexTypeRecursive,
+} MutexType;
+
+class Mutex {
+private:
+    SemaphoreHandle_t semaphore;
+    MutexType type;
+public:
+    Mutex(MutexType type);
+    ~Mutex();
+
+    TtStatus acquire(uint32_t timeout) const;
+    TtStatus release() const;
+    ThreadId getOwner() const;
+};
+
+/** Allocate Mutex
+ *
+ * @param[in]  type  The mutex type
+ *
+ * @return     pointer to Mutex instance
+ */
+
+[[deprecated("use class")]]
+Mutex* tt_mutex_alloc(MutexType type);
+
+/** Free Mutex
+ *
+ * @param      mutex  The Mutex instance
+ */
+[[deprecated("use class")]]
+void tt_mutex_free(Mutex* mutex);
+
+/** Acquire mutex
+ *
+ * @param      mutex  The Mutex instance
+ * @param[in]  timeout   The timeout
+ *
+ * @return     The status.
+ */
+[[deprecated("use class")]]
+TtStatus tt_mutex_acquire(Mutex* mutex, uint32_t timeout);
+
+/** Release mutex
+ *
+ * @param      mutex  The Mutex instance
+ *
+ * @return     The status.
+ */
+[[deprecated("use class")]]
+TtStatus tt_mutex_release(Mutex* mutex);
+
+/** Get mutex owner thread id
+ *
+ * @param      mutex  The Mutex instance
+ *
+ * @return     The thread identifier.
+ */
+[[deprecated("use class")]]
+ThreadId tt_mutex_get_owner(Mutex* mutex);
+
+} // namespace

TactilityCore/Source/Pubsub.cpp

@@ -0,0 +1,84 @@
+#include "Pubsub.h"
+#include "Check.h"
+#include "Mutex.h"
+#include <list>
+
+namespace tt {
+
+struct PubSubSubscription {
+    uint64_t id;
+    PubSubCallback callback;
+    void* callback_context;
+};
+
+typedef std::list<PubSubSubscription> Subscriptions;
+
+struct PubSub {
+    uint64_t last_id = 0;
+    Subscriptions items;
+    Mutex* mutex;
+};
+
+PubSub* tt_pubsub_alloc() {
+    auto* pubsub = new PubSub();
+
+    pubsub->mutex = tt_mutex_alloc(MutexTypeNormal);
+    tt_assert(pubsub->mutex);
+
+    return pubsub;
+}
+
+void tt_pubsub_free(PubSub* pubsub) {
+    tt_assert(pubsub);
+    tt_check(pubsub->items.empty());
+    tt_mutex_free(pubsub->mutex);
+    delete pubsub;
+}
+
+PubSubSubscription* tt_pubsub_subscribe(PubSub* pubsub, PubSubCallback callback, void* callback_context) {
+    tt_check(tt_mutex_acquire(pubsub->mutex, TtWaitForever) == TtStatusOk);
+    PubSubSubscription subscription = {
+        .id = (++pubsub->last_id),
+        .callback = callback,
+        .callback_context = callback_context
+    };
+    pubsub->items.push_back(
+        subscription
+    );
+
+    tt_check(tt_mutex_release(pubsub->mutex) == TtStatusOk);
+
+    return (PubSubSubscription*)pubsub->last_id;
+}
+
+void tt_pubsub_unsubscribe(PubSub* pubsub, PubSubSubscription* pubsub_subscription) {
+    tt_assert(pubsub);
+    tt_assert(pubsub_subscription);
+
+    tt_check(tt_mutex_acquire(pubsub->mutex, TtWaitForever) == TtStatusOk);
+    bool result = false;
+    auto id = (uint64_t)pubsub_subscription;
+    for (auto it = pubsub->items.begin(); it != pubsub->items.end(); it++) {
+        if (it->id == id) {
+            pubsub->items.erase(it);
+            result = true;
+            break;
+        }
+    }
+
+    tt_check(tt_mutex_release(pubsub->mutex) == TtStatusOk);
+    tt_check(result);
+}
+
+void tt_pubsub_publish(PubSub* pubsub, void* message) {
+    tt_check(tt_mutex_acquire(pubsub->mutex, TtWaitForever) == TtStatusOk);
+
+    // Iterate over subscribers
+    for (auto& it : pubsub->items) {
+        it.callback(message, it.callback_context);
+    }
+
+    tt_check(tt_mutex_release(pubsub->mutex) == TtStatusOk);
+}
+
+} // namespace

TactilityCore/Source/Pubsub.h

@@ -0,0 +1,64 @@
+/**
+ * @file pubsub.h
+ * PubSub
+ */
+#pragma once
+
+namespace tt {
+
+/** PubSub Callback type */
+typedef void (*PubSubCallback)(const void* message, void* context);
+
+/** PubSub type */
+typedef struct PubSub PubSub;
+
+/** PubSubSubscription type */
+typedef struct PubSubSubscription PubSubSubscription;
+
+/** Allocate PubSub
+ *
+ * Reentrable, Not threadsafe, one owner
+ *
+ * @return     pointer to PubSub instance
+ */
+PubSub* tt_pubsub_alloc();
+
+/** Free PubSub
+ * 
+ * @param      pubsub  PubSub instance
+ */
+void tt_pubsub_free(PubSub* pubsub);
+
+/** Subscribe to PubSub
+ * 
+ * Threadsafe, Reentrable
+ * 
+ * @param      pubsub            pointer to PubSub instance
+ * @param[in]  callback          The callback
+ * @param      callback_context  The callback context
+ *
+ * @return     pointer to PubSubSubscription instance
+ */
+PubSubSubscription*
+tt_pubsub_subscribe(PubSub* pubsub, PubSubCallback callback, void* callback_context);
+
+/** Unsubscribe from PubSub
+ * 
+ * No use of `pubsub_subscription` allowed after call of this method
+ * Threadsafe, Reentrable.
+ *
+ * @param      pubsub               pointer to PubSub instance
+ * @param      pubsub_subscription  pointer to PubSubSubscription instance
+ */
+void tt_pubsub_unsubscribe(PubSub* pubsub, PubSubSubscription* pubsub_subscription);
+
+/** Publish message to PubSub
+ *
+ * Threadsafe, Reentrable.
+ * 
+ * @param      pubsub   pointer to PubSub instance
+ * @param      message  message pointer to publish
+ */
+void tt_pubsub_publish(PubSub* pubsub, void* message);
+
+} // namespace

TactilityCore/Source/Semaphore.cpp

@@ -0,0 +1,128 @@
+#include "Semaphore.h"
+#include "Check.h"
+#include "CoreDefines.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/semphr.h"
+#else
+#include "FreeRTOS.h"
+#include "semphr.h"
+#endif
+
+namespace tt {
+
+Semaphore* tt_semaphore_alloc(uint32_t max_count, uint32_t initial_count) {
+    tt_assert(!TT_IS_IRQ_MODE());
+    tt_assert((max_count > 0U) && (initial_count <= max_count));
+
+    SemaphoreHandle_t hSemaphore = nullptr;
+    if (max_count == 1U) {
+        hSemaphore = xSemaphoreCreateBinary();
+        if ((hSemaphore != nullptr) && (initial_count != 0U)) {
+            if (xSemaphoreGive(hSemaphore) != pdPASS) {
+                vSemaphoreDelete(hSemaphore);
+                hSemaphore = nullptr;
+            }
+        }
+    } else {
+        hSemaphore = xSemaphoreCreateCounting(max_count, initial_count);
+    }
+
+    tt_check(hSemaphore);
+
+    return (Semaphore*)hSemaphore;
+}
+
+void tt_semaphore_free(Semaphore* instance) {
+    tt_assert(instance);
+    tt_assert(!TT_IS_IRQ_MODE());
+
+    SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)instance;
+
+    vSemaphoreDelete(hSemaphore);
+}
+
+TtStatus tt_semaphore_acquire(Semaphore* instance, uint32_t timeout) {
+    tt_assert(instance);
+
+    SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)instance;
+    TtStatus status;
+    BaseType_t yield;
+
+    status = TtStatusOk;
+
+    if (TT_IS_IRQ_MODE()) {
+        if (timeout != 0U) {
+            status = TtStatusErrorParameter;
+        } else {
+            yield = pdFALSE;
+
+            if (xSemaphoreTakeFromISR(hSemaphore, &yield) != pdPASS) {
+                status = TtStatusErrorResource;
+            } else {
+                portYIELD_FROM_ISR(yield);
+            }
+        }
+    } else {
+        if (xSemaphoreTake(hSemaphore, (TickType_t)timeout) != pdPASS) {
+            if (timeout != 0U) {
+                status = TtStatusErrorTimeout;
+            } else {
+                status = TtStatusErrorResource;
+            }
+        }
+    }
+
+    return status;
+}
+
+TtStatus tt_semaphore_release(Semaphore* instance) {
+    tt_assert(instance);
+
+    SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)instance;
+    TtStatus stat;
+    BaseType_t yield;
+
+    stat = TtStatusOk;
+
+    if (TT_IS_IRQ_MODE()) {
+        yield = pdFALSE;
+
+        if (xSemaphoreGiveFromISR(hSemaphore, &yield) != pdTRUE) {
+            stat = TtStatusErrorResource;
+        } else {
+            portYIELD_FROM_ISR(yield);
+        }
+    } else {
+        if (xSemaphoreGive(hSemaphore) != pdPASS) {
+            stat = TtStatusErrorResource;
+        }
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+uint32_t tt_semaphore_get_count(Semaphore* instance) {
+    tt_assert(instance);
+
+    SemaphoreHandle_t hSemaphore = (SemaphoreHandle_t)instance;
+    uint32_t count;
+
+    if (TT_IS_IRQ_MODE()) {
+        // TODO: uxSemaphoreGetCountFromISR is not supported on esp-idf 5.1.2 - perhaps later on?
+#ifdef uxSemaphoreGetCountFromISR
+        count = (uint32_t)uxSemaphoreGetCountFromISR(hSemaphore);
+#else
+        count = (uint32_t)uxQueueMessagesWaitingFromISR((QueueHandle_t)hSemaphore);
+#endif
+    } else {
+        count = (uint32_t)uxSemaphoreGetCount(hSemaphore);
+    }
+
+    /* Return number of tokens */
+    return (count);
+}
+
+} // namespace

TactilityCore/Source/Semaphore.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "CoreTypes.h"
+#include "Thread.h"
+
+namespace tt {
+
+typedef void Semaphore;
+
+/** Allocate semaphore
+ *
+ * @param[in]  max_count      The maximum count
+ * @param[in]  initial_count  The initial count
+ *
+ * @return     pointer to Semaphore instance
+ */
+Semaphore* tt_semaphore_alloc(uint32_t max_count, uint32_t initial_count);
+
+/** Free semaphore
+ *
+ * @param      instance  The pointer to Semaphore instance
+ */
+void tt_semaphore_free(Semaphore* instance);
+
+/** Acquire semaphore
+ *
+ * @param      instance  The pointer to Semaphore instance
+ * @param[in]  timeout   The timeout
+ *
+ * @return     The status.
+ */
+TtStatus tt_semaphore_acquire(Semaphore* instance, uint32_t timeout);
+
+/** Release semaphore
+ *
+ * @param      instance  The pointer to Semaphore instance
+ *
+ * @return     The status.
+ */
+TtStatus tt_semaphore_release(Semaphore* instance);
+
+/** Get semaphore count
+ *
+ * @param      instance  The pointer to Semaphore instance
+ *
+ * @return     Semaphore count
+ */
+uint32_t tt_semaphore_get_count(Semaphore* instance);
+
+} // namespace

TactilityCore/Source/StreamBuffer.cpp

@@ -0,0 +1,98 @@
+#include "StreamBuffer.h"
+
+#include "Check.h"
+#include "CoreDefines.h"
+#include "CoreTypes.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/stream_buffer.h"
+#else
+#include "FreeRTOS.h"
+#include "stream_buffer.h"
+#endif
+
+namespace tt {
+
+StreamBuffer* stream_buffer_alloc(size_t size, size_t trigger_level) {
+    tt_assert(size != 0);
+
+    StreamBufferHandle_t handle = xStreamBufferCreate(size, trigger_level);
+    tt_check(handle);
+
+    return handle;
+};
+
+void stream_buffer_free(StreamBuffer* stream_buffer) {
+    tt_assert(stream_buffer);
+    vStreamBufferDelete((StreamBufferHandle_t)stream_buffer);
+};
+
+bool stream_set_trigger_level(StreamBuffer* stream_buffer, size_t trigger_level) {
+    tt_assert(stream_buffer);
+    return xStreamBufferSetTriggerLevel((StreamBufferHandle_t)stream_buffer, trigger_level) == pdTRUE;
+};
+
+size_t stream_buffer_send(
+    StreamBuffer* stream_buffer,
+    const void* data,
+    size_t length,
+    uint32_t timeout
+) {
+    size_t ret;
+
+    if (TT_IS_IRQ_MODE()) {
+        BaseType_t yield;
+        ret = xStreamBufferSendFromISR((StreamBufferHandle_t)stream_buffer, data, length, &yield);
+        portYIELD_FROM_ISR(yield);
+    } else {
+        ret = xStreamBufferSend((StreamBufferHandle_t)stream_buffer, data, length, timeout);
+    }
+
+    return ret;
+};
+
+size_t stream_buffer_receive(
+    StreamBuffer* stream_buffer,
+    void* data,
+    size_t length,
+    uint32_t timeout
+) {
+    size_t ret;
+
+    if (TT_IS_IRQ_MODE()) {
+        BaseType_t yield;
+        ret = xStreamBufferReceiveFromISR((StreamBufferHandle_t)stream_buffer, data, length, &yield);
+        portYIELD_FROM_ISR(yield);
+    } else {
+        ret = xStreamBufferReceive((StreamBufferHandle_t)stream_buffer, data, length, timeout);
+    }
+
+    return ret;
+}
+
+size_t stream_buffer_bytes_available(StreamBuffer* stream_buffer) {
+    return xStreamBufferBytesAvailable((StreamBufferHandle_t)stream_buffer);
+};
+
+size_t stream_buffer_spaces_available(StreamBuffer* stream_buffer) {
+    return xStreamBufferSpacesAvailable((StreamBufferHandle_t)stream_buffer);
+};
+
+bool stream_buffer_is_full(StreamBuffer* stream_buffer) {
+    return xStreamBufferIsFull((StreamBufferHandle_t)stream_buffer) == pdTRUE;
+};
+
+bool stream_buffer_is_empty(StreamBuffer* stream_buffer) {
+    return (xStreamBufferIsEmpty((StreamBufferHandle_t)stream_buffer) == pdTRUE);
+};
+
+TtStatus stream_buffer_reset(StreamBuffer* stream_buffer) {
+    if (xStreamBufferReset((StreamBufferHandle_t)stream_buffer) == pdPASS) {
+        return TtStatusOk;
+    } else {
+        return TtStatusError;
+    }
+}
+
+} // namespace

TactilityCore/Source/StreamBuffer.h

@@ -0,0 +1,152 @@
+/**
+ * @file stream_buffer.h
+ * Tactility stream buffer primitive.
+ * 
+ * Stream buffers are used to send a continuous stream of data from one task or
+ * interrupt to another.  Their implementation is light weight, making them
+ * particularly suited for interrupt to task and core to core communication
+ * scenarios.
+ * 
+ * ***NOTE***: Stream buffer implementation assumes there is only one task or
+ * interrupt that will write to the buffer (the writer), and only one task or
+ * interrupt that will read from the buffer (the reader).
+ */
+#pragma once
+
+#include "CoreTypes.h"
+#include <cstddef>
+#include <cstdint>
+
+namespace tt {
+
+typedef void StreamBuffer;
+
+/**
+ * @brief Allocate stream buffer instance.
+ * Stream buffer implementation assumes there is only one task or
+ * interrupt that will write to the buffer (the writer), and only one task or
+ * interrupt that will read from the buffer (the reader).
+ * 
+ * @param size The total number of bytes the stream buffer will be able to hold at any one time.
+ * @param trigger_level The number of bytes that must be in the stream buffer 
+ * before a task that is blocked on the stream buffer to wait for data is moved out of the blocked state.
+ * @return The stream buffer instance.
+ */
+StreamBuffer* stream_buffer_alloc(size_t size, size_t trigger_level);
+
+/**
+ * @brief Free stream buffer instance
+ * 
+ * @param stream_buffer The stream buffer instance.
+ */
+void stream_buffer_free(StreamBuffer* stream_buffer);
+
+/**
+ * @brief Set trigger level for stream buffer.
+ * A stream buffer's trigger level is the number of bytes that must be in the
+ * stream buffer before a task that is blocked on the stream buffer to
+ * wait for data is moved out of the blocked state.
+ * 
+ * @param stream_buffer The stream buffer instance
+ * @param trigger_level The new trigger level for the stream buffer.
+ * @return true if trigger level can be be updated (new trigger level was less than or equal to the stream buffer's length). 
+ * @return false if trigger level can't be be updated (new trigger level was greater than the stream buffer's length).
+ */
+bool stream_set_trigger_level(StreamBuffer* stream_buffer, size_t trigger_level);
+
+/**
+ * @brief Sends bytes to a stream buffer. The bytes are copied into the stream buffer.
+ * Wakes up task waiting for data to become available if called from ISR.
+ * 
+ * @param stream_buffer The stream buffer instance.
+ * @param data A pointer to the data that is to be copied into the stream buffer.
+ * @param length The maximum number of bytes to copy from data into the stream buffer.
+ * @param timeout The maximum amount of time the task should remain in the
+ * Blocked state to wait for space to become available if the stream buffer is full. 
+ * Will return immediately if timeout is zero. 
+ * Setting timeout to TtWaitForever will cause the task to wait indefinitely.
+ * Ignored if called from ISR.
+ * @return The number of bytes actually written to the stream buffer. 
+ */
+size_t stream_buffer_send(
+    StreamBuffer* stream_buffer,
+    const void* data,
+    size_t length,
+    uint32_t timeout
+);
+
+/**
+ * @brief Receives bytes from a stream buffer.
+ * Wakes up task waiting for space to become available if called from ISR.
+ * 
+ * @param stream_buffer The stream buffer instance.
+ * @param data A pointer to the buffer into which the received bytes will be
+ * copied.
+ * @param length The length of the buffer pointed to by the data parameter.
+ * @param timeout The maximum amount of time the task should remain in the
+ * Blocked state to wait for data to become available if the stream buffer is empty. 
+ * Will return immediately if timeout is zero. 
+ * Setting timeout to TtWaitForever will cause the task to wait indefinitely.
+ * Ignored if called from ISR.
+ * @return The number of bytes read from the stream buffer, if any. 
+ */
+size_t stream_buffer_receive(
+    StreamBuffer* stream_buffer,
+    void* data,
+    size_t length,
+    uint32_t timeout
+);
+
+/**
+ * @brief Queries a stream buffer to see how much data it contains, which is equal to
+ * the number of bytes that can be read from the stream buffer before the stream
+ * buffer would be empty.
+ * 
+ * @param stream_buffer The stream buffer instance.
+ * @return The number of bytes that can be read from the stream buffer before
+ * the stream buffer would be empty.
+ */
+size_t stream_buffer_bytes_available(StreamBuffer* stream_buffer);
+
+/**
+ * @brief Queries a stream buffer to see how much free space it contains, which is
+ * equal to the amount of data that can be sent to the stream buffer before it
+ * is full.
+ * 
+ * @param stream_buffer The stream buffer instance.
+ * @return The number of bytes that can be written to the stream buffer before
+ * the stream buffer would be full. 
+ */
+size_t stream_buffer_spaces_available(StreamBuffer* stream_buffer);
+
+/**
+ * @brief Queries a stream buffer to see if it is full.
+ * 
+ * @param stream_buffer stream buffer instance.
+ * @return true if the stream buffer is full.
+ * @return false if the stream buffer is not full.
+ */
+bool stream_buffer_is_full(StreamBuffer* stream_buffer);
+
+/**
+ * @brief Queries a stream buffer to see if it is empty.
+ * 
+ * @param stream_buffer The stream buffer instance.
+ * @return true if the stream buffer is empty.
+ * @return false if the stream buffer is not empty.
+ */
+bool tt_stream_buffer_is_empty(StreamBuffer* stream_buffer);
+
+/**
+ * @brief Resets a stream buffer to its initial, empty, state. Any data that was 
+ * in the stream buffer is discarded. A stream buffer can only be reset if there 
+ * are no tasks blocked waiting to either send to or receive from the stream buffer.
+ * 
+ * @param stream_buffer The stream buffer instance.
+ * @return TtStatusOk if the stream buffer is reset.
+ * @return TtStatusError if there was a task blocked waiting to send to or read
+ * from the stream buffer then the stream buffer is not reset.
+ */
+TtStatus tt_stream_buffer_reset(StreamBuffer* stream_buffer);
+
+} // namespace

TactilityCore/Source/StringUtils.cpp

@@ -0,0 +1,30 @@
+#include "StringUtils.h"
+#include <cstring>
+
+namespace tt {
+
+int string_find_last_index(const char* text, size_t from_index, char find) {
+    for (size_t i = from_index; i >= 0; i--) {
+        if (text[i] == find) {
+            return (int)i;
+        }
+    }
+    return -1;
+}
+
+bool string_get_path_parent(const char* path, char* output) {
+    int index = string_find_last_index(path, strlen(path) - 1, '/');
+    if (index == -1) {
+        return false;
+    } else if (index == 0) {
+        output[0] = '/';
+        output[1] = 0x00;
+        return true;
+    } else {
+        memcpy(output, path, index);
+        output[index] = 0x00;
+        return true;
+    }
+}
+
+} // namespace

TactilityCore/Source/StringUtils.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <cstdio>
+
+namespace tt {
+
+/**
+ * Find the last occurrence of a character.
+ * @param[in] text the text to search in
+ * @param[in] from_index the index to search from (searching from right to left)
+ * @param[in] find the character to search for
+ * @return the index of the found character, or -1 if none found
+ */
+int string_find_last_index(const char* text, size_t from_index, char find);
+
+/**
+ * Given a filesystem path as input, try and get the parent path.
+ * @param[in] path input path
+ * @param[out] output an output buffer that is allocated to at least the size of "current"
+ * @return true when successful
+ */
+bool string_get_path_parent(const char* path, char* output);
+
+} // namespace

TactilityCore/Source/TactilityCore.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <cstdio>
+
+#include "Check.h"
+#include "CoreDefines.h"
+#include "CoreExtraDefines.h"
+#include "CoreTypes.h"
+#include "Critical.h"
+#include "EventFlag.h"
+#include "Kernel.h"
+#include "Log.h"

TactilityCore/Source/TactilityCoreConfig.h

@@ -0,0 +1,3 @@
+#pragma once
+
+#define TT_CONFIG_THREAD_MAX_PRIORITIES 10

TactilityCore/Source/Thread.cpp

@@ -0,0 +1,519 @@
+#include "Thread.h"
+#include <cstdlib>
+#include <cstring>
+
+#include "Check.h"
+#include "CoreDefines.h"
+#include "Kernel.h"
+#include "Log.h"
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#else
+#include "FreeRTOS.h"
+#include "task.h"
+#endif
+
+namespace tt {
+
+#define TAG "Thread"
+
+#define THREAD_NOTIFY_INDEX 1 // Index 0 is used for stream buffers
+
+// Limits
+#define MAX_BITS_TASK_NOTIFY 31U
+#define MAX_BITS_EVENT_GROUPS 24U
+
+#define THREAD_FLAGS_INVALID_BITS (~((1UL << MAX_BITS_TASK_NOTIFY) - 1U))
+#define EVENT_FLAGS_INVALID_BITS (~((1UL << MAX_BITS_EVENT_GROUPS) - 1U))
+
+static_assert(ThreadPriorityHighest <= TT_CONFIG_THREAD_MAX_PRIORITIES, "highest thread priority is higher than max priority");
+static_assert(TT_CONFIG_THREAD_MAX_PRIORITIES <= configMAX_PRIORITIES, "highest tactility priority is higher than max FreeRTOS priority");
+
+struct Thread {
+    ThreadState state;
+    int32_t ret;
+
+    ThreadCallback callback;
+    void* context;
+
+    ThreadStateCallback state_callback;
+    void* state_context;
+
+    char* name;
+    char* appid;
+
+    ThreadPriority priority;
+
+    TaskHandle_t task_handle;
+
+    // Keep all non-alignable byte types in one place,
+    // this ensures that the size of this structure is minimal
+    bool is_static;
+
+    configSTACK_DEPTH_TYPE stack_size;
+};
+
+/** Catch threads that are trying to exit wrong way */
+__attribute__((__noreturn__)) void thread_catch() { //-V1082
+    // If you're here it means you're probably doing something wrong
+    // with critical sections or with scheduler state
+    asm volatile("nop");                // extra magic
+    tt_crash("You are doing it wrong"); //-V779
+    __builtin_unreachable();
+}
+
+static void thread_set_state(Thread* thread, ThreadState state) {
+    tt_assert(thread);
+    thread->state = state;
+    if (thread->state_callback) {
+        thread->state_callback(state, thread->state_context);
+    }
+}
+
+static void thread_body(void* context) {
+    tt_assert(context);
+    auto* thread = static_cast<Thread*>(context);
+
+    // Store thread instance to thread local storage
+    tt_assert(pvTaskGetThreadLocalStoragePointer(nullptr, 0) == nullptr);
+    vTaskSetThreadLocalStoragePointer(nullptr, 0, thread);
+
+    tt_assert(thread->state == ThreadStateStarting);
+    thread_set_state(thread, ThreadStateRunning);
+
+    thread->ret = thread->callback(thread->context);
+
+    tt_assert(thread->state == ThreadStateRunning);
+
+    if (thread->is_static) {
+        TT_LOG_I(
+            TAG,
+            "%s static task memory will not be reclaimed",
+            thread->name ? thread->name : "<unnamed service>"
+        );
+    }
+
+    thread_set_state(thread, ThreadStateStopped);
+
+    vTaskSetThreadLocalStoragePointer(nullptr, 0, nullptr);
+    thread->task_handle = nullptr;
+
+    vTaskDelete(nullptr);
+    thread_catch();
+}
+
+Thread* thread_alloc() {
+    auto* thread = static_cast<Thread*>(malloc(sizeof(Thread)));
+    // TODO: create default struct instead of using memset()
+    memset(thread, 0, sizeof(Thread));
+    thread->is_static = false;
+
+    Thread* parent = nullptr;
+    if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
+        // TLS is not available, if we called not from thread context
+        parent = (Thread*)pvTaskGetThreadLocalStoragePointer(nullptr, 0);
+
+        if (parent && parent->appid) {
+            thread_set_appid(thread, parent->appid);
+        } else {
+            thread_set_appid(thread, "unknown");
+        }
+    } else {
+        // If scheduler is not started, we are starting driver thread
+        thread_set_appid(thread, "driver");
+    }
+
+    return thread;
+}
+
+Thread* thread_alloc_ex(
+    const char* name,
+    uint32_t stack_size,
+    ThreadCallback callback,
+    void* context
+) {
+    Thread* thread = thread_alloc();
+    thread_set_name(thread, name);
+    thread_set_stack_size(thread, stack_size);
+    thread_set_callback(thread, callback);
+    thread_set_context(thread, context);
+    return thread;
+}
+
+void thread_free(Thread* thread) {
+    tt_assert(thread);
+
+    // Ensure that use join before free
+    tt_assert(thread->state == ThreadStateStopped);
+    tt_assert(thread->task_handle == nullptr);
+
+    if (thread->name) free(thread->name);
+    if (thread->appid) free(thread->appid);
+
+    free(thread);
+}
+
+void thread_set_name(Thread* thread, const char* name) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    if (thread->name) free(thread->name);
+    thread->name = name ? strdup(name) : nullptr;
+}
+
+void thread_set_appid(Thread* thread, const char* appid) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    if (thread->appid) free(thread->appid);
+    thread->appid = appid ? strdup(appid) : nullptr;
+}
+
+void thread_mark_as_static(Thread* thread) {
+    thread->is_static = true;
+}
+
+bool thread_mark_is_static(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    assert(!TT_IS_IRQ_MODE() && (hTask != nullptr));
+    auto* thread = (Thread*)pvTaskGetThreadLocalStoragePointer(hTask, 0);
+    assert(thread != nullptr);
+    return thread->is_static;
+}
+
+void thread_set_stack_size(Thread* thread, size_t stack_size) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    tt_assert(stack_size % 4 == 0);
+    thread->stack_size = stack_size;
+}
+
+void thread_set_callback(Thread* thread, ThreadCallback callback) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    thread->callback = callback;
+}
+
+void thread_set_context(Thread* thread, void* context) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    thread->context = context;
+}
+
+void thread_set_priority(Thread* thread, ThreadPriority priority) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    tt_assert(priority >= 0 && priority <= TT_CONFIG_THREAD_MAX_PRIORITIES);
+    thread->priority = priority;
+}
+
+void thread_set_current_priority(ThreadPriority priority) {
+    UBaseType_t new_priority = priority ? priority : ThreadPriorityNormal;
+    vTaskPrioritySet(nullptr, new_priority);
+}
+
+ThreadPriority thread_get_current_priority() {
+    return (ThreadPriority)uxTaskPriorityGet(nullptr);
+}
+
+void thread_set_state_callback(Thread* thread, ThreadStateCallback callback) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    thread->state_callback = callback;
+}
+
+void thread_set_state_context(Thread* thread, void* context) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    thread->state_context = context;
+}
+
+ThreadState thread_get_state(Thread* thread) {
+    tt_assert(thread);
+    return thread->state;
+}
+
+void thread_start(Thread* thread) {
+    tt_assert(thread);
+    tt_assert(thread->callback);
+    tt_assert(thread->state == ThreadStateStopped);
+    tt_assert(thread->stack_size > 0 && thread->stack_size < (UINT16_MAX * sizeof(StackType_t)));
+
+    thread_set_state(thread, ThreadStateStarting);
+
+    uint32_t stack = thread->stack_size / sizeof(StackType_t);
+    UBaseType_t priority = thread->priority ? thread->priority : ThreadPriorityNormal;
+    if (thread->is_static) {
+#if configSUPPORT_STATIC_ALLOCATION == 1
+        thread->task_handle = xTaskCreateStatic(
+            thread_body,
+            thread->name,
+            stack,
+            thread,
+            priority,
+            static_cast<StackType_t*>(malloc(sizeof(StackType_t) * stack)),
+            static_cast<StaticTask_t*>(malloc(sizeof(StaticTask_t)))
+        );
+#else
+        TT_LOG_E(TAG, "static tasks are not supported by current FreeRTOS config/platform - creating regular one");
+        BaseType_t ret = xTaskCreate(
+            thread_body, thread->name, stack, thread, priority, &(thread->task_handle)
+        );
+        tt_check(ret == pdPASS);
+#endif
+    } else {
+        BaseType_t ret = xTaskCreate(
+            thread_body, thread->name, stack, thread, priority, &(thread->task_handle)
+        );
+        tt_check(ret == pdPASS);
+    }
+
+    tt_check(thread->state == ThreadStateStopped || thread->task_handle);
+}
+
+bool thread_join(Thread* thread) {
+    tt_assert(thread);
+
+    tt_check(thread_get_current() != thread);
+
+    // !!! IMPORTANT NOTICE !!!
+    //
+    // If your thread exited, but your app stuck here: some other thread uses
+    // all cpu time, which delays kernel from releasing task handle
+    while (thread->task_handle) {
+        delay_ms(10);
+    }
+
+    return true;
+}
+
+ThreadId thread_get_id(Thread* thread) {
+    tt_assert(thread);
+    return thread->task_handle;
+}
+
+int32_t thread_get_return_code(Thread* thread) {
+    tt_assert(thread);
+    tt_assert(thread->state == ThreadStateStopped);
+    return thread->ret;
+}
+
+ThreadId thread_get_current_id() {
+    return xTaskGetCurrentTaskHandle();
+}
+
+Thread* thread_get_current() {
+    auto* thread = static_cast<Thread*>(pvTaskGetThreadLocalStoragePointer(nullptr, 0));
+    return thread;
+}
+
+void thread_yield() {
+    tt_assert(!TT_IS_IRQ_MODE());
+    taskYIELD();
+}
+
+uint32_t thread_flags_set(ThreadId thread_id, uint32_t flags) {
+    auto hTask = (TaskHandle_t)thread_id;
+    uint32_t rflags;
+    BaseType_t yield;
+
+    if ((hTask == nullptr) || ((flags & THREAD_FLAGS_INVALID_BITS) != 0U)) {
+        rflags = (uint32_t)TtStatusErrorParameter;
+    } else {
+        rflags = (uint32_t)TtStatusError;
+
+        if (TT_IS_IRQ_MODE()) {
+            yield = pdFALSE;
+
+            (void)xTaskNotifyIndexedFromISR(hTask, THREAD_NOTIFY_INDEX, flags, eSetBits, &yield);
+            (void)xTaskNotifyAndQueryIndexedFromISR(
+                hTask, THREAD_NOTIFY_INDEX, 0, eNoAction, &rflags, nullptr
+            );
+
+            portYIELD_FROM_ISR(yield);
+        } else {
+            (void)xTaskNotifyIndexed(hTask, THREAD_NOTIFY_INDEX, flags, eSetBits);
+            (void)xTaskNotifyAndQueryIndexed(hTask, THREAD_NOTIFY_INDEX, 0, eNoAction, &rflags);
+        }
+    }
+    /* Return flags after setting */
+    return (rflags);
+}
+
+uint32_t thread_flags_clear(uint32_t flags) {
+    TaskHandle_t hTask;
+    uint32_t rflags, cflags;
+
+    if (TT_IS_IRQ_MODE()) {
+        rflags = (uint32_t)TtStatusErrorISR;
+    } else if ((flags & THREAD_FLAGS_INVALID_BITS) != 0U) {
+        rflags = (uint32_t)TtStatusErrorParameter;
+    } else {
+        hTask = xTaskGetCurrentTaskHandle();
+
+        if (xTaskNotifyAndQueryIndexed(hTask, THREAD_NOTIFY_INDEX, 0, eNoAction, &cflags) ==
+            pdPASS) {
+            rflags = cflags;
+            cflags &= ~flags;
+
+            if (xTaskNotifyIndexed(hTask, THREAD_NOTIFY_INDEX, cflags, eSetValueWithOverwrite) !=
+                pdPASS) {
+                rflags = (uint32_t)TtStatusError;
+            }
+        } else {
+            rflags = (uint32_t)TtStatusError;
+        }
+    }
+
+    /* Return flags before clearing */
+    return (rflags);
+}
+
+uint32_t thread_flags_get() {
+    TaskHandle_t hTask;
+    uint32_t rflags;
+
+    if (TT_IS_IRQ_MODE()) {
+        rflags = (uint32_t)TtStatusErrorISR;
+    } else {
+        hTask = xTaskGetCurrentTaskHandle();
+
+        if (xTaskNotifyAndQueryIndexed(hTask, THREAD_NOTIFY_INDEX, 0, eNoAction, &rflags) !=
+            pdPASS) {
+            rflags = (uint32_t)TtStatusError;
+        }
+    }
+
+    return (rflags);
+}
+
+uint32_t thread_flags_wait(uint32_t flags, uint32_t options, uint32_t timeout) {
+    uint32_t rflags, nval;
+    uint32_t clear;
+    TickType_t t0, td, tout;
+    BaseType_t rval;
+
+    if (TT_IS_IRQ_MODE()) {
+        rflags = (uint32_t)TtStatusErrorISR;
+    } else if ((flags & THREAD_FLAGS_INVALID_BITS) != 0U) {
+        rflags = (uint32_t)TtStatusErrorParameter;
+    } else {
+        if ((options & TtFlagNoClear) == TtFlagNoClear) {
+            clear = 0U;
+        } else {
+            clear = flags;
+        }
+
+        rflags = 0U;
+        tout = timeout;
+
+        t0 = xTaskGetTickCount();
+        do {
+            rval = xTaskNotifyWaitIndexed(THREAD_NOTIFY_INDEX, 0, clear, &nval, tout);
+
+            if (rval == pdPASS) {
+                rflags &= flags;
+                rflags |= nval;
+
+                if ((options & TtFlagWaitAll) == TtFlagWaitAll) {
+                    if ((flags & rflags) == flags) {
+                        break;
+                    } else {
+                        if (timeout == 0U) {
+                            rflags = (uint32_t)TtStatusErrorResource;
+                            break;
+                        }
+                    }
+                } else {
+                    if ((flags & rflags) != 0) {
+                        break;
+                    } else {
+                        if (timeout == 0U) {
+                            rflags = (uint32_t)TtStatusErrorResource;
+                            break;
+                        }
+                    }
+                }
+
+                /* Update timeout */
+                td = xTaskGetTickCount() - t0;
+
+                if (td > tout) {
+                    tout = 0;
+                } else {
+                    tout -= td;
+                }
+            } else {
+                if (timeout == 0) {
+                    rflags = (uint32_t)TtStatusErrorResource;
+                } else {
+                    rflags = (uint32_t)TtStatusErrorTimeout;
+                }
+            }
+        } while (rval != pdFAIL);
+    }
+
+    /* Return flags before clearing */
+    return (rflags);
+}
+
+const char* thread_get_name(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    const char* name;
+
+    if (TT_IS_IRQ_MODE() || (hTask == nullptr)) {
+        name = nullptr;
+    } else {
+        name = pcTaskGetName(hTask);
+    }
+
+    return (name);
+}
+
+const char* thread_get_appid(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    const char* appid = "system";
+
+    if (!TT_IS_IRQ_MODE() && (hTask != nullptr)) {
+        auto* thread = (Thread*)pvTaskGetThreadLocalStoragePointer(hTask, 0);
+        if (thread) {
+            appid = thread->appid;
+        }
+    }
+
+    return (appid);
+}
+
+uint32_t thread_get_stack_space(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    uint32_t sz;
+
+    if (TT_IS_IRQ_MODE() || (hTask == nullptr)) {
+        sz = 0U;
+    } else {
+        sz = (uint32_t)(uxTaskGetStackHighWaterMark(hTask) * sizeof(StackType_t));
+    }
+
+    return (sz);
+}
+
+void thread_suspend(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    vTaskSuspend(hTask);
+}
+
+void thread_resume(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    if (TT_IS_IRQ_MODE()) {
+        xTaskResumeFromISR(hTask);
+    } else {
+        vTaskResume(hTask);
+    }
+}
+
+bool thread_is_suspended(ThreadId thread_id) {
+    auto hTask = (TaskHandle_t)thread_id;
+    return eTaskGetState(hTask) == eSuspended;
+}
+
+} // namespace

TactilityCore/Source/Thread.h

@@ -0,0 +1,283 @@
+#pragma once
+
+#include "CoreDefines.h"
+#include "CoreTypes.h"
+
+#include <cstddef>
+#include <cstdint>
+
+namespace tt {
+
+/** ThreadState */
+typedef enum {
+    ThreadStateStopped,
+    ThreadStateStarting,
+    ThreadStateRunning,
+} ThreadState;
+
+/** ThreadPriority */
+typedef enum {
+    ThreadPriorityNone = 0, /**< Uninitialized, choose system default */
+    ThreadPriorityIdle = 1,
+    ThreadPriorityLowest = 2,
+    ThreadPriorityLow = 3,
+    ThreadPriorityNormal = 4,
+    ThreadPriorityHigh = 5,
+    ThreadPriorityHigher = 6,
+    ThreadPriorityHighest = 7
+} ThreadPriority;
+
+#define THREAD_PRIORITY_APP ThreadPriorityNormal
+#define THREAD_PRIORITY_SERVICE ThreadPriorityHigh
+#define THREAD_PRIORITY_RENDER ThreadPriorityHigher
+#define THREAD_PRIORITY_ISR (TT_CONFIG_THREAD_MAX_PRIORITIES - 1)
+
+/** Thread anonymous structure */
+typedef struct Thread Thread;
+
+/** ThreadId proxy type to OS low level functions */
+typedef void* ThreadId;
+
+/** ThreadCallback Your callback to run in new thread
+ * @warning    never use osThreadExit in Thread
+ */
+typedef int32_t (*ThreadCallback)(void* context);
+
+/** Write to stdout callback
+ * @param      data     pointer to data
+ * @param      size     data size @warning your handler must consume everything
+ */
+typedef void (*ThreadStdoutWriteCallback)(const char* data, size_t size);
+
+/** Thread state change callback called upon thread state change
+ * @param      state    new thread state
+ * @param      context  callback context
+ */
+typedef void (*ThreadStateCallback)(ThreadState state, void* context);
+
+/** Allocate Thread
+ *
+ * @return     Thread instance
+ */
+Thread* thread_alloc();
+
+/** Allocate Thread, shortcut version
+ * 
+ * @param name 
+ * @param stack_size 
+ * @param callback 
+ * @param context 
+ * @return Thread*
+ */
+Thread* thread_alloc_ex(
+    const char* name,
+    uint32_t stack_size,
+    ThreadCallback callback,
+    void* context
+);
+
+/** Release Thread
+ *
+ * @warning    see tt_thread_join
+ *
+ * @param      thread  Thread instance
+ */
+void thread_free(Thread* thread);
+
+/** Set Thread name
+ *
+ * @param      thread  Thread instance
+ * @param      name    string
+ */
+void thread_set_name(Thread* thread, const char* name);
+
+/**
+ * @brief Set Thread appid
+ * Technically, it is like a "process id", but it is not a system-wide unique identifier.
+ * All threads spawned by the same app will have the same appid.
+ * 
+ * @param thread 
+ * @param appid 
+ */
+void thread_set_appid(Thread* thread, const char* appid);
+
+/** Mark thread as service
+ * The service cannot be stopped or removed, and cannot exit from the thread body
+ * 
+ * @param thread 
+ */
+void thread_mark_as_static(Thread* thread);
+
+/** Set Thread stack size
+ *
+ * @param      thread      Thread instance
+ * @param      stack_size  stack size in bytes
+ */
+void thread_set_stack_size(Thread* thread, size_t stack_size);
+
+/** Set Thread callback
+ *
+ * @param      thread    Thread instance
+ * @param      callback  ThreadCallback, called upon thread run
+ */
+void thread_set_callback(Thread* thread, ThreadCallback callback);
+
+/** Set Thread context
+ *
+ * @param      thread   Thread instance
+ * @param      context  pointer to context for thread callback
+ */
+void thread_set_context(Thread* thread, void* context);
+
+/** Set Thread priority
+ *
+ * @param      thread   Thread instance
+ * @param      priority ThreadPriority value
+ */
+void thread_set_priority(Thread* thread, ThreadPriority priority);
+
+/** Set current thread priority
+ *
+ * @param      priority ThreadPriority value
+ */
+void thread_set_current_priority(ThreadPriority priority);
+
+/** Get current thread priority
+ *
+ * @return     ThreadPriority value
+ */
+ThreadPriority thread_get_current_priority();
+
+/** Set Thread state change callback
+ *
+ * @param      thread    Thread instance
+ * @param      callback  state change callback
+ */
+void thread_set_state_callback(Thread* thread, ThreadStateCallback callback);
+
+/** Set Thread state change context
+ *
+ * @param      thread   Thread instance
+ * @param      context  pointer to context
+ */
+void thread_set_state_context(Thread* thread, void* context);
+
+/** Get Thread state
+ *
+ * @param      thread  Thread instance
+ *
+ * @return     thread state from ThreadState
+ */
+ThreadState thread_get_state(Thread* thread);
+
+/** Start Thread
+ *
+ * @param      thread  Thread instance
+ */
+void thread_start(Thread* thread);
+
+/** Join Thread
+ *
+ * @warning    Use this method only when CPU is not busy(Idle task receives
+ *             control), otherwise it will wait forever.
+ *
+ * @param      thread  Thread instance
+ *
+ * @return     bool
+ */
+bool thread_join(Thread* thread);
+
+/** Get FreeRTOS ThreadId for Thread instance
+ *
+ * @param      thread  Thread instance
+ *
+ * @return     ThreadId or NULL
+ */
+ThreadId thread_get_id(Thread* thread);
+
+/** Get thread return code
+ *
+ * @param      thread  Thread instance
+ *
+ * @return     return code
+ */
+int32_t thread_get_return_code(Thread* thread);
+
+/** Thread related methods that doesn't involve Thread directly */
+
+/** Get FreeRTOS ThreadId for current thread
+ *
+ * @param      thread  Thread instance
+ *
+ * @return     ThreadId or NULL
+ */
+ThreadId thread_get_current_id();
+
+/** Get Thread instance for current thread
+ * 
+ * @return pointer to Thread or NULL if this thread doesn't belongs to Tactility
+ */
+Thread* thread_get_current();
+
+/** Return control to scheduler */
+void thread_yield();
+
+uint32_t thread_flags_set(ThreadId thread_id, uint32_t flags);
+
+uint32_t thread_flags_clear(uint32_t flags);
+
+uint32_t thread_flags_get();
+
+uint32_t thread_flags_wait(uint32_t flags, uint32_t options, uint32_t timeout);
+
+/**
+ * @brief Get thread name
+ * 
+ * @param thread_id 
+ * @return const char* name or NULL
+ */
+const char* thread_get_name(ThreadId thread_id);
+
+/**
+ * @brief Get thread appid
+ * 
+ * @param thread_id 
+ * @return const char* appid
+ */
+const char* thread_get_appid(ThreadId thread_id);
+
+/**
+ * @brief Get thread stack watermark
+ * 
+ * @param thread_id 
+ * @return uint32_t 
+ */
+uint32_t thread_get_stack_space(ThreadId thread_id);
+
+/** Suspend thread
+ * 
+ * @param thread_id thread id
+ */
+void thread_suspend(ThreadId thread_id);
+
+/** Resume thread
+ * 
+ * @param thread_id thread id
+ */
+void thread_resume(ThreadId thread_id);
+
+/** Get thread suspended state
+ * 
+ * @param thread_id thread id
+ * @return true if thread is suspended
+ */
+bool thread_is_suspended(ThreadId thread_id);
+
+/** Check if the thread was created with static memory
+ *
+ * @param thread_id  thread id
+ * @return true if thread memory is static
+ */
+bool thread_mark_is_static(ThreadId thread_id);
+
+} // namespace

TactilityCore/Source/Timer.cpp

@@ -0,0 +1,160 @@
+#include "Timer.h"
+#include "Check.h"
+#include "Kernel.h"
+#include <cstdlib>
+
+#ifdef ESP_PLATFORM
+#include "freertos/FreeRTOS.h"
+#include "freertos/timers.h"
+#else
+#include "FreeRTOS.h"
+#include "timers.h"
+#endif
+
+namespace tt {
+
+typedef struct {
+    TimerCallback func;
+    void* context;
+} TimerCallback_t;
+
+static void timer_callback(TimerHandle_t hTimer) {
+    auto* callback = static_cast<TimerCallback_t*>(pvTimerGetTimerID(hTimer));
+
+    if (callback != nullptr) {
+        callback->func(callback->context);
+    }
+}
+
+Timer* timer_alloc(TimerCallback func, TimerType type, void* context) {
+    tt_assert((kernel_is_irq() == 0U) && (func != nullptr));
+    auto* callback = static_cast<TimerCallback_t*>(malloc(sizeof(TimerCallback_t)));
+
+    callback->func = func;
+    callback->context = context;
+
+    UBaseType_t reload;
+    if (type == TimerTypeOnce) {
+        reload = pdFALSE;
+    } else {
+        reload = pdTRUE;
+    }
+
+    // TimerCallback function is always provided as a callback and is used to call application
+    // specified function with its context both stored in structure callb.
+    // TODO: should we use pointer to function or function directly as-is?
+    TimerHandle_t hTimer = xTimerCreate(nullptr, portMAX_DELAY, (BaseType_t)reload, callback, timer_callback);
+    tt_assert(hTimer);
+
+    /* Return timer ID */
+    return (Timer*)hTimer;
+}
+
+void timer_free(Timer* instance) {
+    tt_assert(!kernel_is_irq());
+    tt_assert(instance);
+
+    auto hTimer = static_cast<TimerHandle_t>(instance);
+    auto* callback = static_cast<TimerCallback_t*>(pvTimerGetTimerID(hTimer));
+
+    tt_check(xTimerDelete(hTimer, portMAX_DELAY) == pdPASS);
+
+    while (timer_is_running(instance)) delay_tick(2);
+
+    /* Return allocated memory to dynamic pool */
+    free(callback);
+}
+
+TtStatus timer_start(Timer* instance, uint32_t ticks) {
+    tt_assert(!kernel_is_irq());
+    tt_assert(instance);
+    tt_assert(ticks < portMAX_DELAY);
+
+    auto hTimer = static_cast<TimerHandle_t>(instance);
+    TtStatus stat;
+
+    if (xTimerChangePeriod(hTimer, ticks, portMAX_DELAY) == pdPASS) {
+        stat = TtStatusOk;
+    } else {
+        stat = TtStatusErrorResource;
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+TtStatus timer_restart(Timer* instance, uint32_t ticks) {
+    tt_assert(!kernel_is_irq());
+    tt_assert(instance);
+    tt_assert(ticks < portMAX_DELAY);
+
+    auto hTimer = static_cast<TimerHandle_t>(instance);
+    TtStatus stat;
+
+    if (xTimerChangePeriod(hTimer, ticks, portMAX_DELAY) == pdPASS &&
+        xTimerReset(hTimer, portMAX_DELAY) == pdPASS) {
+        stat = TtStatusOk;
+    } else {
+        stat = TtStatusErrorResource;
+    }
+
+    /* Return execution status */
+    return (stat);
+}
+
+TtStatus timer_stop(Timer* instance) {
+    tt_assert(!kernel_is_irq());
+    tt_assert(instance);
+
+    auto hTimer = static_cast<TimerHandle_t>(instance);
+
+    tt_check(xTimerStop(hTimer, portMAX_DELAY) == pdPASS);
+
+    return TtStatusOk;
+}
+
+uint32_t timer_is_running(Timer* instance) {
+    tt_assert(!kernel_is_irq());
+    tt_assert(instance);
+
+    auto hTimer = static_cast<TimerHandle_t>(instance);
+
+    /* Return 0: not running, 1: running */
+    return (uint32_t)xTimerIsTimerActive(hTimer);
+}
+
+uint32_t timer_get_expire_time(Timer* instance) {
+    tt_assert(!kernel_is_irq());
+    tt_assert(instance);
+
+    auto hTimer = static_cast<TimerHandle_t>(instance);
+
+    return (uint32_t)xTimerGetExpiryTime(hTimer);
+}
+
+void timer_pending_callback(TimerPendigCallback callback, void* context, uint32_t arg) {
+    BaseType_t ret = pdFAIL;
+    if (kernel_is_irq()) {
+        ret = xTimerPendFunctionCallFromISR(callback, context, arg, nullptr);
+    } else {
+        ret = xTimerPendFunctionCall(callback, context, arg, TtWaitForever);
+    }
+    tt_assert(ret == pdPASS);
+}
+
+void timer_set_thread_priority(TimerThreadPriority priority) {
+    tt_assert(!kernel_is_irq());
+
+    TaskHandle_t task_handle = xTimerGetTimerDaemonTaskHandle();
+    tt_assert(task_handle); // Don't call this method before timer task start
+
+    if (priority == TimerThreadPriorityNormal) {
+        vTaskPrioritySet(task_handle, configTIMER_TASK_PRIORITY);
+    } else if (priority == TimerThreadPriorityElevated) {
+        vTaskPrioritySet(task_handle, configMAX_PRIORITIES - 1);
+    } else {
+        tt_crash("Unsupported timer priority");
+    }
+}
+
+} // namespace

TactilityCore/Source/Timer.h

@@ -0,0 +1,102 @@
+#pragma once
+
+#include "CoreTypes.h"
+
+namespace tt {
+
+typedef void (*TimerCallback)(void* context);
+
+typedef enum {
+    TimerTypeOnce = 0,    ///< One-shot timer.
+    TimerTypePeriodic = 1 ///< Repeating timer.
+} TimerType;
+
+typedef void Timer;
+
+/** Allocate timer
+ *
+ * @param[in]  func     The callback function
+ * @param[in]  type     The timer type
+ * @param      context  The callback context
+ *
+ * @return     The pointer to Timer instance
+ */
+Timer* timer_alloc(TimerCallback func, TimerType type, void* context);
+
+/** Free timer
+ *
+ * @param      instance  The pointer to Timer instance
+ */
+void timer_free(Timer* instance);
+
+/** Start timer
+ *
+ * @warning    This is asynchronous call, real operation will happen as soon as
+ *             timer service process this request.
+ *
+ * @param      instance  The pointer to Timer instance
+ * @param[in]  ticks     The interval in ticks
+ *
+ * @return     The status.
+ */
+TtStatus timer_start(Timer* instance, uint32_t ticks);
+
+/** Restart timer with previous timeout value
+ *
+ * @warning    This is asynchronous call, real operation will happen as soon as
+ *             timer service process this request.
+ *
+ * @param      instance  The pointer to Timer instance
+ * @param[in]  ticks     The interval in ticks
+ *
+ * @return     The status.
+ */
+TtStatus timer_restart(Timer* instance, uint32_t ticks);
+
+/** Stop timer
+ *
+ * @warning    This is asynchronous call, real operation will happen as soon as
+ *             timer service process this request.
+ *
+ * @param      instance  The pointer to Timer instance
+ *
+ * @return     The status.
+ */
+TtStatus timer_stop(Timer* instance);
+
+/** Is timer running
+ *
+ * @warning    This cal may and will return obsolete timer state if timer
+ *             commands are still in the queue. Please read FreeRTOS timer
+ *             documentation first.
+ *
+ * @param      instance  The pointer to Timer instance
+ *
+ * @return     0: not running, 1: running
+ */
+uint32_t timer_is_running(Timer* instance);
+
+/** Get timer expire time
+ *
+ * @param      instance  The Timer instance
+ *
+ * @return     expire tick
+ */
+uint32_t timer_get_expire_time(Timer* instance);
+
+typedef void (*TimerPendigCallback)(void* context, uint32_t arg);
+
+void timer_pending_callback(TimerPendigCallback callback, void* context, uint32_t arg);
+
+typedef enum {
+    TimerThreadPriorityNormal,   /**< Lower then other threads */
+    TimerThreadPriorityElevated, /**< Same as other threads */
+} TimerThreadPriority;
+
+/** Set Timer thread priority
+ *
+ * @param[in]  priority  The priority
+ */
+void timer_set_thread_priority(TimerThreadPriority priority);
+
+} // namespace