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"
#include "strategy/stk_strategy_pthreshold.h"

 
namespace stk {
 
// Helper function for Kernel::UpdateTaskState.
template <bool TicklessMode> __stk_forceinline Timeout GetInitialSleepTicks();
template <> __stk_forceinline Timeout GetInitialSleepTicks<true>()  { return STK_TICKLESS_TICKS_MAX; }
template <> __stk_forceinline Timeout GetInitialSleepTicks<false>() { return 1; }

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

    typedef StackMemoryWrapper<STACK_SIZE_MIN> ExitTrapStackMemory;

    enum ERequest : uint8_t
    {
        REQ_NONE     = 0,       
        REQ_ADD_TASK = (1 << 0) 
    };

    class KernelTask final : public IKernelTask
    {
        friend class Kernel;

        enum EStateFlags : uint32_t
        {
            STATE_NONE           = 0U,        
            STATE_REMOVE_PENDING = (1U << 0), 
            STATE_SLEEP_PENDING  = (1U << 1), 
            STATE_WAKE_PENDING   = (1U << 2), 
            STATE_SUSPENDED      = (1U << 3), 
            STATE_YIELD_PENDING  = (1U << 4), 
        };
 
    public:
        struct AddTaskRequest
        {
            ITask *user_task; 
        };

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

        ITask *GetUserTask() override { return m_user; }

        Stack GetUserStack() const override { return m_stack;}

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

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

        __stk_forceinline bool IsSleepInfinite() const { return (m_time_sleep == -WAIT_INFINITE); }

        TId GetTid() const { return GetTidFromUserTask(m_user); }

        void Wake() override
        {
            STK_ASSERT(IsSleeping());
 
            // wakeup on a next cycle
            m_time_sleep = -1;
 
            // notify kernel that this task, even if was going to sleep will be woken on a next tick
            if ((m_state & STATE_SLEEP_PENDING) != 0U)
            {
                m_state |= STATE_WAKE_PENDING;
            }
        }

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

        Weight GetWeight() const override
        {
            Weight static_weight;
 
            if __stk_constexpr_cpp17 (TStrategy::PRIORITY_INHERITANCE_API)
            {
                if (m_rt_weight[0] != NO_WEIGHT)
                {
                    static_weight = m_rt_weight[0];
                }
                else
                {
                    static_weight = GetBaseWeight();
                }
            }
            else
            {
                static_weight = GetBaseWeight();
            }
 
            return static_weight;
        }

        Weight GetBaseWeight() const override
        {
            Weight base_weight;
 
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API)
            {
                // run-time override set via IKernelService::SetWeight() takes precedence
                base_weight = (m_base_weight[0] != NO_WEIGHT ? m_base_weight[0] : m_user->GetWeight());
            }
            else
            {
                base_weight = DEFAULT_WEIGHT;
            }
 
            return base_weight;
        }

