Having fun with STR9 context switch for the RealView compiler

Hello all,
If you are interested, I am working on a new personal project - an ARM9 scheduler ("Sand Storm", but the name is still volatile. It will inherit a lot from sourceforge.net/.../, however it will of course run on STR7/9 variants instead). It is still a rudimentary implementation, which I made for demonstration purposes only.

we begin with this declaration:

typedef struct
{
        int32u          context[17] ; // buffer for all processor registers
        int8u           id ;
        task_state      status ;
        int8u           priority ; // task priority
} t_tcb ;

(tcb = task control block).

assuming 3 tasks are run, the following definitions are made:

t_tcb g_task1tcb, g_task2tcb, g_task3tcb,
         *g_tasks_tcb[] = {&g_task1tcb, &g_task2tcb, &g_task3tcb} ;

and

#define TASK_STACK_SIZE_NORMAL 0x100
int32u g_current_task_index = 0, g_current_task_index_tcb_index = 0, g_next_task_ptr = 0, g_current_task_ptr = 0,
           lp_task1_stack[TASK_STACK_SIZE_NORMAL],
           lp_task2_stack[TASK_STACK_SIZE_NORMAL],
           lp_task3_stack[TASK_STACK_SIZE_NORMAL] ;

each task needs to be configured prior to running, like this:

void tcb_setup(t_tcb *const ap_tcb, task_ptr ap_task, int32u *ap_stack, int16u a_stack_size)
{
        int32u *l_tcb_word = &ap_tcb->context[16], *lp_stack = ap_stack, l_value ;

        // IRQ ISR return address (ending of tcb)
        // the IRQ ISR must (according to the ARM architecture) decrement 4 from each
        // return address. so the return address of each IRQ ISR must be incremented by 4.
        *(l_tcb_word--) = (int32u)ap_task + 4 ;

        // SPSR
        *(l_tcb_word--) = (int32u)0x50 ;

        // R14 - internal task return address
        *(l_tcb_word--) = (int32u)ap_task ;

        // R13 - stack top
        *(l_tcb_word--) = (int32u)ap_stack ;

        // R12
        *(l_tcb_word--) = (int32u)0x12012012 ;

        // R11
        *(l_tcb_word--) = (int32u)0x11011011 ;

        // R10
        *(l_tcb_word--) = (int32u)0x10101010 ;

        // R9
        *(l_tcb_word--) = (int32u)0x99999999 ;

        // R8
        *(l_tcb_word--) = (int32u)0x88888888 ;

        // R7
        *(l_tcb_word--) = (int32u)0x77777777 ;

        // R6
        *(l_tcb_word--) = (int32u)0x66666666 ;

        // R5
        *(l_tcb_word--) = (int32u)0x55555555 ;

        // R4
        *(l_tcb_word--) = (int32u)0x44444444 ;

        // R3
        *(l_tcb_word--) = (int32u)0x33333333 ;

        // R2
        *(l_tcb_word--) = (int32u)0x22222222 ;

        // R1
        *(l_tcb_word--) = (int32u)0x11111111 ;

        // R0 (beginning of tcb)
        *(l_tcb_word--) = (int32u)0x00000000 ;

        // fill user provided stack with predicted, handy from debugging
        l_value = 0x1111 ;

        while (a_stack_size-- > 0)
        {
                *(lp_stack++) = l_value ;
                l_value <<= 1 ;

                if (l_value >= 0x1111000)
                {
                        l_value = 0x1111 ;
                }
        }
}

and

tcb_setup(&g_task1tcb, &task1, lp_task1_stack, TASK_STACK_SIZE_NORMAL) ;
tcb_setup(&g_task2tcb, &task2, lp_task2_stack, TASK_STACK_SIZE_NORMAL) ;
tcb_setup(&g_task3tcb, &task3, lp_task3_stack, TASK_STACK_SIZE_NORMAL) ;

where the task functions are defined the following way:
(only task1 is demonstrated):

void task1(void)
{
    while(1)
    {
    }
}