Arm Cortex-M Context Switching Part 2

From the previous example we saw how to restore a task and set up an initial task’s stack to allow a task to be started. However to run concurrent tasks we must also have a method to save a tasks state. This example demonstrates how to do this.

Example code can be cloned from BitBucket

 

First modify task.h to increase the amount of tasks available, as follows:

#ifndef __TASK_H
#define __TASK_H

#include <stdint.h>

#define TASK_MAX 4

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

/* 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;
};

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

Now we modify task.c to save tasks using the task_save() function, we have also changed the task_switch() function to use this new functionality, note also at the start of the function the value of the PSP (Process Stack Pointer) is checked, if this is zero it means that the no tasks have been started, so the code should not attempt to save the task state, as shown in the following code snippet:

        /* 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");

If it is non-zero, however, it means a task is being executed and the state must be saved.

The complete task.c is now as follows:

#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))
{
        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].state = TASK_READY;
                        task_created++; 
                        return 0;
                }
        } 
        
        return -1;
}

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

        if(i == (task_created - 1)) {
                i = 0;
        }

        for(i; 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;
}

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) {
                /* Don't set this here */
                /* task_cur->state = TASK_READY; */
                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_READY;
        }
        
        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");
}


Now change timer_func()  in timer.c to not disable the timer on the first call:

#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__((naked)) void timer_func()
{
    asm volatile ("b task_switch");
}

And, finally, change main.c to have more than one task:

#include <task.h>

#define TIMER_RELOAD 0x00fffff

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);
    task_create(task_2);
    task_create(task_3);
    
    timer_reload(TIMER_RELOAD);
    timer_start();

    while(1) { ; }

    return 0;    
}

Start the QEMU instance using make run

arm-none-eabi-as -g -o vector_table.o vector_table.s
arm-none-eabi-gcc -I. -nostdlib -g -mcpu=cortex-m3 -mthumb -c main.c
arm-none-eabi-gcc -I. -nostdlib -g -mcpu=cortex-m3 -mthumb -c serial.c
arm-none-eabi-gcc -I. -nostdlib -g -mcpu=cortex-m3 -mthumb -c timer.c
arm-none-eabi-gcc -I. -nostdlib -g -mcpu=cortex-m3 -mthumb -c task.c
arm-none-eabi-ld -g -o startup.elf -T startup.ld vector_table.o main.o serial.o timer.o task.o 
qemu-system-arm -machine lm3s6965evb -S -gdb tcp::1234 -nographic -kernel startup.elf -chardev tty,id=any,path=/dev/pts/2

Start the debugger and continue:

make debug
arm-none-eabi-gdb -x gdbinit
GNU gdb (GNU Tools for ARM Embedded Processors) 7.6.0.20131129-cvs
Copyright (C) 2013 Free Software Foundation, Inc.
License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>
This is free software: you are free to change and redistribute it.
There is NO WARRANTY, to the extent permitted by law.  Type "show copying"
and "show warranty" for details.
This GDB was configured as "--host=i686-linux-gnu --target=arm-none-eabi".
For bug reporting instructions, please see:
<http://www.gnu.org/software/gdb/bugs/>.
0x00000040 in ?? ()
Loading section .text, size 0x4c4 lma 0x0
Loading section .rodata, size 0x18 lma 0x4c4
Loading section .data, size 0x28 lma 0x20000000
Start address 0x0, load size 1284
Transfer rate: 1253 KB/sec, 428 bytes/write.
Breakpoint 1 at 0x40: file vector_table.s, line 29.

Breakpoint 1, reset_handler () at vector_table.s:29
29        bl main
r0             0x0    0
r1             0x0    0
r2             0x0    0
r3             0x0    0
r4             0x0    0
r5             0x0    0
r6             0x0    0
r7             0x0    0
r8             0x0    0
r9             0x0    0
r10            0x0    0
r11            0x0    0
r12            0x0    0
sp             0x20001000    0x20001000
lr             0x0    0
pc             0x40    0x40 <reset_handler>
cpsr           0x40000173    1073742195
(gdb) cont
Continuing

The output from the QEMU instance is as follows:

TASK_1
TASK_1
TASK_1
TASK_1
<snipped>
TASK_2
TASK_2
TASK_2
TASK_2
<snipped>
TASK_3
TASK_3
TASK_3
TASK_3
<snipped>
TASK_1
TASK_1
TASK_1
TASK_1

The output shows the tasks being executed. In the next example we show how to defer execution of the task switcher using the PendSV handler.

 

Advertisements