Arm Cortex-M Context Switching Part 4

The following changes change the task switching mechanism to allow a simple priority system for tasks, first, alter task.h as follows:

#ifndef __TASK_H
#define __TASK_H

#include <stdint.h>

#define TASK_MAX 4

#define TASK_FREE     -1
#define TASK_WAITING    1    
#define TASK_READY     2
#define TASK_RUNNING     3

/* Need to mask out thumb bit */
#define TASK_PC_MASK 0xfffffffe

#define MAIN_SP     0x20001000
#define PROC_STACK_SIZE 256
#define PROC_STACK_TOP     (MAIN_SP - PROC_STACK_SIZE)

#define PSR_INIT 0x21000000 

#define EXC_RET_THREAD 0xfffffff9


struct task_t {
    uint32_t *sp;
    uint32_t state;
    
    /* Lower value is higher priority */
    int32_t priority;

    /* Each time the timer_func is called this gets
       decremented, if it reached 0 then the task is 
       set to TASK_READY and the task_wait is reinitialised
       with priority */

    int32_t wait;
};

void task_init();
int task_create(void (*task)(void), uint32_t priority);
struct task_t * task_ready();
__attribute__((naked)) void task_switch(void);
#endif /* __TASK_H */

 

Each task has a counter which is decremented by each timer_func() invocation, if the timer reaches zero than it is scheduled to run, this a quite crude mechanism in a sense it has given a particular task more opportunity to be run based on the priority.

Change task.c, as shown below:

#include <stdint.h>
#include <task.h>

static struct task_t task_table[TASK_MAX] __attribute__((section(".data")));

/* Number of tasks created */
static int task_created  __attribute__((section(".data"))); 

void task_init()
{
        int i = 0;

        for(i = 0; i < TASK_MAX; i++){
                task_table[i].state = TASK_FREE;
        }
}

int task_create(void (*task)(void), uint32_t priority)
{
        int i = 0;

        for(i = 0; i < TASK_MAX; i++) {
                if(task_table[i].state == TASK_FREE) {
                        task_table[i].sp = (void *) (PROC_STACK_TOP 
                                                - (i * PROC_STACK_SIZE));
        
                        /* setup initial stack frame */
                        *(task_table[i].sp--) =  PSR_INIT;
                        *(task_table[i].sp--) = (uint32_t) task & TASK_PC_MASK;
                        *(task_table[i].sp--) = 0;      /* lr */ 
                        *(task_table[i].sp--) = 0;      /* r12 */
                        *(task_table[i].sp--) = 0;      /* r3  */
                        *(task_table[i].sp--) = 0;      /* r2  */
                        *(task_table[i].sp--) = 0;      /* r1  */
                        *(task_table[i].sp--) = 0;      /* r0  */

                        *(task_table[i].sp--) = 0;      /* r11  */
                        *(task_table[i].sp--) = 0;      /* r10  */
                        *(task_table[i].sp--) = 0;      /* r9   */
                        *(task_table[i].sp--) = 0;      /* r8   */
                        *(task_table[i].sp--) = 0;      /* r7   */
                        *(task_table[i].sp--) = 0;      /* r6   */
                        *(task_table[i].sp--) = 0;      /* r5   */
                        *(task_table[i].sp) = 0;        /* r4   */

                        task_table[i].priority = priority;
                        task_table[i].wait = priority;  
                        task_table[i].state = TASK_WAITING;
                        task_created++; 
                        return 0;
                }
        } 
        
        return -1;
}

struct task_t * task_ready()
{
        int i = 0;

        for(i = 0; i < TASK_MAX; i++) {
                if(task_table[i].state == TASK_READY) {
                        return &(task_table[i]);
                }
        }

        return (struct task_t *) 0;
}

struct task_t * task_running()
{
        int i = 0;

        for(i = 0; i < TASK_MAX; i++) {
                if(task_table[i].state == TASK_RUNNING) {
                        return &(task_table[i]);
                }
        }

        return (struct task_t *) 0;
}

int task_schedule()
{
        static int i = 0;
        int reset_i = 0;
        int ret = 0;

        /* Reset round robin */
        if(i == task_created) {
                i = 0;
        }
        
        
        for(i; i < task_created; i++) {
                if(task_table[i].state == TASK_RUNNING) {
                        if(i == (task_created - 1)) {
                                /* Last task, so wrap around back to
                                   start of task table */
                                i = -1;
                                /* Also make sure i is zero at return */
                                reset_i = 1;
                        }

                        continue;
                }

                task_table[i].wait--;

                if(task_table[i].wait < 0) {
                        task_table[i].state = TASK_READY;
                        task_table[i].wait = task_table[i].priority;
                        ret = 1;
                        break;
                } 
        }

        if(reset_i) {
                i = 0;
        }

        return ret;
        
}

