Rtos.cpp

//------------------------------------------------------------------------------
//This file is part of embKernel. 
//See license.txt for the full license governing this code.
//------------------------------------------------------------------------------
#include "Rtos.hpp"
#include "RtosTask.hpp"
#include "RtosList.hpp"
#include "RtosPortable.hpp"
#include "RtosHeap.hpp"
#include <string.h>

Rtos rtos;

RtosTask* Rtos::sCurrentTask;
Rtos::TICK Rtos::sTick;
RtosList Rtos::sListReady[RTOS_CFG_MAX_PRIORITIES];
RtosList Rtos::sListSleep;
RtosList Rtos::sListSuspended;
size_t Rtos::sMaxPriorities = RTOS_CFG_MAX_PRIORITIES;
size_t Rtos::sCurrentTaskCount = 0;
void* Rtos::sMemToFree = 0;
RtosTask Rtos::sIdleTask(0, "idle", RTOS_CFG_MIN_STACK_SIZE, Rtos::idleRun);

#if RTOS_CFG_TRACE_BUFFER_SIZE
Rtos::TRACE_BUFFER Rtos::sTraceBuffer;
#endif

void Rtos::start() {
    sMemToFree = 0;
    sTick = 0xFFFFFFFF - 1234;

    sCurrentTask = &sIdleTask;
    schedule(sCurrentTask->mCurrentStackPointer);

#if RTOS_CFG_TRACE_BUFFER_SIZE
    clearTraceBuffer();
#endif
    RtosPortable::start();
}

void Rtos::sleep(TICK tick) {
    if (!tick) { //Tick=0 -> return
        return;
    }

    RtosPortable::enterCriticalSection();

    putOnSleepingList(sCurrentTask, tick);

    RtosPortable::yield();
    RtosPortable::exitCriticalSection();
}

void Rtos::sleepUntil(TICK tick) {
    RtosPortable::enterCriticalSection();

    TICK ticksToSleep = 1;
    if (!isElapsed(tick)) {
        ticksToSleep = tick - sTick;
    }

    putOnSleepingList(sCurrentTask, ticksToSleep);

    RtosPortable::yield();
    RtosPortable::exitCriticalSection();
}

bool Rtos::isElapsed(TICK tick) {
    if ((sTick == tick)) {
        return true;
    }
    TICK diff = tick - sTick;
    if (diff > (TICK_MAX >> 1)) {
        return true;
    }
    return false;

}

void Rtos::addTask(RtosTask* task) {
    RtosPortable::enterCriticalSection();
    sCurrentTaskCount++;
    sListReady[task->mPriority].insertAtBeginning(task->mIterableCore);
    RtosPortable::exitCriticalSection();
}

void Rtos::removeTask(RtosTask* task) {
    RtosPortable::enterCriticalSection();
    if (task->mIterableCore.mListOwner) {
        task->mIterableCore.mListOwner->remove(task->mIterableCore);
    }
    if (task->mIterableSyncObj.mListOwner) {
        task->mIterableSyncObj.mListOwner->remove(task->mIterableCore);
    }
    sCurrentTaskCount--;

    schedule(sCurrentTask->mCurrentStackPointer);

    sMemToFree = task->mStack;
    RtosPortable::yield();
    RtosPortable::exitCriticalSection();
}

void Rtos::putOnSleepingList(RtosTask* task, TICK tick) {
    RTOS_ASSERT(!RtosPortable::isStackOverflow(task->mStack, task->mStackSize));

    task->mIsTimeout = false;

    RtosListIterable* iterable = &task->mIterableCore;

    iterable->mListOwner->remove(*iterable);

    if (tick == TICK_INFINITE) {
        return;
    }

    iterable->mSortValue = sTick + tick;
    sListSleep.insertSorted(*iterable, sTick);
}