        void SetBaseWeight(Weight weight)
        {
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API)
            {
                m_base_weight[0] = weight;
            }
            else
            {
                STK_UNUSED(weight);
            }
        }

        Weight GetCurrentWeight() const override
        {
            Weight cur_weight;
          
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API)
            {
                cur_weight = m_rt_weight[0];
            }
            else
            {
                cur_weight = DEFAULT_WEIGHT;
            }
            
            return cur_weight;
        }

        Timeout GetHrtPeriodicity() const override
        {
            STK_ASSERT(IsHrtMode());
            
            Timeout to;
            
            if __stk_constexpr_cpp17 (IsHrtMode())
            {
                to = m_hrt[0].periodicity;
            }
            else
            {
                to = 0;
            }
            
            return to;
        }

        Timeout GetHrtDeadline() const override
        {
            STK_ASSERT(IsHrtMode());
            
            Timeout deadline;
 
            if __stk_constexpr_cpp17 (IsHrtMode())
            {
                deadline = m_hrt[0].deadline;
            }
            else
            {
                deadline = 0;
            }
            
            return deadline;
        }

        Timeout GetHrtRelativeDeadline() const override
        {
            STK_ASSERT(IsHrtMode());
            STK_ASSERT(!IsSleeping());
            
            Timeout relative_deadline;
 
            if __stk_constexpr_cpp17 (IsHrtMode())
            {
                relative_deadline = (m_hrt[0].deadline - m_hrt[0].duration);
            }
            else
            {
                relative_deadline = 0;
            }
            
            return relative_deadline;
        }
 
        Timeout GetSleepTicks(Timeout sleep_ticks)
        {
            // note: task sleep time is negative
            Timeout task_sleep = Max<Timeout>(NO_WAIT, -m_time_sleep);
 
            if __stk_constexpr_cpp17 (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 > NO_WAIT)
                    {
                        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:
        STK_VIRT_DTOR ~KernelTask() = default;

        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 final : public IWaitObject
        {
            explicit WaitObject() : m_task(nullptr), m_sync_obj(nullptr), m_time_wait(NO_WAIT),
                m_state(STATE_NONE)
            {}

            STK_VIRT_DTOR ~WaitObject() = default;

            struct WaitRequest
            {
                ISyncObject *sync_obj; 
            };

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

            EState GetState() const override { return m_state; }

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

            EWaitResult GetWaitResult() const
            {
                STK_ASSERT(!IsWaiting());
 
                switch (GetState())
                {
                case STATE_TIMEOUT:
                    return WAIT_RESULT_TIMEOUT;
                case STATE_CANCELLED:
                    return WAIT_RESULT_CANCELED;
                default: {
                    STK_ASSERT(m_state != STATE_NONE);
                    STK_ASSERT(m_state != STATE_WAIT);
                    STK_ASSERT(m_state == STATE_SIGNALED);
                    return WAIT_RESULT_SIGNAL; }
                }
            }

            void ClearState()
            {
                STK_ASSERT(!IsWaiting());
 
                m_state = STATE_NONE;
            }

            void Wake(bool timeout) override
            {
                STK_ASSERT(IsWaiting());
 
                m_state     = (timeout ? STATE_TIMEOUT : STATE_SIGNALED);
                m_time_wait = NO_WAIT;
 
                m_sync_obj->RemoveWaitObject(this);
                m_sync_obj = nullptr;
 
                return m_task->Wake();
            }

            bool Tick(Timeout elapsed_ticks) override
            {
                if (m_time_wait != WAIT_INFINITE)
                {
                    if (m_state == STATE_WAIT)
                    {                  
                        m_time_wait -= elapsed_ticks;
 
                        if (m_time_wait <= NO_WAIT)
                        {
                            m_state = STATE_TIMEOUT;
                        }
                    }
                }
 
                return (m_state == STATE_WAIT);
            }

            void SetupWait(ISyncObject *sync_obj, Timeout timeout)
            {
                STK_ASSERT(!IsWaiting());
 
                m_sync_obj  = sync_obj;
                m_time_wait = timeout;
                m_state     = STATE_WAIT;
 
                sync_obj->AddWaitObject(this);
            }

            void CancelNoWake()
            {
                if (IsWaiting())
                {
                    m_state = STATE_CANCELLED;
 
                    m_sync_obj->RemoveWaitObject(this);
                    m_sync_obj = nullptr;
                }
            }

            void Cancel()
            {
                STK_ASSERT(IsWaiting());
 
                m_state     = STATE_CANCELLED;
                m_time_wait = NO_WAIT;
 
                m_sync_obj->RemoveWaitObject(this);
                m_sync_obj = nullptr;
 
                m_task->Wake();
            }
 
            KernelTask     *m_task;      
            ISyncObject    *m_sync_obj;  
            Timeout         m_time_wait; 
            volatile EState m_state;     
        };

        void Bind(TPlatform *platform, ITask *user_task)
        {
            // bind user task (GetTid depends on m_user)
            m_user = user_task;
 
            // set access mode for this stack
            m_stack.access_mode = user_task->GetAccessMode();
 
            // set task id for tracking purpose
        #if STK_STACK_NEEDS_TASK_ID
            m_stack.tid = GetTid();
            STK_ASSERT(m_stack.tid != TID_NONE);
        #endif
 
            // init stack of the user task
            platform->InitStack(STACK_USER_TASK, &m_stack, user_task, user_task);
 
            // initialize current weight to NO_WEIGHT for priority inheritance mechanism
            if __stk_constexpr_cpp17 (TStrategy::PRIORITY_INHERITANCE_API)
            {
                SetCurrentWeight(NO_WEIGHT);
            }
 
            // no run-time override of the base weight: ITask::GetWeight() is used
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API)
            {
                m_base_weight[0] = NO_WEIGHT;
            }
        }

        void Unbind()
        {
            if __stk_constexpr_cpp17 (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 __stk_constexpr_cpp17 (IsHrtMode())
            {
                m_hrt[0].Clear();
            }
            else
            {
                m_srt->Clear();
            }
        }

        void ScheduleRemoval()
        {
            // if blocked on a sync object, detach first - otherwise the WaitObject stays linked
            // in the sync object's wait list after Unbind() frees this slot, and a later
            // Wake()/WakeAll() would corrupt whatever task the slot gets rebound to
            if __stk_constexpr_cpp17 (IsSyncMode())
            {
                m_wait_obj->CancelNoWake();
            }
 
            // make this task sleeping to switch it out from scheduling process
            ScheduleSleep(WAIT_INFINITE);
 
            // mark it as done HRT task
            if __stk_constexpr_cpp17 (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
        {
            bool is_match = false;
 
            const Word start = hw::PtrToWord(m_user->GetStack());
            const Word end   = start + (m_user->GetStackSize() * sizeof(Word));
 
            if ((SP >= start) && (SP <= end))
            {
                is_match = true;
            }
        #if STK_TZ_SECURE // lookup Secure memory region too when on a Secure side
            else
            {
                IStackMemory *const secure_mem = m_user->GetSecureStackMemory();
 
                if (secure_mem != nullptr)
                {
                    const Word s_start = hw::PtrToWord(secure_mem->GetStack());
                    const Word s_end   = s_start + (secure_mem->GetStackSize() * sizeof(Word));
 
                    if ((SP >= s_start) && (SP <= s_end))
                    {
                        is_match = true;
                    }
                }
            }
        #endif
 
            return is_match;
        }

        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()
        {
            const Timeout duration = m_hrt[0].duration;
 
            STK_ASSERT(duration >= 0);
 
            const 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 (!IsSleeping())
            {
                m_state |= STATE_SLEEP_PENDING;
            }
 
            m_time_sleep = -ticks;
 
            __stk_full_memfence();
        }

        void ScheduleYield()
        {
            if __stk_constexpr_cpp17 (TStrategy::NOSLEEP_YIELD_API)
            {
                m_state |= STATE_YIELD_PENDING;
 
                __stk_full_memfence();
            }
        }

        void BusyWaitWhileYielding(Kernel *kernel) const
        {
            if __stk_constexpr_cpp17 (TStrategy::NOSLEEP_YIELD_API)
            {
                kernel->m_platform.ForceContextSwitch(GetTid());
 
                while ((m_state & STATE_YIELD_PENDING) != 0U)
                {
                    __stk_relax_cpu();
                }
            }
        }

        void BusyWaitWhileSleeping(Kernel *kernel) const
        {
            kernel->m_platform.ForceContextSwitch(GetTid());
 
            while (IsSleeping())
            {
                __stk_relax_cpu();
            }
        }
        
        Stack *GetUserStackPtr() { return &m_stack; }
 
        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, 0U, 1U>::Value];         
        HrtInfo           m_hrt        [STK_ALLOCATE_COUNT<TMode, KERNEL_HRT, 1U, 0U>::Value];         
        Weight            m_rt_weight  [STK_ALLOCATE_COUNT<TStrategy::WEIGHT_API, 1U, 1U, 0U>::Value]; 
        Weight            m_base_weight[STK_ALLOCATE_COUNT<TStrategy::WEIGHT_API, 1U, 1U, 0U>::Value]; 
        WaitObject        m_wait_obj   [STK_ALLOCATE_COUNT<TMode, KERNEL_SYNC, 1U, 0U>::Value];        
    };

    class KernelService final : public IKernelService
    {
        friend class Kernel;
 
    public:
        TId GetTid() const override { return m_kernel->m_platform.GetTid(); }
 
        Ticks GetTicks() const override { return hw::ReadVolatile64(&m_ticks); }
 
        uint32_t GetTickResolution() const override  { return m_kernel->m_platform.GetTickResolution(); }
 
        Cycles GetSysTimerCount() const override { return m_kernel->m_platform.GetSysTimerCount(); }
 
        uint32_t GetSysTimerFrequency() const override { return m_kernel->m_platform.GetSysTimerFrequency(); }
 
        void Delay(Timeout ticks) override
        {
            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();
            }
        }
 
        void Sleep(Timeout ticks) override
        {
            STK_ASSERT(!hw::IsInsideISR());
            STK_ASSERT(ticks >= 0);
 
            if __stk_constexpr_cpp17 (!IsHrtMode())
            {
                m_kernel->m_platform.Sleep(ticks);
            }
            else
            {
                // sleeping is not supported in HRT mode, task will sleep according to its periodicity and workload
                STK_ASSERT(false);
            }
        }
 
        bool SleepUntil(Ticks timestamp) override
        {
            STK_ASSERT(!hw::IsInsideISR());
 
            if __stk_constexpr_cpp17 (!IsHrtMode())
            {
                return m_kernel->m_platform.SleepUntil(timestamp);
            }
            else
            {
                // sleeping is not supported in HRT mode, task will sleep according to its periodicity and workload
                STK_ASSERT(false);
                return false;
            }
        }
 
        void SleepCancel(TId task_id) override
        {
            if __stk_constexpr_cpp17 (!IsHrtMode())
            {
                m_kernel->OnTaskSleepCancel(task_id);
            }
        }
 
        void SwitchToNext() override
        {
            STK_ASSERT(!hw::IsInsideISR());
 
            m_kernel->m_platform.SwitchToNext();
        }
 
        EWaitResult Wait(ISyncObject *sobj, IMutex *mutex, Timeout ticks) override
        {
            if __stk_constexpr_cpp17 (IsSyncMode())
            {
                return m_kernel->m_platform.Wait(sobj, mutex, ticks);
            }
            else
            {
                STK_ASSERT(false);
                return WAIT_RESULT_FAIL;
            }
        }
 
        void Wake(ISyncObject *sobj, bool all) override
        {
            if __stk_constexpr_cpp17 (IsSyncMode())
            {
                if (all)
                {
                    ISyncObject::WakeAll(GetWaitList(sobj));
                }
                else
                {
                    ISyncObject::WakeOne(GetWaitList(sobj));
                }
            }
            else
            {
                STK_ASSERT(false);
            }
        }
 
        Timeout Suspend() override
        {
            if __stk_constexpr_cpp17 (IsTicklessMode())
            {
                return m_kernel->m_platform.Suspend();
            }
            else
            {
                STK_ASSERT(false);
                return 0;
            }
        }
 
        void Resume(Timeout elapsed_ticks) override
        {
            if __stk_constexpr_cpp17 (IsTicklessMode())
            {
                return m_kernel->m_platform.Resume(elapsed_ticks);
            }
            else
            {
                STK_ASSERT(false);
            }
        }
 
        void InheritWeight(TId tid, Weight weight) override
        {
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API && TStrategy::PRIORITY_INHERITANCE_API)
            {
                m_kernel->OnInheritWeight(tid, weight);
            }
        }
 
        void RestoreWeight(TId tid, ISyncObject *sobj) override
        {
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API && TStrategy::PRIORITY_INHERITANCE_API)
            {
                m_kernel->OnRestoreWeight(tid, sobj);
            }
        }
 
        Weight SetWeight(TId tid, Weight weight) override
        {
            if __stk_constexpr_cpp17 (TStrategy::WEIGHT_API && TStrategy::PRIORITY_INHERITANCE_API)
            {
                const hw::CriticalSection::ScopedLock cs_;
 
                return m_kernel->OnSetWeight(tid, weight);
            }
            else
            {
                // strategy must handle OnTaskWeightChange() events
                STK_UNUSED(tid);
                STK_UNUSED(weight);
                STK_ASSERT(false);
                return NO_WEIGHT;
            }
        }
 
    private:
        explicit KernelService() : m_kernel(nullptr), m_ticks(0)
        {}

        STK_VIRT_DTOR ~KernelService() = default;

        void Initialize(Kernel *kernel)
        {
            m_kernel = kernel;
        }

        void IncrementTicks(Ticks advance)
        {
            // using WriteVolatile64() to guarantee correct lockless reading order by ReadVolatile64
            hw::WriteVolatile64(&m_ticks, m_ticks + advance);
        }
 
        Kernel        *m_kernel; 
        volatile Ticks m_ticks;  
    };
 
