robinkrens.nl - git repository - cortex-from-scratch/blob - systick.c

   1 /* (CC-BY-NC-SA) ROBIN KRENS - ROBIN @ ROBINKRENS.NL
   2  *
   3  * $LOG$
   4  * 2019/8/14 - ROBIN KRENS
   5  * Initial version
   6  *
   7  * $DESCRIPTION$
   8  * SysTick of Cortex M* MCUs. Have a look at the more complex RTC
   9  * in case more accurate timing is needed.
  10  *
  11  *
  12  * */
  13
  14 #include <stdbool.h>
  15 #include <stddef.h>
  16 #include <stdint.h>
  17
  18 #include <sys/robsys.h>
  19 #include <sys/mmap.h>
  20
  21 #include <lib/regfunc.h>
  22 #include <lib/tinyprintf.h>
  23
  24 #define NUMB_TASKS 3
  25
  26 int count;
  27 int next_task;
  28 int curr_task;
  29 uint32_t currentp;
  30 uint32_t nextp;
  31
  32 //struct proces {
  33 //      int32_t nr;
  34 //      uint32_t *stack_ptr;
  35 //      uint32_t mask;
  36 //};
  37 //
  38 //struct proces p1, p2, p3;
  39 //
  40 //
  41 //      uint32_t oldpsp;
  42 //      uint32_t newpsp;
  43 //
  44 //uint32_t task0_stack[50];
  45 //uint32_t task1_stack[50];
  46 //uint32_t task2_stack[50];
  47 //uint32_t PSP_array[3];
  48
  49 struct interrupt_frame {
  50
  51         uint32_t r0; // N-32
  52         uint32_t r1;
  53         uint32_t r2;
  54         uint32_t r3;
  55         uint32_t r12;
  56         uint32_t lr;
  57         uint32_t pc;
  58         uint32_t psr; // N-4
  59 };
  60
  61 //__attribute__ ((interrupt))
  62 void * systick_handler(/* struct interrupt_frame * frame */) {
  63
  64         //printf("Ticking...\n");
  65         //printf("Current task: %d", curr_task);
  66         //count++;
  67
  68
  69
  70
  71         //switch(curr_task) {
  72         //      case(0): next_task = 1; break;
  73         //      case(1): next_task = 2; break;
  74         //      case(2): next_task = 0; break;
  75         //      default: next_task = 0; break;
  76         //}
  77
  78         //if (curr_task != next_task) {
  79
  80
  81
  82         //}
  83
  84 }
  85
  86
  87 __attribute__ ((naked))
  88         void * pendsv_handler_c(void) {
  89 //
  90 //      asm volatile ("push {r0-r11, lr}");
  91 //      asm volatile ("mrs %0, psp" : "=r" (oldpsp));
  92 //      asm volatile ("push {lr}");
  93 //
  94 //      //asm volatile ("push {lr}");
  95 //
  96 //      //printf("FROM MSP %x", oldpsp);
  97 //      //PSP_array[curr_task] = oldpsp;
  98 //      //curr_task = next_task;
  99 //      //newpsp = PSP_array[next_task];
 100 //
 101 //      asm volatile ("msr psp, %0" : : "r" (newpsp));
 102 //
 103 //      asm volatile("pop {lr}");
 104 //      //asm volatile ("pop {r0-r12}");
 105 //      asm volatile("bx lr"); // return
 106 }
 107
 108 uint32_t set_psp(uint32_t) __attribute__( ( naked ) );
 109 uint32_t set_psp(uint32_t stackie) {
 110
 111         asm volatile ("msr psp, r0" "\n\t"
 112                   "bx lr");
 113 }
 114
 115 void systick_init() {
 116
 117 //      count = 0;
 118 //      curr_task = 0;
 119 //      next_task = 1;
 120
 121         /* Every time the counter counts down to zero
 122          * a systick exception is invoked. Systick has
 123          * exception number 15. in the vector table  */
 124         ivt_set_gate(15, systick_handler, 0);
 125
 126         /* Get calibration and set this to 1 sec
 127          * !Most boards have a 1 ms or 10 ms
 128          * calibration value */
 129         int calib = (*STK_CALIB << 0) * 200;
 130
 131         /* The counter reload registers counts down to zero
 132          * and then it is restores the value */
 133         rwrite(STK_RELOAD, calib);
 134
 135         /* Enable the counter and enable the interrupt
 136          * associated with it */
 137         rsetbit(STK_CTRL, 0);
 138         rsetbit(STK_CTRL, 1);
 139
 140 //      extern void task0(void), task1(void), task2(void);
 141 //
 142 //      int size_stack = sizeof(task0_stack);
 143 //
 144 //      p1.stack_ptr  = ((unsigned int) task0_stack) + size_stack - 0x1C;
 145 //      p1.stack_ptr[6] = (uint32_t) task0;
 146 //      p1.stack_ptr[7] = 0x01000000;
 147 //      p2.stack_ptr  = ((unsigned int) task0_stack) + size_stack - 0x1C;
 148 //      p2.stack_ptr[6] = (uint32_t) task1;
 149 //      p2.stack_ptr[7] = 0x01000000;
 150 //      p3.stack_ptr  = ((unsigned int) task0_stack) + size_stack - 0x1C;
 151 //      p3.stack_ptr[6] = (uint32_t) task2;
 152 //      p3.stack_ptr[7] = 0x01000000;
 153 //
 154 //      set_psp(p1.stack_ptr[-7]);
 155 //
 156 //      for(;;);
 157
 158         /* Initialize processes */
 159         //PSP_array[0] = ((unsigned int) task0_stack) + sizeof(task0_stack) - 32*4;
 160         //HW_ADDR(PSP_array[0] + 0x18) = (unsigned long) task0;
 161         //HW_ADDR(PSP_array[0] + 0x1C) = 0x01000000;
 162         //PSP_array[1] = ((unsigned int) task1_stack) + sizeof(task1_stack) - 32*4;
 163         //HW_ADDR(PSP_array[1] + 0x18) = (unsigned long) task1;
 164         //HW_ADDR(PSP_array[1] + 0x1C) = 0x01000000;
 165         //PSP_array[2] = ((unsigned int) task2_stack) + sizeof(task2_stack) - 32*4;
 166         //HW_ADDR(PSP_array[2] + 0x18) = (unsigned long) task2;
 167         //HW_ADDR(PSP_array[2] + 0x1C) = 0x01000000;
 168
 169         //extern void pendsv_handler;
 170
 171         ivt_set_gate(14, pendsv_handler_c, 0);
 172
 173         //set_psp((PSP_array[curr_task] + 32*4 ));
 174
 175         //int startpsp = PSP_array[curr_task] + 16*4;
 176
 177         //asm volatile ("msr psp, %0" : : "r" (startpsp));
 178
 179
 180         //asm volatile ("mov r0, 0x3" "\n\t"
 181         //"msr control, r0" "\n\t"
 182         //"isb" "\n\t");
 183
 184         //for(;;);
 185
 186         //set current PSP
 187         //printf("0: %x\n", PSP_array[0]);
 188         //printf("1: %x", PSP_array[0]);
 189
 190         //task0();
 191
 192 }