void Rtos::putOnSuspendedList(RtosTask* task) {
    RtosListIterable* iterable = &task->mIterableCore;
    iterable->mListOwner->remove(*iterable);
    sListSuspended.insertAtEnd(*iterable);
}

void Rtos::onTick() {
    sTick++;

    for (RtosListIterable* iterable = sListSleep.mFirst; iterable && (iterable->mSortValue == sTick);) {
        RtosListIterable* next = iterable->mNext;
        sListSleep.getFirst();

        RtosTask* task = iterable->mTaskOwner;

        if (task->mIterableSyncObj.mListOwner) { //Event timeout
            task->mIsTimeout = true;
            task->mIterableSyncObj.mListOwner->remove(task->mIterableSyncObj);
        }

        sListReady[task->mPriority].insertAtBeginning(*iterable);

        iterable = next;
    }
}

void* Rtos::schedule(void* stackPointer) {
    sCurrentTask->mCurrentStackPointer = stackPointer;

    //remove memory used by killed task
    if (sMemToFree) {
        RtosHeap::free(sMemToFree);
        sMemToFree = 0;
    }

    RTOS_ASSERT(!RtosPortable::isStackOverflow(sCurrentTask->mStack, sCurrentTask->mStackSize)); //

#if RTOS_CFG_TRACE_BUFFER_SIZE
    RtosTask* previousTask = sCurrentTask;
#endif

    for (int i = RTOS_CFG_MAX_PRIORITIES - 1; i >= 0; i--) {
        RtosListIterable* iterable = sListReady[i].getFirst();
        if (iterable) {
            sCurrentTask = iterable->mTaskOwner;
            sListReady[i].insertAtEnd(sCurrentTask->mIterableCore);
            break;
        }
    }

#if RTOS_CFG_TRACE_BUFFER_SIZE
    if (previousTask != sCurrentTask) {
        if (sTraceBuffer.count < RTOS_CFG_TRACE_BUFFER_SIZE) {
            sTraceBuffer.buffer[sTraceBuffer.indexIn].cycles = RtosPortable::getCyclesCounter();
            sTraceBuffer.buffer[sTraceBuffer.indexIn].task = sCurrentTask;
            sTraceBuffer.indexIn++;
            if (sTraceBuffer.indexIn >= RTOS_CFG_TRACE_BUFFER_SIZE) {
                sTraceBuffer.indexIn = 0;
            }
            sTraceBuffer.count++;
        }
        else {
            sTraceBuffer.overloadError = true;
        }
    }
#endif

    return sCurrentTask->mCurrentStackPointer;
}

void Rtos::idleRun(RtosTask* task) {
    while (1) {
#if RTOS_CFG_USE_LOW_POWER_IN_IDLE
        RTOS_LOW_POWER_MODE();
#else
        RTOS_NOP();
#endif
    }
}



#if RTOS_CFG_TRACE_BUFFER_SIZE
int Rtos::getTraceBuffer(TRACE_TAG** p) {
    if (sTraceBuffer.overloadError) {
        return -1;
    }
    int count = sTraceBuffer.count;
    if (count <= 0) {
        return 0;
    }
    *p = &sTraceBuffer.buffer[sTraceBuffer.indexOut];
    if (sTraceBuffer.indexOut + count > RTOS_CFG_TRACE_BUFFER_SIZE) {
        count = RTOS_CFG_TRACE_BUFFER_SIZE - sTraceBuffer.indexOut;
        sTraceBuffer.indexOut = 0;
    }
    else {
        sTraceBuffer.indexOut += count;
    }
    return count;
}

void Rtos::releaseBuffer(int count) {
    RtosPortable::enterCriticalSection();
    sTraceBuffer.count -= count;
    RtosPortable::exitCriticalSection();
}

void Rtos::clearTraceBuffer() {
    RtosPortable::enterCriticalSection();
    memset(&sTraceBuffer, 0, sizeof(sTraceBuffer));
    RtosPortable::exitCriticalSection();
}
#endif