public:
    static constexpr size_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(REQ_NONE), m_kstate(KSTATE_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
    }

    STK_VIRT_DTOR ~Kernel() = default;

    __stk_attr_noinline void Initialize(uint32_t resolution_us = PERIODICITY_DEFAULT) override
    {
        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   = REQ_NONE;
        
        // exit trap is required only for KERNEL_DYNAMIC mode
        Stack *exit_trap;
        if __stk_constexpr_cpp17 (IsDynamicMode())
        {
            exit_trap = &m_exit_trap[0].stack;
        }
        else
        {
            exit_trap = nullptr;
        }
 
        m_service.Initialize(this);
        m_platform.Initialize(this, &m_service, resolution_us, exit_trap);
 
        // now ready to Start()
        m_kstate = KSTATE_READY;
    }

    __stk_attr_noinline void AddTask(ITask *user_task) override
    {
        if __stk_constexpr_cpp17 (!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 __stk_constexpr_cpp17 (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) override
    {
        if __stk_constexpr_cpp17 (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) override
    {
        if __stk_constexpr_cpp17 (IsDynamicMode())
        {
            STK_ASSERT(user_task != nullptr);
            STK_ASSERT(!IsStarted());
 
            KernelTask *const 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) override
    {
        if __stk_constexpr_cpp17 (IsDynamicMode())
        {
            STK_ASSERT(user_task != nullptr);
            STK_ASSERT(IsStarted());
 
            const hw::CriticalSection::ScopedLock cs_;
 
            KernelTask *const 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) override
    {
        STK_ASSERT(user_task != nullptr);
 
        bool self = false;
 
        // avoid race with OnTick
        {
            const hw::CriticalSection::ScopedLock cs_;
 
            KernelTask *const task = FindTaskByUserTask(user_task);
            STK_ASSERT(task != nullptr);
 
            // nesting not supported, must not be suspended by SuspendTask
            STK_ASSERT((task->m_state & KernelTask::STATE_SUSPENDED) == 0U);
 
            // 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()
            suspended = !task->IsSleeping();
            if (suspended == true)
            {
                task->ScheduleSleep(WAIT_INFINITE);
 
                // check if suspending self
                self = (task == m_task_now);
 
                // trace blocked via Suspend
                if (self)
                {
                #if STK_SEGGER_SYSVIEW
                    SEGGER_SYSVIEW_OnTaskStopReady(task->GetUserStackPtr()->tid, TRACE_EVENT_SUSPEND);
                #endif
                }
 
                // set state
                task->m_state |= KernelTask::STATE_SUSPENDED;
            }
        }
 
        // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
        if (self)
        {
            m_task_now->BusyWaitWhileSleeping(this);
        }
    }

    void ResumeTask(ITask *user_task) override
    {
        STK_ASSERT(user_task != nullptr);
 
        // avoid race with OnTick
        const hw::CriticalSection::ScopedLock cs_;
 
        KernelTask *const task = FindTaskByUserTask(user_task);
        STK_ASSERT(task != nullptr);
 
        // must be suspended by SuspendTask, otherwise noop
        if ((task->m_state & KernelTask::STATE_SUSPENDED) != 0U)
        {
            // clear state
            task->m_state &= ~KernelTask::STATE_SUSPENDED;
 
            // wake up
            if (task->IsSleeping())
            {
                task->Wake();
            }
        }
    }

    void CancelTaskWait(ITask *user_task) override
    {
        STK_ASSERT(user_task != nullptr);
 
        if __stk_constexpr_cpp17 (IsSyncMode())
        {
            // avoid race with OnTick
            const hw::CriticalSection::ScopedLock cs_;
 
            KernelTask *const task = FindTaskByUserTask(user_task);
            STK_ASSERT(task != nullptr);
 
            // only cancel if actually blocked in OnTaskWait() on a sync object; no-op otherwise
            // (e.g. task is running, suspended via SuspendTask(), or plainly sleeping)
            if (task->m_wait_obj->IsWaiting())
            {
                task->m_wait_obj->Cancel();
            }
        }
        else
        {
            // kernel operating mode must include KERNEL_SYNC for tasks to be able to wait
            STK_ASSERT(false);
        }
    }

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

    __stk_attr_noinline void Start() override
    {
        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 (size_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() override { return &m_platform; }

    ITaskSwitchStrategy *GetSwitchStrategy() override { return &m_strategy; }

    EKernelState GetState() const override { return m_kstate; }
 
    // Used by tests only, not Public API:
#ifdef _STK_UNDER_TEST
    void ScheduleSleepOnActiveTask(Timeout ticks)
    {
        m_task_now->ScheduleSleep(ticks);
    }
#endif
 
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 = 1;

    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.access_mode = ACCESS_PRIVILEGED;
        #if STK_STACK_NEEDS_TASK_ID
            sleep.stack.tid  = SYS_TASK_ID_SLEEP;
        #endif
 
            STK_UNUSED(m_platform.InitStack(STACK_SLEEP_TRAP, &sleep.stack, &wrapper, nullptr));
        }
 
        // init stack for an Exit trap
        if __stk_constexpr_cpp17 (IsDynamicMode())
        {
            ExitTrapStack &exit = m_exit_trap[0];
 
            ExitTrapStackMemory wrapper(&exit.memory);
            exit.stack.access_mode = ACCESS_PRIVILEGED;
        #if STK_STACK_NEEDS_TASK_ID
            exit.stack.tid  = SYS_TASK_ID_EXIT;
        #endif
 
            STK_UNUSED(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 (size_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *const 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
            }
            else
            {
                // noop, continue to the next slot
            }
        }
 
        // 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->GetUserStackPtr()->tid);
        if (IsStarted())
        {
            SendTaskTraceInfo(task);
        }
    #endif
 
        m_strategy.AddTask(task);
    }

    void AllocateAndAddNewTask(ITask *user_task)
    {
        KernelTask *const 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 *const 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 *const user_task)
    {
        KernelTask *const 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();
        }
 
        // wait until request is consumed
        if __stk_constexpr_cpp17 (TStrategy::NOSLEEP_YIELD_API)
        {
            while (caller->m_srt[0].add_task_req != nullptr)
            {
                __stk_relax_cpu();
            }
        }
 
        STK_ASSERT(caller->m_srt[0].add_task_req == nullptr);
    }

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

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

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

    __stk_attr_noinline void OnStart(Stack *&active) override
    {
        STK_ASSERT(m_strategy.GetSize() != 0);
 
        // iterate tasks and generate OnTaskSleep for a strategy for all initially sleeping tasks
        for (size_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *const task = &m_task_storage[i];
 
            if ((task->m_state & KernelTask::STATE_SLEEP_PENDING) != 0U)
            {
                STK_ASSERT(task->IsSleeping());
 
                task->m_state &= ~KernelTask::STATE_SLEEP_PENDING;
 
                // notify strategy that task is sleeping
                if __stk_constexpr_cpp17 (TStrategy::SLEEP_EVENT_API)
                {
                    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->GetUserStackPtr();
 
                if __stk_constexpr_cpp17 (IsHrtMode())
                {
                    next->HrtOnSwitchedIn();
                }
            }
            else
            if (m_fsm_state == FSM_STATE_SLEEPING)
            {
                m_task_now = util::DListCast::ListEntryToParent<KernelTask>(m_strategy.GetFirst());
                active     = &m_sleep_trap[0].stack;
            }
            else
            {
                // unexpected state
                STK_KERNEL_PANIC(KERNEL_PANIC_BAD_STATE);
            }
        }
 
        // is in running state
        m_kstate = KSTATE_RUNNING;
    }

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

    bool OnTick(Stack *&idle, Stack *&active
    #if STK_TICKLESS_IDLE
        , Timeout &ticks
    #endif
    ) override
    {
    #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
        STK_UNUSED(
    #endif
        UpdateTasks(ticks));
 
        // decide on a context switch
        return UpdateFsmState(idle, active);
    }
 
    bool OnForceContextSwitch(TId id, Stack *&idle, Stack *&active)
    {
        bool switch_context = false;
        KernelTask *const task = m_task_now;
 
        STK_ASSERT(task != nullptr);
 
        // note: called from inside ISR, therefore protection by critical section is not needed
 
        if (task->GetTid() == id)
        {
            // current task is busy-waiting to be de-scheduled
            if (task->IsSleeping())
            {
                if ((task->m_state & KernelTask::STATE_SLEEP_PENDING) != 0U)
                {
                    ProcessTaskPendingSleep(task);
 
                    switch_context = UpdateFsmState(idle, active);
                }
            }
            else
            {
                if __stk_constexpr_cpp17 (TStrategy::NOSLEEP_YIELD_API)
                {
                    // current task yielded and stays runnable: let the strategy pick the next task
                    if ((task->m_state & KernelTask::STATE_YIELD_PENDING) != 0U)
                    {
                        ProcessTaskPendingYield(task);
 
                        switch_context = UpdateFsmState(idle, active);
                    }
                }
            }
        }
 
        return switch_context;
    }
 
    void OnTaskSwitch(Word caller_SP) override
    {
        // let the strategy handle the yield without putting the task to sleep (e.g. to hand
        // the CPU to same-priority peers only), not applicable in HRT mode where Yield()
        // means work completion
        if __stk_constexpr_cpp17 (TStrategy::NOSLEEP_YIELD_API && !IsHrtMode())
        {
            KernelTask *const task = FindTaskBySP(caller_SP);
            STK_ASSERT(task != nullptr);
 
            if (task != nullptr)
            {
                bool handled = false;
                {
                    const hw::CriticalSection::ScopedLock cs_;
 
                    if (!task->IsSleeping())
                    {
                        if (m_strategy.OnTaskYield(task))
                        {
                            task->ScheduleYield();
                            handled = true;
                        }
                    }
                }
 
                if (handled)
                {
                    task->BusyWaitWhileYielding(this);
                }
                else
                {
                    OnTaskSleep(caller_SP, YIELD_TICKS);
                }
            }
            else
            {
                // defensive: do nothing on null task
            }
        }
        else
        {
            OnTaskSleep(caller_SP, YIELD_TICKS);
        }
    }
 
    void OnTaskSleep(Word caller_SP, Timeout ticks) override
    {
        KernelTask *const task = FindTaskBySP(caller_SP);
        STK_ASSERT(task != nullptr);
 
        // make change to HRT state and sleep time atomic
        {
            const hw::CriticalSection::ScopedLock cs_;
 
            if __stk_constexpr_cpp17 (IsHrtMode())
            {
                task->HrtOnWorkCompleted();
            }
 
            if (ticks > 0)
            {
                task->ScheduleSleep(ticks);
 
                // trace blocked on Sleep
            #if STK_SEGGER_SYSVIEW
                SEGGER_SYSVIEW_OnTaskStopReady(task->GetUserStackPtr()->tid, TRACE_EVENT_SLEEP);
            #endif
            }
        }
 
        // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
        task->BusyWaitWhileSleeping(this);
    }
 
    bool OnTaskSleepUntil(Word caller_SP, Ticks timestamp) override
    {
        KernelTask *const task = FindTaskBySP(caller_SP);
        STK_ASSERT(task != nullptr);
 
        bool result = true;
 
        // make change to HRT state and sleep time atomic
        {
            const hw::CriticalSection::ScopedLock cs_;
 
            // calculate signed delta (handles wrap-around correctly)
            const Ticks delta = timestamp - m_service.m_ticks;
 
            if (delta > 0)
            {
                const Ticks infinite_ticks = WAIT_INFINITE;              
                task->ScheduleSleep(static_cast<Timeout>(Min(delta, infinite_ticks)));
 
                // trace blocked on on SleepUntil
            #if STK_SEGGER_SYSVIEW
                SEGGER_SYSVIEW_OnTaskStopReady(task->GetUserStackPtr()->tid, TRACE_EVENT_SLEEP);
            #endif
            }
            else
            {
                result = false; // deadline already hit or passed
            }
        }
 
        // note: we do not spin long here, kernel will switch this task out from scheduling on the next tick
        if (result)
        {
            task->BusyWaitWhileSleeping(this);
        }
 
        return result;
    }
 
    void OnTaskSleepCancel(TId task_id)
    {
        KernelTask *const task = FindTaskByUserTask(GetUserTaskFromTid(task_id));
        if (task != nullptr)
        {
            const hw::CriticalSection::ScopedLock cs_;
 
            if (task->IsSleeping())
            {
                task->Wake();
            }
        }
    }
 
    void OnTaskExit(Stack *stack) override
    {
        if __stk_constexpr_cpp17 (IsDynamicMode())
        {
            KernelTask *const task = FindTaskByStack(stack);
            STK_ASSERT(task != nullptr);
 
            // task is being stopped and will terminate
        #if STK_SEGGER_SYSVIEW
            SEGGER_SYSVIEW_OnTaskStopExec();
        #endif
 
            // notify kernel to execute removal
            {
                // make change to HRT/sleep/removal state atomic (same as OnTaskSleep/OnTaskSleepUntil)
                const hw::CriticalSection::ScopedLock cs_;
 
                task->ScheduleRemoval();
            }
        }
        else
        {
            // kernel operating mode must be KERNEL_DYNAMIC for tasks to be able to exit
            STK_KERNEL_PANIC(KERNEL_PANIC_BAD_MODE);
        }
    }
 
    EWaitResult OnTaskWait(Word caller_SP, ISyncObject *sync_obj, IMutex *mutex, Timeout timeout) override
    {
        if __stk_constexpr_cpp17 (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 *const 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);
 
            // trace blocked on sync object's Wait
        #if STK_SEGGER_SYSVIEW
            SEGGER_SYSVIEW_OnTaskStopReady(task->GetUserStackPtr()->tid, TRACE_EVENT_WAIT);
        #endif
 
            // 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(this);
 
            // extract & clear this wait's result BEFORE mutex->Lock(): a contended Lock() recurses
            // into OnTaskWait() and reuses this same per-task WaitObject, which would otherwise
            // silently overwrite/clear our still-unread result
            const EWaitResult result = task->m_wait_obj->GetWaitResult();
            task->m_wait_obj->ClearState();
 
            // re-lock mutex when returning to the task's execution space
            mutex->Lock();
 
            return result;
        }
        else
        {
            STK_ASSERT(false);
            return WAIT_RESULT_FAIL;
        }
    }
 
    TId OnGetTid(Word caller_SP) override
    {
        KernelTask *const task = FindTaskBySP(caller_SP);
        STK_ASSERT(task != nullptr);
 
        return task->GetTid();
    }
 
    void OnSuspend(bool suspended) override
    {
        // toggle kernel state
        if (suspended)
        {
            if (m_kstate == KSTATE_RUNNING) 
            {
                m_kstate = KSTATE_SUSPENDED;
            }
        }
        else
        {
            if (m_kstate == KSTATE_SUSPENDED) 
            {
                m_kstate = KSTATE_RUNNING;
            }
        }
 
        // force yield for a currently active task
        if (!m_task_now->IsSleeping())
        {
            m_task_now->ScheduleSleep(YIELD_TICKS);
        }
    }
 
    void OnInheritWeight(TId tid, Weight weight)
    {
        STK_ASSERT(tid != TID_NONE);
        STK_ASSERT(TStrategy::WEIGHT_API && TStrategy::PRIORITY_INHERITANCE_API);
 
        if (weight != NO_WEIGHT)
        {
            KernelTask *const task = FindTaskByUserTask(GetUserTaskFromTid(tid));
            STK_ASSERT(task != nullptr);
 
            const Weight prev_weight = task->GetWeight();
 
            if (prev_weight < weight)
            {
                task->SetCurrentWeight(weight);
                m_strategy.OnTaskWeightChange(task, prev_weight);
            }
        }
    }
 
    Weight OnSetWeight(TId tid, Weight weight)
    {
        STK_ASSERT(tid != TID_NONE);
        STK_ASSERT(TStrategy::WEIGHT_API && TStrategy::PRIORITY_INHERITANCE_API);
        STK_ASSERT(weight != NO_WEIGHT);
 
        KernelTask *const task = FindTaskByUserTask(GetUserTaskFromTid(tid));
        STK_ASSERT(task != nullptr);
 
        const Weight prev_weight = task->GetWeight();     // effective weight (base or inherited)
        const Weight prev_base   = task->GetBaseWeight();
 
        task->SetBaseWeight(weight);
 
        // an inherited weight that does not exceed the new base weight is no longer a boost
        if ((task->GetCurrentWeight() != NO_WEIGHT) && (task->GetCurrentWeight() <= weight))
        {
            task->SetCurrentWeight(NO_WEIGHT);
        }
 
        // always notify: the strategy may depend on parts of the weight that do not change
        // the effective weight while a boost is active (e.g. preemption-threshold)
        m_strategy.OnTaskWeightChange(task, prev_weight);
 
        return prev_base;
    }
 
    void OnRestoreWeight(TId tid, ISyncObject *sobj)
    {
        STK_ASSERT(tid != TID_NONE);
        STK_ASSERT(TStrategy::WEIGHT_API && TStrategy::PRIORITY_INHERITANCE_API);
 
        KernelTask *const task = FindTaskByUserTask(GetUserTaskFromTid(tid));
        STK_ASSERT(task != nullptr);
 
        const Weight prev_weight = task->GetWeight();
 
        // restore to original or boost from wait objects
        task->SetCurrentWeight(sobj != nullptr ? sobj->FindWeightHigherThan(task->GetWeight()) : NO_WEIGHT);
 
        m_strategy.OnTaskWeightChange(task, prev_weight);
    }

    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 __stk_constexpr_cpp17 (IsSyncMode())
        {
            UpdateSyncObjects(elapsed_ticks);
        }
 
        if (m_request != REQ_NONE)
        {
            UpdateTaskRequest();
        }
 
        return UpdateTaskState(elapsed_ticks);
    }
 
    void ProcessTaskPendingSleep(KernelTask *const task)
    {
        task->m_state &= ~(KernelTask::STATE_SLEEP_PENDING | KernelTask::STATE_WAKE_PENDING);
 
        // notify strategy that task is sleeping
        if __stk_constexpr_cpp17 (TStrategy::SLEEP_EVENT_API)
        {
            m_strategy.OnTaskSleep(task);
        }
    }
 
    void ProcessTaskPendingYield(KernelTask *const task)
    {
        task->m_state &= ~KernelTask::STATE_YIELD_PENDING;
    }
 
        
    Timeout UpdateTaskState(const Timeout elapsed_ticks)
    {
        Timeout sleep_ticks = GetInitialSleepTicks<IsTicklessMode()>();
 
        for (size_t i = 0U; i < TASKS_MAX; ++i)
        {
            KernelTask *const task = &m_task_storage[i];
 
            if __stk_constexpr_cpp17 (TStrategy::NOSLEEP_YIELD_API)
            {
                // a pending yield is consumed by this tick (the strategy already moved its cursor)
                if ((task->m_state & KernelTask::STATE_YIELD_PENDING) != 0U)
                {
                    task->m_state &= ~KernelTask::STATE_YIELD_PENDING;
                }
            }
 
            if (task->IsSleeping())
            {
                if __stk_constexpr_cpp17 (IsDynamicMode())
                {
                    // task is pending removal, wait until it is switched out
                    if (task->IsPendingRemoval())
                    {
                        const size_t tasks_left = m_strategy.GetSize();
                      
                        if ((task != m_task_now) ||
                            ((tasks_left == 1U) && (m_fsm_state == FSM_STATE_SLEEPING)))
                        {
                            RemoveTask(task);
                            continue;
                        }
                    }
                }
 
                bool just_entered_sleep = false;
 
                // note: only currently scheduled task can be pending to sleep
                if ((task->m_state & KernelTask::STATE_SLEEP_PENDING) != 0U)
                {
                    STK_ASSERT(elapsed_ticks == 1); // entry tick must be a single, uncoalesced tick
 
                    // if Wake() raced in before this entry tick ran, STATE_WAKE_PENDING is set:
                    // deliver OnTaskSleep() below as usual, then fall through to the advance block
                    // in this same pass instead of waiting an extra tick, so the paired OnTaskWake()
                    // still fires on schedule.
                    const bool wake_pending = ((task->m_state & KernelTask::STATE_WAKE_PENDING) != 0U);
 
                    ProcessTaskPendingSleep(task);
 
                    just_entered_sleep = !wake_pending;
                }
 
                if (!just_entered_sleep && !task->IsSleepInfinite())
                {
                    // advance sleep time by number of elapsed ticks (always 1 if non-Tickless)
                    task->m_time_sleep += elapsed_ticks;
 
                    // deliver sleep event to the strategy
                    if __stk_constexpr_cpp17 (TStrategy::SLEEP_EVENT_API)
                    {
                        // notify strategy that the task woke up
                        if (!task->IsSleeping())
                        {
                            m_strategy.OnTaskWake(task);
                        }
                    }
                }
            }
            else
            {
                if __stk_constexpr_cpp17 (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))
                        {
                            // report deadline overrun to the strategy which supports overrun recovery
                            if __stk_constexpr_cpp17 (TStrategy::DEADLINE_MISSED_API)
                            {                                
                                if (!m_strategy.OnTaskDeadlineMissed(task))
                                {
                                    // report failure if it could not be recovered by the scheduling strategy
                                    task->HrtHardFailDeadline(&m_platform);
                                }
                            }
                            else
                            {
                                task->HrtHardFailDeadline(&m_platform);
                            }
                        }
                    }
                }
            }
 
            // get the number of ticks the driver has to keep CPU in Idle
            if __stk_constexpr_cpp17 (IsTicklessMode())
            {
                if ((sleep_ticks > 1) && task->IsBusy())
                {
                    sleep_ticks = task->GetSleepTicks(sleep_ticks);
                }
            }
        }
 
        return sleep_ticks;
    }

    void UpdateSyncObjects(const Timeout elapsed_ticks)
    {
        ISyncObject::ListEntryType *itr = m_sync_list->GetFirst();
 
        while (itr != nullptr)
        {
            ISyncObject::ListEntryType *const next = itr->GetNext();
 
            if (!util::DListCast::ListEntryToParent<ISyncObject>(itr)->Tick(elapsed_ticks))
            {
                m_sync_list->Unlink(itr);
            }
 
            itr = next;
        }
    }

    void UpdateTaskRequest()
    {
        // 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 __stk_constexpr_cpp17 (IsDynamicMode() && !IsHrtMode())
        {
            // process serialized AddTask request made from another active task, requesting process
            // is currently waiting due to SwitchToNext()
            if ((m_request & REQ_ADD_TASK) != 0U)
            {
                m_request &= ~REQ_ADD_TASK;
 
                for (size_t i = 0U; i < TASKS_MAX; ++i)
                {
                    KernelTask *const 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_SLEEP;
 
        // try getting next task for scheduling
        next = util::DListCast::ListEntryToParent<KernelTask>(m_strategy.GetNext());
 
        // sleep-aware strategy returns nullptr if no active tasks available
        if (next != nullptr)
        {
            // strategy must provide active-only task
            STK_ASSERT(!next->IsSleeping());
 
            // if was sleeping, process wake event first
            type = (m_fsm_state == FSM_STATE_SLEEPING ? FSM_EVENT_WAKE : FSM_EVENT_SWITCH);
        }
        // start sleeping
        else
        {
            if __stk_constexpr_cpp17 (IsDynamicMode())
            {
                // if nullptr is returned then either strategy has all tasks sleeping or none left,
                // if KERNEL_DYNAMIC mode and no tasks left then exit from scheduling
                if (m_strategy.GetSize() == 0U)
                {
                    next = nullptr;
                    type = FSM_EVENT_EXIT;
                }
            }
        }
 
        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 *const now = m_task_now, *next = nullptr;
        bool switch_context = false;
 
        const EFsmState new_state = GetNewFsmState(next);
 
        switch (new_state)
        {
        case FSM_STATE_SWITCHING:
            switch_context = StateSwitch(now, next, idle, active);
            m_fsm_state = new_state;
            break;
        case FSM_STATE_SLEEPING:
            switch_context = StateSleep(now, next, idle, active);
            m_fsm_state = new_state;
            break;
        case FSM_STATE_WAKING:
            switch_context = StateWake(now, next, idle, active);
            m_fsm_state = new_state;
            break;
        case FSM_STATE_EXITING:
            switch_context = StateExit(now, next, idle, active);
            m_fsm_state = new_state;
            break;
        case FSM_STATE_NONE:
            break; // valid intermittent non-persisting state: no-transition
        case FSM_STATE_MAX:
        default:   // invalid state value
            STK_KERNEL_PANIC(KERNEL_PANIC_BAD_STATE);
            break;
        }
 
        return switch_context;
    }

    bool StateSwitch(KernelTask *now, KernelTask *next, Stack *&idle, Stack *&active)
    {
        STK_ASSERT(now != nullptr);
        STK_ASSERT(next != nullptr);
        
        bool switch_context = false;
 
        // if equal: do not switch context because task did not change
        if (next != now)
        {
            idle   = now->GetUserStackPtr();
            active = next->GetUserStackPtr();
 
            // if stack memory is exceeded these assertions will be hit
        #if STK_STACK_GUARD
            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);
        #endif
 
            m_task_now = next;
 
            if __stk_constexpr_cpp17 (IsHrtMode())
            {
                if (now->m_hrt[0].done)
                {
                    now->HrtOnSwitchedOut();
                    next->HrtOnSwitchedIn();
                }
            }
 
            switch_context = true;
        }
 
        return 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->GetUserStackPtr();
 
        // if stack memory is exceeded these assertions will be hit
        STK_ASSERT(m_sleep_trap[0].memory[0] == STK_STACK_MEMORY_FILLER);
    #if STK_STACK_GUARD
        STK_ASSERT(next->GetUserTask()->GetStack()[0] == STK_STACK_MEMORY_FILLER);
    #endif
 
        m_task_now = next;
 
        if __stk_constexpr_cpp17 (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->GetUserStackPtr();
        active = &m_sleep_trap[0].stack;
 
        m_task_now = util::DListCast::ListEntryToParent<KernelTask>(m_strategy.GetFirst());
 
        if __stk_constexpr_cpp17 (IsHrtMode())
        {
            if (!now->IsPendingRemoval())
            {
                now->HrtOnSwitchedOut();
            }
        }
 
        return true; // switch context
    }

    bool StateExit(KernelTask *now, KernelTask *next, Stack *&idle, Stack *&active)
    {
        STK_UNUSED(now);
        STK_UNUSED(next);
 
        if __stk_constexpr_cpp17 (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_kstate != KSTATE_INACTIVE); }

    void ScheduleAddTask()
    {
        const hw::CriticalSection::ScopedLock cs_;
        m_request |= REQ_ADD_TASK;
    }
 
#if STK_SEGGER_SYSVIEW
    void SendTaskTraceInfo(KernelTask *task)
    {
        STK_ASSERT(task->IsBusy());
 
        const SEGGER_SYSVIEW_TASKINFO info =
        {
            .TaskID    = task->GetUserStackPtr()->tid,
            .sName     = task->GetUserTask()->GetTraceName(),
            .Prio      = static_cast<U32>(task->GetWeight()),
            .StackBase = hw::PtrToWord(task->GetUserTask()->GetStack()),
            .StackSize = (task->GetUserTask()->GetStackSize() * sizeof(Word))
        };
        SEGGER_SYSVIEW_SendTaskInfo(&info);
    }
#endif
 
    // Kernel modes:
    static constexpr bool IsStaticMode()   { return ((TMode & KERNEL_STATIC) != 0U); }
    static constexpr bool IsDynamicMode()  { return ((TMode & KERNEL_DYNAMIC) != 0U); }
    static constexpr bool IsHrtMode()      { return ((TMode & KERNEL_HRT) != 0U); }
    static constexpr bool IsSyncMode()     { return ((TMode & KERNEL_SYNC) != 0U); }
    static constexpr 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)));
 
    // If hit here: Strategy which supports Priority Inheritance API must also support Weight API.
    STK_STATIC_ASSERT_N(KERNEL_MODE_MUST_BE_SET, (TStrategy::PRIORITY_INHERITANCE_API && TStrategy::WEIGHT_API) ||
        !TStrategy::PRIORITY_INHERITANCE_API);

    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, 1U, 0U>::Value]; 
    EFsmState        m_fsm_state;       
    volatile uint8_t m_request;         
    volatile EKernelState m_kstate;     
    SyncObjectList   m_sync_list[STK_ALLOCATE_COUNT<TMode, KERNEL_SYNC, 1U, 0U>::Value]; 
 
    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_ */