stk.h

Go to the documentation of this file.

/*
 * SuperTinyKernel(TM) RTOS: Lightweight High-Performance Deterministic C++ RTOS for Embedded Systems.
 *
 * Source: https://github.com/SuperTinyKernel-RTOS
 *
 * Copyright (c) 2022-2026 Neutron Code Limited <stk@neutroncode.com>. All Rights Reserved.
 * License: MIT License, see LICENSE for a full text.
 */
 
#ifndef STK_H_
#define STK_H_
 
#include "stk_helper.h"
#include "strategy/stk_strategy_rrobin.h"
#include "strategy/stk_strategy_swrrobin.h"
#include "strategy/stk_strategy_monotonic.h"
#include "strategy/stk_strategy_edf.h"
#include "strategy/stk_strategy_fpriority.h"

 
namespace stk {

template <uint8_t TMode, uint32_t TSize, class TStrategy, class TPlatform>
class Kernel : public IKernel, private IPlatform::IEventHandler
{
protected:
    typedef StackMemoryWrapper<STK_SLEEP_TRAP_STACK_SIZE> SleepTrapStackMemory;

    typedef StackMemoryWrapper<STACK_SIZE_MIN> ExitTrapStackMemory;

    enum ERequest : uint8_t
    {
        REQUEST_NONE     = 0,       
        REQUEST_ADD_TASK = (1 << 0) 
    };

    class KernelTask : public IKernelTask
    {
        friend class Kernel;

        enum EStateFlags
        {
            STATE_NONE           = 0,        
            STATE_REMOVE_PENDING = (1 << 0), 
            STATE_SLEEP_PENDING  = (1 << 1)  
        };
 
    public:
        struct AddTaskRequest
        {
            ITask *user_task; 
        };

        explicit KernelTask() : m_user(nullptr), m_stack(), m_state(STATE_NONE), m_time_sleep(0),
            m_srt(), m_hrt(), m_rt_weight()
        {
            // bind to wait object
            if (IsSyncMode())
                m_wait_obj->m_task = this;
        }

        ITask *GetUserTask() { return m_user; }

        Stack *GetUserStack() { return &m_stack; }

        bool IsBusy() const { return (m_user != nullptr); }

        bool IsSleeping() const { return (m_time_sleep < 0); }

        TId GetTid() const { return hw::PtrToWord(m_user); }

        void Wake()
        {
            STK_ASSERT(IsSleeping());
 
            // wakeup on a next cycle
            m_time_sleep = -1;
        }

        void SetCurrentWeight(int32_t weight)
        {
            if (TStrategy::WEIGHT_API)
                m_rt_weight[0] = weight;
        }

        int32_t GetWeight() const { return (TStrategy::WEIGHT_API ? m_user->GetWeight() : 1); }

        int32_t GetCurrentWeight() const { return (TStrategy::WEIGHT_API ? m_rt_weight[0] : 1); }

        Timeout GetHrtPeriodicity() const
        {
            STK_ASSERT(IsHrtMode());
 
            return (IsHrtMode() ? m_hrt[0].periodicity : 0);
        }

        Timeout GetHrtDeadline() const
        {
            STK_ASSERT(IsHrtMode());
 
            return (IsHrtMode() ? m_hrt[0].deadline : 0);
        }

        Timeout GetHrtRelativeDeadline() const
        {
            STK_ASSERT(IsHrtMode());
            STK_ASSERT(!IsSleeping());
 
            return (IsHrtMode() ? (m_hrt[0].deadline - m_hrt[0].duration) : 0);
        }
 
        Timeout GetSleepTicks(Timeout sleep_ticks)
        {
            // note: task sleep time is negative
            Timeout task_sleep = Max<Timeout>(0, -m_time_sleep);
 
            if (IsSyncMode())
            {
                // likely task is sleeping during sync operation (see Wait)
                if (m_wait_obj->IsWaiting())
                {
                    // note: sync wait time is positive
                    task_sleep = m_wait_obj->m_time_wait;
 
                    // we shall account for only valid time (when task is waiting during sync operation)
                    if (task_sleep > 0)
                        sleep_ticks = Min(sleep_ticks, task_sleep);
                }
                else
                {
                    sleep_ticks = Min(sleep_ticks, task_sleep);
                }
            }
            else
            {
                sleep_ticks = Min(sleep_ticks, task_sleep);
            }
 
            // clamp to [1, STK_TICKLESS_TICKS_MAX] range
            return Max<Timeout>(1, sleep_ticks);
        }
 
    protected:
        ~KernelTask()
        {}

        struct SrtInfo
        {
            SrtInfo() : add_task_req(nullptr)
            {}

            void Clear()
            {
                add_task_req = nullptr;
            }

            AddTaskRequest *add_task_req;
        };

        struct HrtInfo
        {
            HrtInfo() : periodicity(0), deadline(0), duration(0), done(false)
            {}

            void Clear()
            {
                periodicity = 0;
                deadline    = 0;
                duration    = 0;
                done        = false;
            }
 
            Timeout periodicity; 
            Timeout deadline;    
            Timeout duration;    
            volatile bool done;  
        };

        struct WaitObject : public IWaitObject
        {
            explicit WaitObject() : m_task(nullptr), m_sync_obj(nullptr), m_timeout(false), m_time_wait(0)
            {}

            ~WaitObject()
            {}

            struct WaitRequest
            {
                ISyncObject *sync_obj; 
            };

            TId GetTid() const { return m_task->GetTid(); }

            bool IsTimeout() const { return m_timeout; }

            bool IsWaiting() const { return (m_sync_obj != nullptr); }

            void Wake(bool timeout)
            {
                STK_ASSERT(IsWaiting());
 
                m_timeout   = timeout;
                m_time_wait = 0;
 
                m_sync_obj->RemoveWaitObject(this);
                m_sync_obj = nullptr;
 
                return m_task->Wake();
            }

            bool Tick(Timeout elapsed_ticks)
            {
                if ((m_time_wait != WAIT_INFINITE) && !m_timeout)
                {
                    m_time_wait -= elapsed_ticks;
 
                    if (m_time_wait <= 0)
                        m_timeout = true;
                }
 
                return !m_timeout;
            }

