RtosPortable.cpp

//------------------------------------------------------------------------------
//This file is part of embKernel. 
//See license.txt for the full license governing this code.
//------------------------------------------------------------------------------

#include "RtosPortable.hpp"
#include "RtosInterrupt.hpp"
#include "RtosCfg.hpp"
#include "Rtos.hpp"
#include "RtosTask.hpp"
#include "RtosMacros.hpp"
#include <stdint.h>

void RtosPortable::start() {
    SCB ->SHP[7] = RTOS_INT_PRI; //SVCall interrupt priority
    SCB ->SHP[10] = RTOS_INT_PRI; //SysTick interrupt priority
    SCB ->SHP[11] = RTOS_INT_PRI; //PendSV interrupt priority

    SysTick ->LOAD = (RTOS_CFG_CPU_FREQUENCY / RTOS_CFG_TICK_FREQUENCY) - 1; //set frequency
    SysTick ->CTRL = 0x00000007; //uC clock, interrupt enabled, counter enabled

    //Start the first task
    __asm volatile(
            "    ldr r0, =0xE000ED08  \n"
            "    ldr r0, [r0]         \n"
            "    ldr r0, [r0]         \n"
            "    msr msp, r0          \n"
            "    svc 0                \n"
    );
}

void RtosPortable::yield() {
    RTOS_ASSERT((uint32_t) Rtos::sCurrentTask->mIterableCore.mTaskOwner && (uint32_t) Rtos::sCurrentTask->mIterableCore.mTaskOwner < 0x2000A000); //
    SCB ->ICSR |= 1 << 28; //pendSV interrupt trigger
}

void* RtosPortable::stackInit(void* stack, size_t stackSize, void (*run)(void*), void *parameter) {
    uint32_t* ptr = (uint32_t*) stack;
    size_t stackSizeWords = stackSize >> 2;

    for (size_t i = 0; i < stackSizeWords; i++) {
        ptr[i] = STACK_FILL_VALUE;
    }

    ptr = &(((uint32_t*) stack)[stackSizeWords - 1]);

    *--ptr = 0x01000000; // PSR
    *--ptr = (uint32_t) run; // PC (R15)
    *--ptr = (uint32_t) 0; // LR (R14)
    *--ptr = 0x12121212; // R12
    *--ptr = 0x03030303; // R3
    *--ptr = 0x02020202; // R2
    *--ptr = 0x01010101; // R1
    *--ptr = (uint32_t) parameter; // R0
    *--ptr = 0xFFFFFFFD; // EXC_RETURN
    *--ptr = 0x11111111; // R11
    *--ptr = 0x10101010; // R10
    *--ptr = 0x09090909; // R9
    *--ptr = 0x08080808; // R8
    *--ptr = 0x07070707; // R7
    *--ptr = 0x06060606; // R6
    *--ptr = 0x05050505; // R5
    *--ptr = 0x04040404; // R4

    return ptr;
}

bool RtosPortable::isStackOverflow(void* stack, size_t stackSize) {
    return (((uint32_t*) stack)[0] != STACK_FILL_VALUE);
}

size_t RtosPortable::getFreeStackSize(void* stack, size_t stackSize) {
    uint32_t* p = (uint32_t*) stack;
    size_t size = 0;
    for (; size < stackSize; size += 4) {
        if (*p++ != STACK_FILL_VALUE) {
            break;
        }
    }
    return size;
}

uint32_t RtosPortable::getCyclesCounter() {
    uint32_t cycles = SysTick ->LOAD - SysTick ->VAL;
    uint32_t ticks = Rtos::sTick;
    if (SysTick ->CTRL & SysTick_CTRL_COUNTFLAG_Msk) { //Systick overflow?
        ticks++;
        cycles = SysTick ->LOAD - SysTick ->VAL;
    }
    return ((ticks * (RTOS_CFG_CPU_FREQUENCY / RTOS_CFG_TICK_FREQUENCY)) + cycles);
}

void RtosPortable::enterCriticalSection() {
    __asm volatile
    (
            "    mov r0, #0x80                            \n"
            "    msr basepri, r0                          \n"
            :::"r0"
    );
}

void RtosPortable::exitCriticalSection() {
    __asm volatile
    (
            "    mov r0, #0                               \n"
            "    msr basepri, r0                          \n"
            :::"r0"
    );
}

void RtosPortable::disableAllInt() {
    __asm volatile
    (
            "    mov r0, #1                \n "
            "    msr primask, r0           \n "
            :::"r0"
    );
}

void RtosPortable::enableAllInt() {
    __asm volatile
    (
            "    mov r0, #0                     \n "
            "    msr primask, r0                \n "
            :::"r0"
    );
}

__attribute__((naked)) void RtosInterrupt::IRQ_SVCall(void) {
    __asm volatile (
            "    mov    r0, %[currentTask]                \n"
            "    ldmia  r0!, {r4-r11}                     \n"
            "    ldmia  r0!, {lr}                         \n"
            "    msr    psp, r0                           \n"
            "    mov    r0, #0                            \n"
            "    msr    basepri, r0                       \n"
            "    bx     lr                                \n"
            ::[currentTask]"r"(Rtos::sCurrentTask->mCurrentStackPointer):);
}

__attribute__((naked)) void RtosInterrupt::IRQ_SysTick(void) {
    __asm volatile (
            "    mrs    r0, psp                           \n"
            "    tst r14, #0x10                           \n" // Is the task is using the FPU context save hi vfp registers
            "    it eq                                    \n"
            "    vstmdbeq r0!, {s16-s31}                  \n"
            "    stmdb  r0!, {lr}                         \n"
            "    stmdb  r0!, {r4-r11}                     \n"
            "    push   {r0} \n"
            "    bl     _ZN4Rtos6onTickEv                 \n"
#if RTOS_CFG_TRACE_BUFFER_SIZE
            "    movw   r0, #0xe010                       \n" //Reset COUNTFLAG bit
            "    movt   r0, #0xe000                       \n"
            "    ldr    r0,[r0]                           \n"
#endif
            "    pop   {r0} \n"
            "    bl     _ZN4Rtos8scheduleEPv              \n"
            "    ldmia  r0!, {r4-r11}                     \n"
            "    ldmia  r0!, {lr}                         \n"
            "    tst r14, #0x10                           \n" // If the task is using the FPU context restore hi vfp registers
            "    it eq                                    \n"
            "    vldmiaeq r0!, {s16-s31}                  \n"
            "    msr    psp, r0                           \n"
            "    mov    r0, #0                            \n"
            "    msr    basepri, r0                       \n"
            "    bx     lr                                \n"
    );
}

__attribute__((naked)) void RtosInterrupt::IRQ_PendSV(void) {
    __asm volatile (
            "    mrs    r0, psp                           \n"
            "    tst r14, #0x10                           \n" // If the task is using the FPU context save hi vfp registers
            "    it eq                                    \n"
            "    vstmdbeq r0!, {s16-s31}                  \n"
            "    stmdb  r0!, {lr}                         \n"
            "    stmdb  r0!, {r4-r11}                     \n"
            "    bl     _ZN4Rtos8scheduleEPv              \n"
            "    ldmia  r0!, {r4-r11}                     \n"
            "    ldmia  r0!, {lr}                         \n"
            "    tst r14, #0x10                           \n"// If the task is using the FPU context restore hi vfp registers
            "    it eq                                    \n"
            "    vldmiaeq r0!, {s16-s31}                  \n"
            "    msr    psp, r0                           \n"
            "    mov    r0, #0                            \n"
            "    msr    basepri, r0                       \n"
            "    bx     lr                                \n"
    );
}