arch/arm-cortex-m0/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/cm0_regs.h>
   9
  10
  11 static u32 current_counter;
  12
  13 u32 arch_ticks_now()
  14 {
  15         /* so, here's the deal, SYSTICK->VALUE == 0 means we counted down,
  16          * but then current_counter is already incremented, so just treat
  17          * it as 0xffffff, which is the best approximation */
  18         /* VALUE of 0 will add 0 to current_counter here: */
  19         /* or so I thought, but following DOES NOT work ok at all */
  20         //return current_counter + 0xffffff - ((SYSTICK->VALUE-1) & 0xffffff);
  21
  22         return current_counter + 0xffffff - (SYSTICK->VALUE & 0xffffff);
  23 }
  24
  25 /* cortex-m3 pushes following on stack when entering exception: xPSR, PC, LR, r12, r3, r2, r1, r0
  26  * what's remaining/needs to be saved is: PSP, r4-r11
  27  */
  28 void arch_task_new(struct task *task, void (*func)(u32 arg), u32 arg)
  29 {
  30         /* saved by exception */
  31         task->stack[task->stack_len-1] = 1<<24 /* xPSR, thumb bit */;
  32         task->stack[task->stack_len-2] = (u32)func;
  33         task->stack[task->stack_len-3] = 0x01010101*14 /* LR - this could be something nice, so tasks can actually exit */;
  34         task->stack[task->stack_len-4] = 0x01010101*12 /* r12 */;
  35         task->stack[task->stack_len-5] = 0x01010101*3  /* r3 */;
  36         task->stack[task->stack_len-6] = 0x01010101*2  /* r2 */;
  37         task->stack[task->stack_len-7] = 0x01010101*1  /* r1 */;
  38         task->stack[task->stack_len-8] = arg /* r0 */;
  39
  40         /* saved manually */
  41         task->stack[task->stack_len-9] = 0x01010101*7 /* r7 */;
  42         task->stack[task->stack_len-10] = 0x01010101*6 /* r6 */;
  43         task->stack[task->stack_len-11] = 0x01010101*5 /* r5 */;
  44         task->stack[task->stack_len-12] = 0x01010101*4 /* r4 */;
  45         task->stack[task->stack_len-13] = 0x01010101*11 /* r11 */;
  46         task->stack[task->stack_len-14] = 0x01010101*10 /* r10 */;
  47         task->stack[task->stack_len-15] = 0x01010101*9 /* r9 */;
  48         task->stack[task->stack_len-16] = 0x01010101*8 /* r8 */;
  49
  50         task->context.psp = (u32)&task->stack[task->stack_len-16];
  51 }
  52
  53 void __naked arch_task_first(struct task *task)
  54 {
  55         current = task;
  56         asm volatile (
  57                         /* restore created context */
  58                         "mov    sp, %0\n\t"
  59
  60                         "pop    {r4-r7}\n"
  61                         "mov    r8, r4\n"
  62                         "mov    r9, r5\n"
  63                         "mov    r10, r6\n"
  64                         "mov    r11, r7\n"
  65                         "pop    {r4-r7}\n"
  66
  67                         "pop    {r0-r3}\n"
  68                         "pop    {r2,r3}\n"
  69                         "mov    r12, r2\n"
  70                         "mov    lr, r3\n"
  71
  72                         /* r2, r3 are scrubbed, that means arguments 2, 3 */
  73
  74                         //"add  sp, sp, #8\n\t"    /* pc and xPSR space, just ignore it */
  75
  76 //                      "ldr    pc, [sp, #-8]\n\t" /* and jump to the task entry, sp here is top of stack, so -8 is entry -2 = func */
  77                         //"ldr  r3, [sp, #-8]\n\t" /* and jump to the task entry, sp here is top of stack, so -8 is entry -2 = func */
  78                         "pop    {r2, r3}\n" /* r2 (pc), r3 (xPSR) */
  79                         "mov    pc, r2\n"
  80                         : : "r" (task->context.psp)
  81         );
  82 }
  83
  84 /* task to switch to */
  85 static struct task *new_task;
  86
  87 /* cortex-m3 trm 5.11 Setting up multiple stacks
  88  * does the task switching from current to new_task */
  89 void __naked pendsv_handler()
  90 {
  91
  92         // XXX idea, also for m3, could just switch stacks, and use push/pop
  93         asm volatile (
  94                         "mrs    r3, PSP\n\t"
  95
  96                         /* first push r4-r7, then r8-r11 */
  97                         /* emulate stmdb, an alternative would be to abuse MSP */
  98                         "sub    r3, #0x10\n"
  99                         "stmia  r3!, {r4-r7}\n"
 100                         "sub    r3, #0x20\n"
 101                         "mov    r4, r8\n"
 102                         "mov    r5, r9\n"
 103                         "mov    r6, r10\n"
 104                         "mov    r7, r11\n"
 105                         "stmia  r3!, {r4-r7}\n"
 106                         "sub    r3, #0x10\n"
 107
 108                         /* save PSP */
 109                         "ldr    r0, =current\n\t"
 110                         "ldr    r1, [r0]\n\t"
 111                         "str    r3, [r1, %0]\n\t"  /* current->context.psp = PSP */
 112
 113                         /* get new PSP */
 114                         "ldr    r2, =new_task\n\t"
 115                         "ldr    r2, [r2]\n\t"
 116                         "ldr    r3, [r2, %0]\n\t"  /* PSP = new_task->context.psp */
 117
 118                         /* switch tasks */
 119                         "str    r2, [r0]\n\t"      /* current = new_task */
 120
 121                         /* restore saved regs */
 122                         "ldmia  r3!, {r4-r7}\n"
 123                         "mov    r8, r4\n"
 124                         "mov    r9, r5\n"
 125                         "mov    r10, r6\n"
 126                         "mov    r11, r7\n"
 127                         "ldmia  r3!, {r4-r7}\n"
 128
 129                         "msr    PSP, r3\n\t"
 130
 131                         "bx     lr"
 132                         : : "i" (offsetof(struct task, context))
 133         );
 134 }
 135
 136 void arch_task_switch(struct task *newt)
 137 {
 138         new_task = newt;
 139         /* request PendSV exception */
 140         ICSR = ICSR_PENDSVSET;
 141 }
 142
 143 void arch_sched_start_timer()
 144 {
 145         /* core clock, enable interrupt, enable systick */
 146         SYSTICK->RELOAD = 0;
 147         SYSTICK->VALUE = 0;
 148         SYSTICK->CTRL |= 1<<2 | 1<<1 | 1<<0;
 149
 150         /* set SysTick pending to get scheduled asap */
 151         ICSR = ICSR_PENDSTSET;
 152 }
 153
 154 void arch_wait_for_interrupt()
 155 {
 156         asm volatile ("wfi");
 157 }
 158
 159 static int systick_whole;
 160 static int systick_remainder;
 161
 162 /* writing anything to SYSTICK->VALUE clears it (and doesn't cause the interrupt), but the reloads it with RELOAD on next timer tick */
 163 void arch_sched_next_interrupt(int offset)
 164 {
 165         systick_whole = offset >> 24;
 166         systick_remainder = offset & 0xffffff;
 167         if (systick_remainder == 0)
 168                 systick_remainder = 1;
 169
 170         if (systick_whole) {
 171                 SYSTICK->RELOAD = 0xffffff;
 172                 SYSTICK->VALUE = 0;
 173         } else {
 174                 SYSTICK->RELOAD = systick_remainder;
 175                 SYSTICK->VALUE = 0; /* clear */
 176         }
 177 }
 178
 179 void systick_handler()
 180 {
 181         current_counter += SYSTICK->RELOAD;
 182
 183         if (systick_whole) {
 184                 /* last part? then load the remainder */
 185                 if (--systick_whole == 0) {
 186                         SYSTICK->RELOAD = systick_remainder;
 187                         SYSTICK->VALUE = 0;
 188                 }
 189                 return;
 190         }
 191         /* disable timer */
 192         SYSTICK->RELOAD = 0;
 193         SYSTICK->VALUE = 0;
 194         sched_interrupt();
 195 }