            void SetupWait(ISyncObject *sync_obj, Timeout timeout)
            {
                STK_ASSERT(!IsWaiting());
 
                m_sync_obj  = sync_obj;
                m_time_wait = timeout;
                m_timeout   = false;
 
                sync_obj->AddWaitObject(this);
            }
 
            KernelTask   *m_task;      
            ISyncObject  *m_sync_obj;  
            volatile bool m_timeout;   
            Timeout       m_time_wait; 
        };

        void Bind(TPlatform *platform, ITask *user_task)
        {
            // set access mode for this stack
            m_stack.mode = user_task->GetAccessMode();
 
            // set task id for tracking purpose
        #if STK_NEED_TASK_ID
            m_stack.tid = user_task->GetId();
        #endif
 
            // init stack of the user task
            if (!platform->InitStack(STACK_USER_TASK, &m_stack, user_task, user_task))
            {
                STK_ASSERT(false);
            }
 
            // bind user task
            m_user = user_task;
        }

        void Unbind()
        {
            if (IsSyncMode())
            {
                // should be freed from waiting on task exit
                STK_ASSERT(!m_wait_obj->IsWaiting());
            }
 
            m_user       = nullptr;
            m_stack      = {};
            m_state      = STATE_NONE;
            m_time_sleep = 0;
 
            if (IsHrtMode())
                m_hrt[0].Clear();
            else
                m_srt->Clear();
        }

        void ScheduleRemoval()
        {
            // make this task sleeping to switch it out from scheduling process
            ScheduleSleep(WAIT_INFINITE);
 
            // mark it as done HRT task
            if (IsHrtMode())
                HrtOnWorkCompleted();
 
            // mark it as pending for removal
            m_state |= STATE_REMOVE_PENDING;
        }

        bool IsPendingRemoval() const { return ((m_state & STATE_REMOVE_PENDING) != 0U); }

        bool IsMemoryOfSP(Word SP) const
        {
            Word *start = m_user->GetStack();
            Word *end   = start + m_user->GetStackSize();
 
            return (SP >= hw::PtrToWord(start)) && (SP <= hw::PtrToWord(end));
        }

        void HrtInit(Timeout periodicity_tc, Timeout deadline_tc, Timeout start_delay_tc)
        {
            STK_ASSERT(periodicity_tc > 0);
            STK_ASSERT(deadline_tc > 0);
            STK_ASSERT(start_delay_tc >= 0);
            STK_ASSERT(periodicity_tc < INT32_MAX);
            STK_ASSERT(deadline_tc < INT32_MAX);
 
            m_hrt[0].periodicity = periodicity_tc;
            m_hrt[0].deadline    = deadline_tc;
 
            if (start_delay_tc > 0)
                ScheduleSleep(start_delay_tc);
        }

        void HrtOnSwitchedIn() {}

        void HrtOnSwitchedOut(IPlatform */*platform*/)
        {
            const Timeout duration = m_hrt[0].duration;
 
            STK_ASSERT(duration >= 0);
 
            Timeout sleep = m_hrt[0].periodicity - duration;
            if (sleep > 0)
                ScheduleSleep(sleep);
 
            m_hrt[0].duration = 0;
            m_hrt[0].done     = false;
        }

        void HrtHardFailDeadline(IPlatform *platform)
        {
            const Timeout duration = m_hrt[0].duration;
 
            STK_ASSERT(duration >= 0);
            STK_ASSERT(HrtIsDeadlineMissed(duration));
 
            m_user->OnDeadlineMissed(duration);
            platform->ProcessHardFault();
        }

        void HrtOnWorkCompleted()
        {
            m_hrt[0].done = true;
            __stk_full_memfence();
        }

        bool HrtIsDeadlineMissed(Timeout duration) const { return (duration > m_hrt[0].deadline); }

        void ScheduleSleep(Timeout ticks)
        {
            STK_ASSERT(ticks > 0);
 
            // set state first as kernel checks it when task IsSleeping
            if (TStrategy::SLEEP_EVENT_API)
            {
                if (m_time_sleep >= 0)
                    m_state |= STATE_SLEEP_PENDING;
            }
 
            m_time_sleep = -ticks;
            __stk_full_memfence();
        }

        void BusyWaitWhileSleeping() const
        {
            while (IsSleeping())
            {
                __stk_relax_cpu();
            }
        }
 
        ITask            *m_user;       
        Stack             m_stack;      
        volatile uint32_t m_state;      
        volatile Timeout  m_time_sleep; 
        SrtInfo           m_srt[STK_ALLOCATE_COUNT(TMode, KERNEL_HRT, 0, 1)];       
        HrtInfo           m_hrt[STK_ALLOCATE_COUNT(TMode, KERNEL_HRT, 1, 0)];       
        int32_t           m_rt_weight[STK_ALLOCATE_COUNT(TStrategy::WEIGHT_API, 1, 1, 0)]; 
        WaitObject        m_wait_obj[STK_ALLOCATE_COUNT(TMode, KERNEL_SYNC, 1, 0)]; 
    };

    class KernelService : public IKernelService
    {
        friend class Kernel;
 
    public:
        TId GetTid() const { return m_platform->GetTid(); }
 
        Ticks GetTicks() const { return hw::ReadVolatile64(&m_ticks); }
 
        uint32_t GetTickResolution() const  { return m_platform->GetTickResolution(); }
 
        Cycles GetSysTimerCount() const { return m_platform->GetSysTimerCount(); }
 
        uint32_t GetSysTimerFrequency() const { return m_platform->GetSysTimerFrequency(); }
 
        __stk_attr_noinline void Delay(Timeout ticks)
        {
            STK_ASSERT(!hw::IsInsideISR());
            STK_ASSERT(ticks >= 0);
 
            Ticks now = GetTicks();
            const Ticks deadline = now + ticks;
            STK_ASSERT(deadline >= now);
 
            for (; now < deadline; now = GetTicks())
            {
                __stk_relax_cpu();
            }
        }
 
