daily pastebin goal
43%
SHARE
TWEET

Untitled

a guest Mar 26th, 2019 64 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <stdint.h>
  2. #include "os.h"
  3. #include "msp.h"
  4.  
  5. Task * volatile current_task;
  6. Task * volatile next_task;
  7.  
  8. void OS_init(void) {
  9.     *(uint32_t volatile *)0xE000ED20 |= (0xFFU << 16); // Set PendSV priority
  10. }
  11.  
  12. void OS_sched(void) {
  13.     if (next_task != current_task) {
  14.         *(uint32_t volatile *)0xE000ED04 |= (1U << 28); // PendSV interrupt
  15.     }
  16. }
  17.  
  18. void Task_start(Task *t, TaskHandler th, void *stack, uint32_t stackSz) {
  19.     uint32_t *sp = (uint32_t *)((((uint32_t)stack + stackSz) / 8) * 8);
  20.     uint32_t *stack_limit;
  21.  
  22.     *(--sp) = (1U << 24);  /* xPSR */
  23.     *(--sp) = (uint32_t)th; /* PC */
  24.     *(--sp) = 0x0000000EU; /* LR  */
  25.     *(--sp) = 0x0000000CU; /* R12 */
  26.     *(--sp) = 0x00000003U; /* R3  */
  27.     *(--sp) = 0x00000002U; /* R2  */
  28.     *(--sp) = 0x00000001U; /* R1  */
  29.     *(--sp) = 0x00000000U; /* R0  */
  30.     /* additionally, fake registers R4-R11 */
  31.     *(--sp) = 0x0000000BU; /* R11 */
  32.     *(--sp) = 0x0000000AU; /* R10 */
  33.     *(--sp) = 0x00000009U; /* R9 */
  34.     *(--sp) = 0x00000008U; /* R8 */
  35.     *(--sp) = 0x00000007U; /* R7 */
  36.     *(--sp) = 0x00000006U; /* R6 */
  37.     *(--sp) = 0x00000005U; /* R5 */
  38.     *(--sp) = 0x00000004U; /* R4 */
  39.  
  40.     t->sp = sp;
  41.     stack_limit = (uint32_t *)(((((uint32_t)stack - 1U) / 8) + 1U) * 8);
  42.  
  43.     for (sp = sp - 1U; sp >= stack_limit; --sp) {
  44.         *sp = 0xCDCDCDCD;
  45.     }
  46. }
  47.  
  48. void PendSV_Handler(void) {
  49. //  __asm(
  50. //      "  IMPORT current_task\n"
  51. //      "  IMPORT next_task\n"
  52. //  //__disable_irq();
  53. //      "  cpsid    i\n"
  54. //  /* if (OS_curr != (OSThread *)0) { */
  55. //      "  LDR  r1,=current_task\n"
  56. //      "  LDR  r1,r1\n"
  57. //      "  CBZ  r1,PendSV_restore\n"
  58. //  /*     push registers r4-r11 on the stack */
  59. //      "  PUSH     {r4-r11}\n"
  60. //  /*     OS_curr->sp = sp; */
  61. //      "  LDR  r1,=current_task\n"
  62. //      "  LDR  r1,r1\n"
  63. //      "  STR  sp,r1\n"
  64. //      "  PendSV_restore\n"
  65. //  /* sp = OS_next->sp; */
  66. //      "  LDR  r1,=next_task\n"
  67. //      "  LDR  r1,r1\n"
  68. //      "  LDR  sp,r1\n"
  69. //  /* OS_curr = OS_next; */
  70. //      "  LDR  r1,=next_task\n"
  71. //      "  LDR  r1,r1\n"
  72. //      "  LDR  r2,=current_task\n"
  73. //      "  STR  r1,r1\n"
  74. //  /* pop registers r4-r11 */
  75. //      "  POP  {r4-r11}\n"
  76. //  //__enable_irq();
  77. //      "  cpsie    i\n"
  78. //      "  BX   lr\n"
  79. //  );
  80.  
  81.     __asm(bl         #0x414
  82.     ldr        r0, [pc, #0x28]
  83.     ldr        r0, [r0]
  84.     cbz        r0, #0x546
  85.     push       {r4-r11}
  86.     ldr        r1, [pc, #0x24]
  87.     ldr        r0, [sp]
  88.     ldr        r1, [r1]
  89.     str        r0, [r1]
  90.     $C$L4
  91.     ldr        r0, [pc, #0x18]
  92.     ldr        r0, [r0]
  93.     ldr        r0, [r0]
  94.     str        r0, [sp]
  95.     ldr        r0, [pc, #0x10]
  96.     ldr        r1, [pc, #0x10]
  97.     ldr        r0, [r0]
  98.     str        r0, [r1]
  99.     pop        {r4-r11}
  100.     bl         #0x410
  101.  
  102.     void *sp;
  103.  
  104.     __disable_irq();
  105.     if (current_task != (Task *)0) {
  106.         /* push r4 r11 on stack */
  107.         current_task->sp = sp;
  108.     }
  109.  
  110.     sp = next_task->sp;
  111.     current_task = next_task;
  112.     __enable_irq();
  113. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top