void *saved_psp __attribute__((section(".data")));

__attribute__((naked))void task_save_psp(void *sp)
{
        saved_psp = sp;
        asm volatile("bx lr");
}

void * task_get_psp(void)
{
        return saved_psp;
}

__attribute__((naked))void task_save(void *task_sp)
{
        /* Save MSP */
        asm volatile("mov r1, sp");
        /* Copy registers */
        asm volatile("mov sp, r0");
        asm volatile("push {r4-r11}");

        /* Save new PSP */
        asm volatile("mov r0, sp");
        
        /* Restore MSP */
        asm volatile("mov sp, r1");

        /* Save lr */
        asm volatile("push {lr}");

        /* Really save psp */
        asm volatile("bl task_save_psp");

        struct task_t *task_cur = (struct task_t *) 0;

        task_cur = task_running();

        if(task_cur) {
                task_cur->sp = task_get_psp();
        } else {
                /* We are in MSP mode already */
        } 
        
        asm volatile("pop {lr}");
        asm volatile("bx lr");  
}

__attribute__((naked)) void task_restore(void *task_sp)
{
        /* Save MSP */
        asm volatile("mov r1, sp");
        asm volatile("msr msp, r1");

        /* Restore task stackpointer */
        asm volatile("mov sp, r0");

        /* Restore sw frame */
        asm volatile("pop {r4-r11}");

        /* Restore psp */
        asm volatile("mov r0, sp");
        asm volatile("msr psp, r0");

        /* MSP back to sp */
        asm volatile("mov sp, r1");
        
        /* Return from exception */
        asm volatile("mov lr, #0xfffffffd");
        asm volatile("bx lr");
}

__attribute__((naked)) void task_switch()
{
        /* Get PSP */
        asm volatile("mrs r0, psp");
        
        /* Is it non zero */
        /* then save state */
        asm volatile("cmp r0, #0");
        asm volatile("it ne");
        asm volatile("blne task_save");
                
        struct task_t *task_cur = (struct task_t *) 0;
        struct task_t *task_next = (struct task_t *) 0;
        
        task_cur = task_running();
        task_next = task_ready();

        if(task_next && task_cur) {
                task_cur->state = TASK_WAITING;
                task_cur->wait = task_cur->priority;    
        }
        
        if(task_next) {
                task_next->state = TASK_RUNNING;
                task_restore(task_next->sp);
                /* Never return from here */
        }
        
        /* Return from exception */
        asm volatile("mov lr, #0xfffffff9");
        asm volatile("bx lr");
}



Change timer.c as shown below:

#include <stdint.h>
#include <task.h>

#define SYSTICK_BASE ((uint32_t *) 0xe000e000)
#define SYSTICK_CTRL ((uint32_t *) 0xe000e010)
#define SYSTICK_TRELOAD ((uint32_t *) 0xe000e014)
#define SYSTICK_CTRL_ACTIVATE (0x7)

void timer_start()
{
        *SYSTICK_CTRL = SYSTICK_CTRL_ACTIVATE;
}

void timer_stop()
{
        *SYSTICK_CTRL = 0x0;
}

void timer_reload(uint32_t reload)
{
        *SYSTICK_TRELOAD = reload;
}

__attribute__((interrupt)) void timer_func()
{
        /* The timer_func gets called at approx 100
           times per second, we only do a task switch
           per second */

        static uint32_t no_task_switch = 100;

        if(no_task_switch) {
                no_task_switch--;
        } else {
                if(task_schedule())
                        /* Trigger the pendSV */
                        *((uint32_t volatile *)0xE000ED04) = 0x10000000;

                /* Reset the counter */
                no_task_switch = 100;
        }
}

Change main.c to now create tasks with a priority paramter.

#include <stdint.h>
#include <stdio.h>
#include <task.h>


/* QEMU uses a 12Mhz system clock */
/* So 0x00b71b00 in theory would be used */
/* However the following value is more accurate on my system YMMV */

#define SYSTICKS_PER_SECOND ((uint32_t) 0x00c00000)
 
#define TIMER_RELOAD (SYSTICKS_PER_SECOND / 100)

void task_1(void)
{
    while(1) {
        puts("TASK_1\n");
    }
}

void task_2(void)
{
    while(1) {
        puts("    TASK_2\n");
    }
}

void task_3(void)
{
    while(1) {
        puts("        TASK_3\n");
    }
}

int main() 
{
    task_init();
    task_create(task_1, 0);
    task_create(task_2, 1);
    task_create(task_3, 2);
    
    timer_reload(TIMER_RELOAD);
    timer_start();

    while(1) { ; }

    return 0;    
}

Try running this example and note the frequency of each tasks output.

 

Advertisements