        __stk_attr_noinline void Sleep(Timeout ticks)
        {
            STK_ASSERT(!hw::IsInsideISR());
            STK_ASSERT(ticks >= 0);
 
            if (!IsHrtMode())
            {
                m_platform->Sleep(ticks);
            }
            else
            {
                // sleeping is not supported in HRT mode, task will sleep according to its periodicity and workload
                STK_ASSERT(false);
            }
        }
 
        __stk_attr_noinline void SleepUntil(Ticks timestamp)
        {
            STK_ASSERT(!hw::IsInsideISR());
            STK_ASSERT(timestamp >= 0);
 
            if (!IsHrtMode())
            {
                m_platform->SleepUntil(timestamp);
            }
            else
            {
                // sleeping is not supported in HRT mode, task will sleep according to its periodicity and workload
                STK_ASSERT(false);
            }
        }
 
        void SwitchToNext()
        {
            STK_ASSERT(!hw::IsInsideISR());
 
            m_platform->SwitchToNext();
        }
 
        IWaitObject *Wait(ISyncObject *sobj, IMutex *mutex, Timeout ticks)
        {
            if (IsSyncMode())
            {
                return m_platform->Wait(sobj, mutex, ticks);
            }
            else
            {
                STK_ASSERT(false);
                return nullptr;
            }
        }
 
        Timeout Suspend()
        {
            if (IsTicklessMode())
            {
                return m_platform->Suspend();
            }
            else
            {
                STK_ASSERT(false);
                return 0;
            }
        }
 
        void Resume(Timeout elapsed_ticks)
        {
            if (IsTicklessMode())
            {
                return m_platform->Resume(elapsed_ticks);
            }
            else
            {
                STK_ASSERT(false);
            }
        }
 
    private:
        explicit KernelService() : m_platform(nullptr), m_ticks(0)
        {}

        ~KernelService()
        {}

        void Initialize(IPlatform *platform)
        {
            m_platform = static_cast<TPlatform *>(platform);
        }

        void IncrementTicks(Ticks advance)
        {
            // using WriteVolatile64() to guarantee correct lockless reading order by ReadVolatile64
            hw::WriteVolatile64(&m_ticks, m_ticks + advance);
        }
 
        TPlatform     *m_platform; 
        volatile Ticks m_ticks;    
    };
 
public:
    static constexpr uint32_t TASKS_MAX = TSize;

    explicit Kernel() : m_platform(), m_strategy(), m_task_now(nullptr), m_task_storage(), m_sleep_trap(),
        m_exit_trap(), m_fsm_state(FSM_STATE_NONE), m_request(REQUEST_NONE), m_state(STATE_INACTIVE)
    {
    #ifdef _DEBUG
        // TPlatform must inherit IPlatform
        IPlatform *platform = &m_platform;
        STK_UNUSED(platform);
 
        // TStrategy must inherit ITaskSwitchStrategy
        ITaskSwitchStrategy *strategy = &m_strategy;
        STK_UNUSED(strategy);
    #endif
 
    #if !STK_TICKLESS_IDLE
        STK_STATIC_ASSERT_DESC(((TMode & KERNEL_TICKLESS) == 0U),
            "STK_TICKLESS_IDLE must be defined to 1 for KERNEL_TICKLESS");
    #endif
    }

    ~Kernel()
    {}

    __stk_attr_noinline void Initialize(uint32_t resolution_us = PERIODICITY_DEFAULT)
    {
        STK_ASSERT(resolution_us != 0);
        STK_ASSERT(resolution_us <= PERIODICITY_MAX);
        STK_ASSERT(!IsInitialized());
 
        // reinitialize key state variables
        m_task_now  = nullptr;
        m_fsm_state = FSM_STATE_NONE;
        m_request   = REQUEST_NONE;
 
        m_service.Initialize(&m_platform);
 
        m_platform.Initialize(this, &m_service, resolution_us, (IsDynamicMode() ? &m_exit_trap[0].stack : nullptr));
 
        // now ready to Start()
        m_state = STATE_READY;
    }

    __stk_attr_noinline void AddTask(ITask *user_task)
    {
        if (!IsHrtMode())
        {
            STK_ASSERT(user_task != nullptr);
            STK_ASSERT(IsInitialized());
 
            // when started the operation must be serialized by switching out from processing until
            // kernel processes this request
            if (IsStarted())
            {
                if (IsDynamicMode())
                {
                    RequestAddTask(user_task);
                }
                else
                {
                    STK_ASSERT(false);
                }
            }
            else
            {
                AllocateAndAddNewTask(user_task);
            }
        }
        else
        {
            STK_ASSERT(false);
        }
    }

    __stk_attr_noinline void AddTask(ITask *user_task, Timeout periodicity_tc, Timeout deadline_tc, Timeout start_delay_tc)
    {
        if (IsHrtMode())
        {
            STK_ASSERT(user_task != nullptr);
            STK_ASSERT(IsInitialized());
            STK_ASSERT(!IsStarted());
 
            HrtAllocateAndAddNewTask(user_task, periodicity_tc, deadline_tc, start_delay_tc);
        }
        else
        {
            STK_ASSERT(false);
        }
    }

    __stk_attr_noinline void RemoveTask(ITask *user_task)
    {
        if (IsDynamicMode())
        {
            STK_ASSERT(user_task != nullptr);
            STK_ASSERT(!IsStarted());
 
            KernelTask *task = FindTaskByUserTask(user_task);
            if (task != nullptr)
                RemoveTask(task);
        }
        else
        {
            // kernel operating mode must be KERNEL_DYNAMIC for tasks to be able to be removed
            STK_ASSERT(false);
        }
    }

    __stk_attr_noinline void ScheduleTaskRemoval(ITask *user_task)
    {
        if (IsDynamicMode())
        {
            STK_ASSERT(user_task != nullptr);
            STK_ASSERT(IsStarted());
 
            hw::CriticalSection::ScopedLock cs_;
 
            KernelTask *task = FindTaskByUserTask(user_task);
            if (task != nullptr)
                task->ScheduleRemoval();
        }
        else
        {
            // kernel operating mode must be KERNEL_DYNAMIC for tasks to be able to be removed
            STK_ASSERT(false);
        }
    }

