arch/arm-cortex-m3/sched.c

   1 /* Author: Domen Puncer <domen@cba.si>.  License: WTFPL, see file LICENSE */
   2 #include <stdio.h>
   3
   4 #include <sched.h>
   5 #include <compiler.h>
   6
   7 #include <arch/sched.h>
   8 #include <arch/cm3_regs.h>
   9
  10
  11 u32 arch_ticks_now()
  12 {
  13         return DWT->CYCCNT;
  14 }
  15
  16 /* cortex-m3 pushes following on stack when entering exception: xPSR, PC, LR, r12, r3, r2, r1, r0
  17  * what's remaining/needs to be saved is: PSP, r4-r11
  18  */
  19 void arch_task_new(struct task *task, void (*func)(u32 arg), u32 arg)
  20 {
  21         /* saved by exception */
  22         task->stack[task->stack_len-1] = 1<<24 /* xPSR, thumb bit */;
  23         task->stack[task->stack_len-2] = (u32)func;
  24         task->stack[task->stack_len-3] = 0x01010101*14 /* LR - this could be something nice, so tasks can actually exit */;
  25         task->stack[task->stack_len-4] = 0x01010101*12 /* r12 */;
  26         task->stack[task->stack_len-5] = 0x01010101*3  /* r3 */;
  27         task->stack[task->stack_len-6] = 0x01010101*2  /* r2 */;
  28         task->stack[task->stack_len-7] = 0x01010101*1  /* r1 */;
  29         task->stack[task->stack_len-8] = arg /* r0 */;
  30
  31         /* saved manually */
  32         task->stack[task->stack_len-9] = 0x01010101*11 /* r11 */;
  33         task->stack[task->stack_len-10] = 0x01010101*10 /* r10 */;
  34         task->stack[task->stack_len-11] = 0x01010101*9 /* r9 */;
  35         task->stack[task->stack_len-12] = 0x01010101*8 /* r8 */;
  36         task->stack[task->stack_len-13] = 0x01010101*7 /* r7 */;
  37         task->stack[task->stack_len-14] = 0x01010101*6 /* r6 */;
  38         task->stack[task->stack_len-15] = 0x01010101*5 /* r5 */;
  39         task->stack[task->stack_len-16] = 0x01010101*4 /* r4 */;
  40
  41         task->context.psp = (u32)&task->stack[task->stack_len-16];
  42 }
  43
  44 void __naked arch_task_first(struct task *task)
  45 {
  46         current = task;
  47         asm volatile (
  48                         /* restore created context */
  49                         "mov    sp, %0\n\t"
  50                         "pop    {r4-r11}\n\t"
  51                         "pop    {r0-r3,r12,lr}\n\t"
  52
  53                         "add    sp, sp, #8\n\t"    /* pc and xPSR space, just ignore it */
  54
  55                         "ldr    pc, [sp, #-8]\n\t" /* and jump to the task entry, sp here is top of stack, so -8 is entry -2 = func */
  56                         : : "r" (task->context.psp)
  57         );
  58 }
  59
  60 /* task to switch to */
  61 static struct task *new_task;
  62
  63 /* cortex-m3 trm 5.11 Setting up multiple stacks
  64  * does the task switching from current to new_task */
  65 void __naked pendsv_handler()
  66 {
  67         asm volatile (
  68                         "ldr    r0, =current\n\t"
  69                         "ldr    r1, [r0]\n\t"
  70
  71                         "mrs    r3, PSP\n\t"
  72                         "stmdb  r3!, {r4-r11}\n\t" /* save regs to process stack */
  73                         "str    r3, [r1, %0]\n\t"  /* current->context.psp = PSP */
  74
  75                         "ldr    r2, =new_task\n\t"
  76                         "ldr    r2, [r2]\n\t"
  77
  78                         "ldr    r3, [r2, %0]\n\t"  /* PSP = new_task->context.psp */
  79                         "ldmia  r3!, {r4-r11}\n\t" /* restore regs from process stack */
  80                         "msr    PSP, r3\n\t"
  81
  82
  83                         "str    r2, [r0]\n\t"      /* current = new_task */
  84
  85                         "bx     lr"
  86                         : : "i" (offsetof(struct task, context))
  87         );
  88 }
  89
  90 void arch_task_switch(struct task *newt)
  91 {
  92         new_task = newt;
  93         /* request PendSV exception */
  94         ICSR = ICSR_PENDSVSET;
  95 }
  96
  97 void arch_sched_start_timer()
  98 {
  99         DEMCR |= DEMCR_TRCENA;
 100         DWT->CYCCNT = 0;
 101         DWT->CTRL |= 1<<0;
 102
 103         /* core clock, enable interrupt, enable systick */
 104         SYSTICK->RELOAD = 0;
 105         SYSTICK->VALUE = 0;
 106         SYSTICK->CTRL |= 1<<2 | 1<<1 | 1<<0;
 107
 108         /* set SysTick pending to get scheduled asap */
 109         ICSR = ICSR_PENDSTSET;
 110 }
 111
 112 void arch_wait_for_interrupt()
 113 {
 114         asm volatile ("wfi");
 115 }
 116
 117 static int systick_whole;
 118 static int systick_remainder;
 119
 120 /* writing anything to SYSTICK->VALUE clears it (and doesn't cause the interrupt), but the reloads it with RELOAD on next timer tick */
 121 void arch_sched_next_interrupt(int offset)
 122 {
 123         systick_whole = offset >> 24;
 124         systick_remainder = offset & 0xffffff;
 125         if (systick_remainder == 0)
 126                 systick_remainder = 1;
 127
 128         if (systick_whole) {
 129                 SYSTICK->RELOAD = 0xffffff;
 130                 SYSTICK->VALUE = 0;
 131         } else {
 132                 SYSTICK->RELOAD = systick_remainder;
 133                 SYSTICK->VALUE = 0; /* clear */
 134         }
 135 }
 136
 137 void systick_handler()
 138 {
 139         if (systick_whole) {
 140                 /* last part? then load the remainder */
 141                 if (--systick_whole == 0) {
 142                         SYSTICK->RELOAD = systick_remainder;
 143                         SYSTICK->VALUE = 0;
 144                 }
 145                 return;
 146         }
 147         /* disable timer */
 148         SYSTICK->RELOAD = 0;
 149         SYSTICK->VALUE = 0;
 150         sched_interrupt();
 151 }