os.c

KaOS is a real-time operating system that has been implemented with the basic real-time constraints

void os_schedule(void)
{
    os_thread *temp_thread, *top_thread;

    #asm("cli");

    temp_thread = g_usedthreads;
    top_thread = g_currthread;

    while (temp_thread != NULL)
    {
        if (
            (
                temp_thread->state == THREADSTATE_RUNNING
            ) &&
            (
                temp_thread->sleep_duration == 0 ||
                --temp_thread->sleep_duration == 0
            ) &&
            (
                temp_thread->priority <= top_thread->priority ||
                top_thread->state != THREADSTATE_RUNNING ||
                top_thread->sleep_duration != 0
            )
        )
        {
            top_thread = temp_thread;
        }
        temp_thread = temp_thread->next;
    }
    if (top_thread != g_currthread)
    {
        // we need to move the thread about to be run to front of chain
        // this will allow 2 threads with same priority to alternate
        if (top_thread != g_usedthreads)
        {
            if (top_thread->prev != NULL)
            {
                // Hook left to right
                top_thread->prev->next = top_thread->next;
            }
            if (top_thread->next != NULL)
            {
                // Hook right to left
                top_thread->next->prev = top_thread->prev;
            }
            // Put current at front
            g_usedthreads->prev = top_thread;
            top_thread->next = g_usedthreads;
            g_usedthreads = top_thread;
        }

        //temp_thread = g_usedthreads;

        g_prevthread = g_currthread;
        g_currthread = top_thread;
            
        os_contextswitch();
    }
    else if (g_currthread != g_usedthreads)
    {
        if (g_currthread->prev != NULL)
        {
            // Hook left to right
            g_currthread->prev->next = g_currthread->next;
        }
        if (g_currthread->next != NULL)
        {
            // Hook right to left
            g_currthread->next->prev = g_currthread->prev;
        }
        // Put current at front
        g_usedthreads->prev = g_currthread;
        g_currthread->next = g_usedthreads;
        g_usedthreads = g_currthread;
    }
    #asm("sei");
}

/*
 * This context swtiching algortim was borrowed from aOS by Anssi Yl鋞alo
 */
void os_contextswitch(void)
{
    UCHAR *new_swstack, *new_hwstack;
    os_thread *curr_thread;

    #asm("cli");
                                
    curr_thread = g_prevthread;             /* Get address to current TCB */
    new_swstack = g_currthread->swstack;	/* Get highest priority ready tasks data stack */			
    new_hwstack = g_currthread->hwstack;	/* Get highest priority ready task's hw stack */				

    /*
        Context switch.

            ;	*dstk_highrdy -> Y+4
            ;	*hstk_highrdy -> Y+2
            ;	*curr_tcb -> Y+0

        1. Save HW SP and data SP to current task's PCB
        2. Load SP from highest priority task's TCB
        3. Restore context and return to new task
    */                    

    #asm
        ; Save all registers except SW stack pointer
        PUSH	R0
        PUSH	R1
        PUSH	R2		                                      
        PUSH	R3
        PUSH	R4
        PUSH	R5
        PUSH	R6
        PUSH	R7
        PUSH	R8
        PUSH	R9
        PUSH	R10
        PUSH	R11
        PUSH	R12
        PUSH	R13
        PUSH	R14
        PUSH	R15
        PUSH	R16
        PUSH	R17
        PUSH	R18
        PUSH	R19
        PUSH	R20
        PUSH	R21		
        PUSH	R22
        PUSH	R23
        PUSH	R24		                                      
        PUSH	R25
        PUSH	R26
        PUSH	R27
        PUSH	R30
        PUSH	R31
        IN		R0,SREG
        PUSH	R0
            																																		
        ; Save current HW stack pointer, low first         		        
        MOV		R26,R20		; hstk_current -> X
        MOV		R27,R21
            
        IN		R30,SPL	  ; *hstk_current = SP;
        ST		X+,R30
        IN		R30,SPH  
        ST		X+,R30

        ; Save current SW stack pointer, low first
        MOV		R30,R28   ; LOW(Y) -> R30
        ADIW	R30,6			; Restore Y value (3 pointers * 2 bytes each) 
        ST		X+,R30		; *dstk_current = Y;
        ST		X,R29

        ; Load new HW stack pointer, low first
        OUT	SPL,R18
        OUT	SPH,R19

        ; Load new SW stack pointer, low first
        MOV		R28,R17
        MOV		R29,R16  

        ; Pop all registers except SW stack pointer
        POP		R0
        OUT		SREG,R0
        POP		R31
        POP		R30
        POP		R27
        POP		R26
        POP		R25
        POP		R24
        POP		R23
        POP		R22
        POP		R21
        POP		R20
        POP		R19
        POP		R18
        POP		R17
        POP		R16
        POP		R15
        POP		R14
        POP		R13
        POP		R12
        POP		R11
        POP		R10
        POP		R9
        POP		R8
        POP		R7
        POP		R6
        POP		R5
        POP		R4
        POP		R3
        POP		R2
        POP		R1
        POP		R0
        RETI
    #endasm
}
void *os_alloc_stack(UINT stkSize)
{
    if (g_stkCurrent + stkSize <= g_stkEnd)
    {
        g_stkCurrent = (g_stkCurrent+stkSize);
        return (void*)(g_stkCurrent);
    }
    return NULL;
}
void os_reset_stack(void)
{
    g_stkCurrent = g_stkStart;
}