    void SuspendTask(ITask *user_task, bool &suspended)
    {
        STK_ASSERT(user_task != nullptr);
 
        bool self = false;
        KernelTask *task = nullptr;
 
        // avoid race with OnTick
        {
            hw::CriticalSection::ScopedLock cs_;
 
            task = FindTaskByUserTask(user_task);
            STK_ASSERT(task != nullptr);
 
            // only suspend if the task is currently awake: if it is already sleeping
            // (e.g. blocked on a mutex or timed Sleep), do not overwrite m_time_sleep,
            // that would corrupt the original sleep state and, for sync-object waits,
            // would interfere with WaitObject::Tick()
            if ((suspended = !task->IsSleeping()) == true)
            {
                task->ScheduleSleep(WAIT_INFINITE);
 
                // check if suspending self
                self = (task == m_task_now);
            }
        }
 
        // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
        if (self)
            task->BusyWaitWhileSleeping();
    }

    void ResumeTask(ITask *user_task)
    {
        STK_ASSERT(user_task != nullptr);
 
        // avoid race with OnTick
        hw::CriticalSection::ScopedLock cs_;
 
        KernelTask *task = FindTaskByUserTask(user_task);
        STK_ASSERT(task != nullptr);
 
        if (task->IsSleeping())
            task->Wake();
    }

    size_t EnumerateTasks(ITask **user_tasks, const size_t max_size)
    {
        size_t count = 0U;
 
        // avoid race with OnTick
        hw::CriticalSection::ScopedLock cs_;
 
        for (uint32_t i = 0U; i < Min(max_size, static_cast<size_t>(TASKS_MAX)); ++i)
        {
            KernelTask *task = &m_task_storage[i];
            if (task->IsBusy())
                user_tasks[count++] = task->GetUserTask();
        }
 
        return count;
    }

    __stk_attr_noinline void Start()
    {
        STK_ASSERT(IsInitialized());
 
         // stacks of the traps must be re-initilized on every subsequent Start
        InitTraps();
 
        // start tracing
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_Start();
        for (uint32_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *task = &m_task_storage[i];
            if (task->IsBusy())
                SendTaskTraceInfo(task);
        }
    #endif
 
        m_platform.Start();
    }

    bool IsStarted() const
    {
        return (m_task_now != nullptr);
    }

    IPlatform *GetPlatform() { return &m_platform; }

    ITaskSwitchStrategy *GetSwitchStrategy() { return &m_strategy; }

    EState GetState() const { return m_state; }
 
protected:
    enum EFsmState : int8_t
    {
        FSM_STATE_NONE      = -1, 
        FSM_STATE_SWITCHING,      
        FSM_STATE_SLEEPING,       
        FSM_STATE_WAKING,         
        FSM_STATE_EXITING,        
        FSM_STATE_MAX             
    };

    enum EFsmEvent : int8_t
    {
        FSM_EVENT_SWITCH = 0, 
        FSM_EVENT_SLEEP,      
        FSM_EVENT_WAKE,       
        FSM_EVENT_EXIT,       
        FSM_EVENT_MAX         
    };

    static constexpr Timeout YIELD_TICKS = 2;

    static __stk_forceinline bool IsValidFsmState(EFsmState state)
    {
        return (state > FSM_STATE_NONE) &&
               (state < FSM_STATE_MAX);
    }

    __stk_attr_noinline void InitTraps()
    {
        // init stack for a Sleep trap
        {
            SleepTrapStack &sleep = m_sleep_trap[0];
 
            SleepTrapStackMemory wrapper(&sleep.memory);
            sleep.stack.mode = ACCESS_PRIVILEGED;
        #if STK_NEED_TASK_ID
            sleep.stack.tid  = SYS_TASK_ID_SLEEP;
        #endif
 
            m_platform.InitStack(STACK_SLEEP_TRAP, &sleep.stack, &wrapper, nullptr);
        }
 
        // init stack for an Exit trap
        if (IsDynamicMode())
        {
            ExitTrapStack &exit = m_exit_trap[0];
 
            ExitTrapStackMemory wrapper(&exit.memory);
            exit.stack.mode = ACCESS_PRIVILEGED;
        #if STK_NEED_TASK_ID
            exit.stack.tid  = SYS_TASK_ID_EXIT;
        #endif
 
            m_platform.InitStack(STACK_EXIT_TRAP, &exit.stack, &wrapper, nullptr);
        }
    }

    KernelTask *AllocateNewTask(ITask *user_task)
    {
        // look for a free kernel task
        KernelTask *new_task = nullptr;
        for (uint32_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *task = &m_task_storage[i];
            if (task->IsBusy())
            {
                // avoid task collision
                STK_ASSERT(task->m_user != user_task);
 
                // avoid stack collision
                STK_ASSERT(task->m_user->GetStack() != user_task->GetStack());
            }
            else
            if (new_task == nullptr)
            {
                new_task = task;
            #if defined(NDEBUG) && !defined(_STK_ASSERT_REDIRECT)
                break; // break if assertions are inactive and do not try to validate collision with existing tasks
            #endif
            }
        }
 
        // if nullptr - exceeded max supported kernel task count, application design failure
        STK_ASSERT(new_task != nullptr);
 
        new_task->Bind(&m_platform, user_task);
 
        return new_task;
    }

    void AddKernelTask(KernelTask *task)
    {
    #if STK_SEGGER_SYSVIEW
        // start tracing new task
        SEGGER_SYSVIEW_OnTaskCreate(task->GetUserStack()->tid);
        if (IsStarted())
            SendTaskTraceInfo(task);
    #endif
 
        m_strategy.AddTask(task);
    }

    void AllocateAndAddNewTask(ITask *user_task)
    {
        KernelTask *task = AllocateNewTask(user_task);
        STK_ASSERT(task != nullptr);
 
        AddKernelTask(task);
    }

    void HrtAllocateAndAddNewTask(ITask *user_task, Timeout periodicity_tc, Timeout deadline_tc, Timeout start_delay_tc)
    {
        KernelTask *task = AllocateNewTask(user_task);
        STK_ASSERT(task != nullptr);
 
        task->HrtInit(periodicity_tc, deadline_tc, start_delay_tc);
 
        AddKernelTask(task);
    }

    __stk_attr_noinline void RequestAddTask(ITask *user_task)
    {
        STK_ASSERT(IsDynamicMode());
 
        KernelTask *caller = FindTaskBySP(m_platform.GetCallerSP());
        STK_ASSERT(caller != nullptr);
 
        typename KernelTask::AddTaskRequest req = { .user_task = user_task };
        caller->m_srt[0].add_task_req = &req;
 
        // notify kernel
        ScheduleAddTask();
 
        // switch out and wait for completion (due to context switch request could be processed here)
        if (caller->m_srt[0].add_task_req != nullptr)
            m_service.SwitchToNext();
 
        STK_ASSERT(caller->m_srt[0].add_task_req == nullptr);
    }

    __stk_attr_noinline KernelTask *FindTaskByUserTask(const ITask *user_task)
    {
        for (uint32_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *task = &m_task_storage[i];
            if (task->GetUserTask() == user_task)
                return task;
        }
 
        return nullptr;
    }

    KernelTask *FindTaskByStack(const Stack *stack)
    {
        for (uint32_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *task = &m_task_storage[i];
            if (task->GetUserStack() == stack)
                return task;
        }
 
        return nullptr;
    }

    __stk_attr_noinline KernelTask *FindTaskBySP(Word SP)
    {
        STK_ASSERT(m_task_now != nullptr);
 
        if (m_task_now->IsMemoryOfSP(SP))
            return m_task_now;
 
        for (uint32_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *task = &m_task_storage[i];
 
            // skip finished tasks (applicable only for KERNEL_DYNAMIC mode)
            if (IsDynamicMode() && !task->IsBusy())
                continue;
 
            if (task->IsMemoryOfSP(SP))
                return task;
        }
 
        return nullptr;
    }

    void RemoveTask(KernelTask *task)
    {
        STK_ASSERT(task != nullptr);
 
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_OnTaskTerminate(task->GetUserStack()->tid);
    #endif
 
        // notify task about pending exit
        task->GetUserTask()->OnExit();
 
        m_strategy.RemoveTask(task);
        task->Unbind();
    }

    __stk_attr_noinline void OnStart(Stack *&active)
    {
        STK_ASSERT(m_strategy.GetSize() != 0);
 
        // iterate tasks and generate OnTaskSleep for a strategy for all initially sleeping tasks
        if (TStrategy::SLEEP_EVENT_API)
        {
            for (uint32_t i = 0U; i < TASKS_MAX; ++i)
            {
                KernelTask *task = &m_task_storage[i];
 
                if (task->IsSleeping())
                {
                    if ((task->m_state & KernelTask::STATE_SLEEP_PENDING) != 0U)
                    {
                        task->m_state &= ~KernelTask::STATE_SLEEP_PENDING;
 
                        // notify strategy that task is sleeping
                        m_strategy.OnTaskSleep(task);
                    }
                }
            }
        }
 
        // get initial state and first task
        {
            m_fsm_state = FSM_STATE_SWITCHING;
 
            KernelTask *next = nullptr;
            m_fsm_state = GetNewFsmState(next);
 
            // expecting only SLEEPING or SWITCHING states
            STK_ASSERT((m_fsm_state == FSM_STATE_SLEEPING) || (m_fsm_state == FSM_STATE_SWITCHING));
 
            if (m_fsm_state == FSM_STATE_SWITCHING)
            {
                m_task_now = next;
 
                active = next->GetUserStack();
 
                if (IsHrtMode())
                    next->HrtOnSwitchedIn();
            }
            else
            if (m_fsm_state == FSM_STATE_SLEEPING)
            {
                // MISRA 5-2-3 deviation: GetNext/GetFirst returns IKernelTask*, all objects in
                // the strategy pool are KernelTask instances - downcast is guaranteed safe.
                m_task_now = static_cast<KernelTask *>(m_strategy.GetFirst());
 
                active = &m_sleep_trap[0].stack;
            }
        }
 
        // is in running state
        m_state = STATE_RUNNING;
 
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_OnTaskStartExec(m_task_now->tid);
    #endif
    }

    __stk_attr_noinline void OnStop()
    {
        if (IsDynamicMode())
        {
            m_fsm_state = FSM_STATE_NONE;
 
            // is in stopped state, i.e. is ready to Start() again
            m_state = STATE_READY;
        }
    }

    bool OnTick(Stack *&idle, Stack *&active
    #if STK_TICKLESS_IDLE
        , Timeout &ticks
    #endif
    )
    {
    #if !STK_TICKLESS_IDLE
        // in non-tickless mode kernel is advancing strictly by 1 tick on every OnTick call
        enum { ticks = 1 };
    #endif
 
        // advance internal timestamp
        m_service.IncrementTicks(ticks);
 
        // consume elapsed and update to ticks to sleep
    #if STK_TICKLESS_IDLE
        ticks = (
    #else
        // notify compiler that we ignore a return value of UpdateTasks
        static_cast<void>(
    #endif
        UpdateTasks(ticks));
 
        // decide on a context switch
        return UpdateFsmState(idle, active);
    }
 
    void OnTaskSwitch(Word caller_SP)
    {
        OnTaskSleep(caller_SP, YIELD_TICKS);
    }
 
    void OnTaskSleep(Word caller_SP, Timeout ticks)
    {
        KernelTask *task = FindTaskBySP(caller_SP);
        STK_ASSERT(task != nullptr);
 
        // make change to HRT state and sleep time atomic
        {
            hw::CriticalSection::ScopedLock cs_;
 
            if (IsHrtMode())
                task->HrtOnWorkCompleted();
 
            if (ticks > 0)
                task->ScheduleSleep(ticks);
        }
 
        // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
        task->BusyWaitWhileSleeping();
    }
 
    void OnTaskSleepUntil(Word caller_SP, Ticks timestamp)
    {
        STK_ASSERT(!IsHrtMode());
 
        KernelTask *task = FindTaskBySP(caller_SP);
        STK_ASSERT(task != nullptr);
 
        // make change to HRT state and sleep time atomic
        {
            hw::CriticalSection::ScopedLock cs_;
 
            Timeout ticks = Max(static_cast<Timeout>(0), static_cast<Timeout>(timestamp - m_service.m_ticks));
 
            if (ticks > 0)
                task->ScheduleSleep(ticks);
        }
 
        // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
        task->BusyWaitWhileSleeping();
    }
 
    void OnTaskExit(Stack *stack)
    {
        if (IsDynamicMode())
        {
            KernelTask *task = FindTaskByStack(stack);
            STK_ASSERT(task != nullptr);
 
            // notify kernel to execute removal
            task->ScheduleRemoval();
        }
        else
        {
            // kernel operating mode must be KERNEL_DYNAMIC for tasks to be able to exit
            STK_KERNEL_PANIC(KERNEL_PANIC_BAD_MODE);
        }
    }
 
    IWaitObject *OnTaskWait(Word caller_SP, ISyncObject *sync_obj, IMutex *mutex, Timeout timeout)
    {
        if (IsSyncMode())
        {
            STK_ASSERT(timeout != 0);        // API contract: caller must not be in ISR
            STK_ASSERT(sync_obj != nullptr); // API contract: ISyncObject instance must be provided
            STK_ASSERT(mutex != nullptr);    // API contract: IMutex instance must be provided
            STK_ASSERT((sync_obj->GetHead() == nullptr) || (sync_obj->GetHead() == &m_sync_list[0]));
 
            KernelTask *task = FindTaskBySP(caller_SP);
            STK_ASSERT(task != nullptr);
 
            // configure waiting
            task->m_wait_obj->SetupWait(sync_obj, timeout);
 
            // register ISyncObject if not yet
            if (sync_obj->GetHead() == nullptr)
                m_sync_list->LinkBack(sync_obj);
 
            // start sleeping infinitely, we rely on a Wake call via WaitObject
            task->ScheduleSleep(WAIT_INFINITE);
 
            // unlock mutex locked externally, so that we could wait in a busy-waiting loop
            mutex->Unlock();
 
            // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
            task->BusyWaitWhileSleeping();
 
            // re-lock mutex when returning to the task's execution space
            mutex->Lock();
 
            return task->m_wait_obj;
        }
        else
        {
            STK_ASSERT(false);
            return nullptr;
        }
    }
 
    TId OnGetTid(Word caller_SP)
    {
        KernelTask *task = FindTaskBySP(caller_SP);
        STK_ASSERT(task != nullptr);
 
        return task->GetTid();
    }
 
    void OnSuspend(bool suspended)
    {
        if (suspended)
            m_state = ((m_state == STATE_RUNNING) ? STATE_SUSPENDED : m_state);
        else
            m_state = ((m_state == STATE_SUSPENDED) ? STATE_RUNNING : m_state);
    }

    Timeout UpdateTasks(const Timeout elapsed_ticks)
    {
        // sync objects are updated before UpdateTaskRequest which may add a new object (newly added object must become 1 tick older)
        if (IsSyncMode())
            UpdateSyncObjects(elapsed_ticks);
 
        UpdateTaskRequest();
 
        return UpdateTaskState(elapsed_ticks);
    }

    Timeout UpdateTaskState(const Timeout elapsed_ticks)
    {
        Timeout sleep_ticks = (IsTicklessMode() ? STK_TICKLESS_TICKS_MAX : 1);
 
        for (uint32_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *task = &m_task_storage[i];
 
            if (task->IsSleeping())
            {
                if (IsDynamicMode())
                {
                    // task is pending removal, wait until it is switched out
                    if (task->IsPendingRemoval())
                    {
                        if ((task != m_task_now) ||
                            ((m_strategy.GetSize() == 1) && (m_fsm_state == FSM_STATE_SLEEPING)))
                        {
                            RemoveTask(task);
                            continue;
                        }
                    }
                }
 
                // deliver sleep event to strategy
                // note: only currently scheduled task can be pending to sleep
                if (TStrategy::SLEEP_EVENT_API)
                {
                    if ((task->m_state & KernelTask::STATE_SLEEP_PENDING) != 0U)
                    {
                        task->m_state &= ~KernelTask::STATE_SLEEP_PENDING;
 
                        // notify strategy that task is sleeping
                        m_strategy.OnTaskSleep(task);
                    }
                }
 
                // advance sleep time by a tick
                task->m_time_sleep += elapsed_ticks;
 
                // deliver sleep event to strategy
                if (TStrategy::SLEEP_EVENT_API)
                {
                    // notify strategy that task woke up
                    if (task->m_time_sleep >= 0)
                        m_strategy.OnTaskWake(task);
                }
            }
            else
            if (IsHrtMode())
            {
                // in HRT mode we trace how long task spent in active state (doing some work)
                if (task->IsBusy())
                {
                    task->m_hrt[0].duration += elapsed_ticks;
 
                    // check if deadline is missed (HRT failure)
                    if (task->HrtIsDeadlineMissed(task->m_hrt[0].duration))
                    {
                        bool can_recover = false;
 
                        // report deadline overrun to a strategy which supports overrun recovery
                        if (TStrategy::DEADLINE_MISSED_API)
                            can_recover = m_strategy.OnTaskDeadlineMissed(task);
 
                        // report failure if it could not be recovered by a scheduling strategy
                        if (!can_recover)
                            task->HrtHardFailDeadline(&m_platform);
                    }
                }
            }
 
            // get the number ticks the driver has to keep CPU in Idle
            if (IsTicklessMode() && (sleep_ticks > 1) && task->IsBusy())
            {
                sleep_ticks = task->GetSleepTicks(sleep_ticks);
            }
        }
 
        return sleep_ticks;
    }

    void UpdateSyncObjects(const Timeout elapsed_ticks)
    {
        STK_ASSERT(IsSyncMode());
 
        ISyncObject::ListEntryType *itr = m_sync_list->GetFirst();
 
        while (itr != nullptr)
        {
            ISyncObject::ListEntryType *next = itr->GetNext();
 
            // MISRA 5-2-3 deviation: GetNext/GetFirst returns ISyncObject*, all objects in
            // m_sync_list are ISyncObject instances - downcast is guaranteed safe
            if (!static_cast<ISyncObject *>(itr)->Tick(elapsed_ticks))
                m_sync_list->Unlink(itr);
 
            itr = next;
        }
    }

    void UpdateTaskRequest()
    {
        if (m_request == REQUEST_NONE)
            return;
 
        // process AddTask requests coming from tasks (KERNEL_DYNAMIC mode only, KERNEL_HRT is
        // excluded as we assume that HRT tasks must be known to the kernel before a Start())
        if (IsDynamicMode() && !IsHrtMode())
        {
            // process serialized AddTask request made from another active task, requesting process
            // is currently waiting due to SwitchToNext()
            if ((m_request & REQUEST_ADD_TASK) != 0U)
            {
                m_request &= ~REQUEST_ADD_TASK;
 
                for (uint32_t i = 0U; i < TASKS_MAX; ++i)
                {
                    KernelTask *task = &m_task_storage[i];
 
                    if (task->m_srt[0].add_task_req != nullptr)
                    {
                        AllocateAndAddNewTask(task->m_srt[0].add_task_req->user_task);
 
                        task->m_srt[0].add_task_req = nullptr;
                        __stk_full_memfence();
                    }
                }
            }
        }
    }

    EFsmEvent FetchNextEvent(KernelTask *&next)
    {
        EFsmEvent type = FSM_EVENT_EXIT;
        KernelTask *itr = nullptr;
 
        // check if no tasks left in KERNEL_DYNAMIC mode and exit, if KERNEL_DYNAMIC is not
        // set then 'is_empty' will always be false
        bool is_empty = IsDynamicMode() && (m_strategy.GetSize() == 0U);
 
        if (!is_empty)
        {
            // MISRA 5-2-3 deviation: GetNext/GetFirst returns IKernelTask*, all objects in
            // the strategy pool are KernelTask instances - downcast is guaranteed safe.
            itr = static_cast<KernelTask *>(m_strategy.GetNext());
 
            // sleep-aware strategy returns nullptr if no active tasks available, start sleeping
            if (itr == nullptr)
            {
                type = FSM_EVENT_SLEEP;
            }
            else
            {
                // strategy must provide active-only task
                STK_ASSERT(!itr->IsSleeping());
 
                // if was sleeping, process wake event first
                type = (m_fsm_state == FSM_STATE_SLEEPING ? FSM_EVENT_WAKE : FSM_EVENT_SWITCH);
            }
        }
 
        next = itr;
        return type;
    }

#ifdef _STK_UNDER_TEST
    virtual
#endif
    EFsmState GetNewFsmState(KernelTask *&next)
    {
        STK_ASSERT(IsValidFsmState(m_fsm_state));
        return m_fsm[m_fsm_state][FetchNextEvent(next)];
    }

    bool UpdateFsmState(Stack *&idle, Stack *&active)
    {
        KernelTask *now = m_task_now, *next = nullptr;
        bool switch_context = false;
 
        EFsmState new_state = GetNewFsmState(next);
 
        switch (new_state)
        {
        case FSM_STATE_SWITCHING:
            switch_context = StateSwitch(now, next, idle, active);
            break;
        case FSM_STATE_SLEEPING:
            switch_context = StateSleep(now, next, idle, active);
            break;
        case FSM_STATE_WAKING:
            switch_context = StateWake(now, next, idle, active);
            break;
        case FSM_STATE_EXITING:
            switch_context = StateExit(now, next, idle, active);
            break;
        case FSM_STATE_NONE:
            return switch_context; // valid intermittent non-persisting state: no-transition
        case FSM_STATE_MAX:
        default:                   // invalid state value
            STK_KERNEL_PANIC(KERNEL_PANIC_BAD_STATE);
            break;
        }
 
        m_fsm_state = new_state;
        return switch_context;
    }

    bool StateSwitch(KernelTask *now, KernelTask *next, Stack *&idle, Stack *&active)
    {
        STK_ASSERT(now != nullptr);
        STK_ASSERT(next != nullptr);
 
        // do not switch context because task did not change
        if (next == now)
            return false;
 
        idle   = now->GetUserStack();
        active = next->GetUserStack();
 
        // if stack memory is exceeded these assertions will be hit
        if (now->IsBusy())
        {
            // current task could exit, thus we check it with IsBusy to avoid referencing nullptr returned by GetUserTask()
            STK_ASSERT(now->GetUserTask()->GetStack()[0] == STK_STACK_MEMORY_FILLER);
        }
        STK_ASSERT(next->GetUserTask()->GetStack()[0] == STK_STACK_MEMORY_FILLER);
 
        m_task_now = next;
 
        if ((IsHrtMode()))
        {
            if (now->m_hrt[0].done)
            {
                now->HrtOnSwitchedOut(&m_platform);
                next->HrtOnSwitchedIn();
            }
        }
 
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_OnTaskStopReady(now->GetUserStack()->tid, TRACE_EVENT_SWITCH);
        SEGGER_SYSVIEW_OnTaskStartReady(next->GetUserStack()->tid);
    #endif
 
        return true; // switch context
    }

    bool StateWake(KernelTask *now, KernelTask *next, Stack *&idle, Stack *&active)
    {
        STK_UNUSED(now);
 
        STK_ASSERT(next != nullptr);
 
        idle   = &m_sleep_trap[0].stack;
        active = next->GetUserStack();
 
        // if stack memory is exceeded these assertions will be hit
        STK_ASSERT(m_sleep_trap[0].memory[0] == STK_STACK_MEMORY_FILLER);
        STK_ASSERT(next->GetUserTask()->GetStack()[0] == STK_STACK_MEMORY_FILLER);
 
        m_task_now = next;
 
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_OnTaskStartReady(next->GetUserStack()->tid);
    #endif
 
        if ((IsHrtMode()))
            next->HrtOnSwitchedIn();
 
        return true; // switch context
    }

    bool StateSleep(KernelTask *now, KernelTask *next, Stack *&idle, Stack *&active)
    {
        STK_UNUSED(next);
 
        STK_ASSERT(now != nullptr);
        STK_ASSERT(m_sleep_trap[0].stack.SP != 0);
 
        idle   = now->GetUserStack();
        active = &m_sleep_trap[0].stack;
 
        m_task_now = static_cast<KernelTask *>(m_strategy.GetFirst());
 
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_OnTaskStopReady(now->GetUserStack()->tid, TRACE_EVENT_SLEEP);
    #endif
 
        if (IsHrtMode())
        {
            if (!now->IsPendingRemoval())
                now->HrtOnSwitchedOut(&m_platform);
        }
 
        return true; // switch context
    }

    bool StateExit(KernelTask *now, KernelTask *next, Stack *&idle, Stack *&active)
    {
        STK_UNUSED(now);
        STK_UNUSED(next);
 
        if (IsDynamicMode())
        {
            // dynamic tasks are not supported if main processes's stack memory is not provided in Start()
            STK_ASSERT(m_exit_trap[0].stack.SP != 0);
 
            idle   = nullptr;
            active = &m_exit_trap[0].stack;
 
            m_task_now = nullptr;
 
            m_platform.Stop();
        }
        else
        {
            STK_UNUSED(idle);
            STK_UNUSED(active);
        }
 
        return false;
    }

    bool IsInitialized() const { return (m_state != STATE_INACTIVE); }

    void ScheduleAddTask()
    {
        hw::CriticalSection::ScopedLock cs_;
        m_request |= REQUEST_ADD_TASK;
    }
 
#if STK_SEGGER_SYSVIEW
    void SendTaskTraceInfo(KernelTask *task)
    {
        STK_ASSERT(task->IsBusy());
 
        SEGGER_SYSVIEW_TASKINFO info =
        {
            .TaskID    = task->GetUserStack()->tid,
            .sName     = task->GetUserTask()->GetTraceName(),
            .Prio      = 0,
            .StackBase = hw::PtrToWord(task->GetUserTask()->GetStack()),
            .StackSize = task->GetUserTask()->GetStackSizeBytes()
        };
        SEGGER_SYSVIEW_SendTaskInfo(&info);
    }
#endif
 
    // Kernel modes:
    static __stk_forceinline bool IsStaticMode() { return ((TMode & KERNEL_STATIC) != 0U); }
    static __stk_forceinline bool IsDynamicMode() { return ((TMode & KERNEL_DYNAMIC) != 0U); }
    static __stk_forceinline bool IsHrtMode() { return ((TMode & KERNEL_HRT) != 0U); }
    static __stk_forceinline bool IsSyncMode() { return ((TMode & KERNEL_SYNC) != 0U); }
    static __stk_forceinline bool IsTicklessMode() { return ((TMode & KERNEL_TICKLESS) != 0U); }
 
    // If hit here: Kernel<N> expects at least 1 task, e.g. N > 0
    STK_STATIC_ASSERT_N(TASKS_MAX, TASKS_MAX > 0U);
 
    // If hit here: Kernel mode must be assigned.
    STK_STATIC_ASSERT_N(KERNEL_MODE_MUST_BE_SET, (TMode != 0U));
 
    // If hit here: KERNEL_STATIC and KERNEL_DYNAMIC can not be mixed, either one of these is possible.
    STK_STATIC_ASSERT_N(KERNEL_MODE_MIX_NOT_ALLOWED,
        (((TMode & KERNEL_STATIC) & (TMode & KERNEL_DYNAMIC)) == 0U));
 
    // If hit here: KERNEL_HRT must accompany KERNEL_STATIC or KERNEL_DYNAMIC.
    STK_STATIC_ASSERT_N(KERNEL_MODE_HRT_ALONE, (((TMode & KERNEL_HRT) == 0U) ||
        ((((TMode & KERNEL_HRT) != 0U)) && (((TMode & KERNEL_STATIC) != 0U) || ((TMode & KERNEL_DYNAMIC) != 0U)))));
 
    // if hit here: KERNEL_TICKLESS is incompatible with KERNEL_HRT. Tickless suppresses the timer,
    // which destroys the precise periodicity HRT depends on.
    STK_STATIC_ASSERT_N(TICKLESS_HRT_CONFLICT,
        (((TMode & KERNEL_TICKLESS) == 0U) || ((TMode & KERNEL_HRT) == 0U)));

    typedef KernelTask TaskStorageType[TASKS_MAX];

    struct SleepTrapStack
    {
        typedef SleepTrapStackMemory::MemoryType Memory;
 
        Stack  stack;  
        Memory memory; 
    };

    struct ExitTrapStack
    {
        typedef ExitTrapStackMemory::MemoryType Memory;
 
        Stack  stack;  
        Memory memory; 
    };

    typedef ISyncObject::ListHeadType SyncObjectList;
 
    KernelService    m_service;         
    TPlatform        m_platform;        
    TStrategy        m_strategy;        
    KernelTask      *m_task_now;        
    TaskStorageType  m_task_storage;    
    SleepTrapStack   m_sleep_trap[1];   
    ExitTrapStack    m_exit_trap[STK_ALLOCATE_COUNT(TMode, KERNEL_DYNAMIC, 1, 0)]; 
    EFsmState        m_fsm_state;       
    volatile uint8_t m_request;         
    volatile EState  m_state;           
    SyncObjectList   m_sync_list[STK_ALLOCATE_COUNT(TMode, KERNEL_SYNC, 1, 0)]; 
 
    const EFsmState  m_fsm[FSM_STATE_MAX][FSM_EVENT_MAX] = {
    //    FSM_EVENT_SWITCH     FSM_EVENT_SLEEP     FSM_EVENT_WAKE    FSM_EVENT_EXIT
        { FSM_STATE_SWITCHING, FSM_STATE_SLEEPING, FSM_STATE_NONE,   FSM_STATE_EXITING }, // FSM_STATE_SWITCHING
        { FSM_STATE_NONE,      FSM_STATE_NONE,     FSM_STATE_WAKING, FSM_STATE_EXITING }, // FSM_STATE_SLEEPING
        { FSM_STATE_SWITCHING, FSM_STATE_SLEEPING, FSM_STATE_NONE,   FSM_STATE_EXITING }, // FSM_STATE_WAKING
        { FSM_STATE_NONE,      FSM_STATE_NONE,     FSM_STATE_NONE,   FSM_STATE_NONE    }  // FSM_STATE_EXITING
    }; 

};
 
} // namespace stk
 
#endif /* STK_H_ */