mi mi / Scheduler

For Cortex-M3,Cortex-M0, Multitask scheduler library. Arduino due compatible

Dependents:   scheduler-demo-cq-lpc11u35 scheduler-demo scheduler-demo-cq-lpc11u35 mbed-scli-test

Scheduler.cpp@0:c68459544a17, 2013-08-25 (annotated)

Committer:
dinau
Date:
Sun Aug 25 16:48:21 2013 +0900
Revision:
0:c68459544a17
Child:
1:8967b575bb46
for mbed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dinau 0:c68459544a17 1 /*
dinau 0:c68459544a17 2 * Copyright (C) 2012 audin
dinau 0:c68459544a17 3 * This program is licensed under the Apache License, Version 2.0.
dinau 0:c68459544a17 4 * Modified 2012/10: For working on Cortex-M0 and M3.
dinau 0:c68459544a17 5 * Defined static tcb option for the cpu that has quite a less SRAM < 8kbyte.
dinau 0:c68459544a17 6 *
dinau 0:c68459544a17 7 * Original file is arduino-1.5\hardware\arduino\sam\libraries\Scheduler
dinau 0:c68459544a17 8 */
dinau 0:c68459544a17 9
dinau 0:c68459544a17 10 /*
dinau 0:c68459544a17 11 * Copyright (C) 2012 The Android Open Source Project
dinau 0:c68459544a17 12 *
dinau 0:c68459544a17 13 * Licensed under the Apache License, Version 2.0 (the "License");
dinau 0:c68459544a17 14 * you may not use this file except in compliance with the License.
dinau 0:c68459544a17 15 * You may obtain a copy of the License at
dinau 0:c68459544a17 16 *
dinau 0:c68459544a17 17 * http://www.apache.org/licenses/LICENSE-2.0
dinau 0:c68459544a17 18 *
dinau 0:c68459544a17 19 * Unless required by applicable law or agreed to in writing, software
dinau 0:c68459544a17 20 * distributed under the License is distributed on an "AS IS" BASIS,
dinau 0:c68459544a17 21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
dinau 0:c68459544a17 22 * See the License for the specific language governing permissions and
dinau 0:c68459544a17 23 * limitations under the License.
dinau 0:c68459544a17 24 */
dinau 0:c68459544a17 25
dinau 0:c68459544a17 26 #include <stdint.h>
dinau 0:c68459544a17 27 #include "Scheduler.h"
dinau 0:c68459544a17 28
dinau 0:c68459544a17 29 #define _ARM_CM0_ 1 /* 1: Cortex-M0 or M3, 0: Cortex-M3 */
dinau 0:c68459544a17 30 #define _USE_MALLOC_ 0 /* 1: 8kbyte >= SRAM, 0: 8kbyte < SRAM */
dinau 0:c68459544a17 31
dinau 0:c68459544a17 32
dinau 0:c68459544a17 33 #if _USE_MALLOC_
dinau 0:c68459544a17 34 # include <stdlib.h>
dinau 0:c68459544a17 35 #endif
dinau 0:c68459544a17 36
dinau 0:c68459544a17 37 #ifdef __cplusplus
dinau 0:c68459544a17 38 extern "C" {
dinau 0:c68459544a17 39 #endif
dinau 0:c68459544a17 40 uint32_t millis(void);
dinau 0:c68459544a17 41
dinau 0:c68459544a17 42 #define NUM_REGS 10 // r4-r11, sp, lr
dinau 0:c68459544a17 43
dinau 0:c68459544a17 44 typedef struct CoopTask {
dinau 0:c68459544a17 45 uint32_t regs[NUM_REGS];
dinau 0:c68459544a17 46 void* stackPtr;
dinau 0:c68459544a17 47 struct CoopTask* next;
dinau 0:c68459544a17 48 struct CoopTask* prev;
dinau 0:c68459544a17 49 } CoopTask;
dinau 0:c68459544a17 50
dinau 0:c68459544a17 51 #if !_USE_MALLOC_
dinau 0:c68459544a17 52 static uint32_t id = 0;
dinau 0:c68459544a17 53 static CoopTask tcb[ COOP_TASK_NUM_MAX ];
dinau 0:c68459544a17 54 static uint8_t task_stack[ COOP_TASK_NUM_MAX - 1 ][ MIN_STACK_SIZE ];
dinau 0:c68459544a17 55 #endif
dinau 0:c68459544a17 56
dinau 0:c68459544a17 57 static CoopTask *cur = 0;
dinau 0:c68459544a17 58
dinau 0:c68459544a17 59 CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) {
dinau 0:c68459544a17 60 CoopTask* next = cur->next;
dinau 0:c68459544a17 61
dinau 0:c68459544a17 62 #if _USE_MALLOC_
dinau 0:c68459544a17 63 if (taskDied) {
dinau 0:c68459544a17 64 // Halt if last task died.
dinau 0:c68459544a17 65 if (next == cur)
dinau 0:c68459544a17 66 while (1)
dinau 0:c68459544a17 67 ;
dinau 0:c68459544a17 68
dinau 0:c68459544a17 69 // Delete task
dinau 0:c68459544a17 70 if (cur->stackPtr)
dinau 0:c68459544a17 71 free(cur->stackPtr);
dinau 0:c68459544a17 72 cur->next->prev = cur->prev;
dinau 0:c68459544a17 73 cur->prev->next = cur->next;
dinau 0:c68459544a17 74 free(cur);
dinau 0:c68459544a17 75 }
dinau 0:c68459544a17 76 #endif
dinau 0:c68459544a17 77 cur = next;
dinau 0:c68459544a17 78 return next;
dinau 0:c68459544a17 79 }
dinau 0:c68459544a17 80
dinau 0:c68459544a17 81 static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) {
dinau 0:c68459544a17 82 #if _ARM_CM0_
dinau 0:c68459544a17 83 /* for Cortex-m0 */
dinau 0:c68459544a17 84 __asm (
dinau 0:c68459544a17 85 "mov r0, r5;"
dinau 0:c68459544a17 86 "blx r4;"
dinau 0:c68459544a17 87 "mov r0, #1;"
dinau 0:c68459544a17 88 "bl coopSchedule;"
dinau 0:c68459544a17 89 /**** "ldmia r0, {r4-r12, lr};" */
dinau 0:c68459544a17 90 "add r0, r0, #16;"
dinau 0:c68459544a17 91 "ldmia r0!, {r4-r7};" /* get r7->r11, r6->r10, r5->r9, r4->r8 */
dinau 0:c68459544a17 92 "mov r11, r7;"
dinau 0:c68459544a17 93 "mov r10, r6;"
dinau 0:c68459544a17 94 "mov r9, r5;"
dinau 0:c68459544a17 95 "mov r8 , r4;"
dinau 0:c68459544a17 96
dinau 0:c68459544a17 97 "ldmia r0!, {r4-r5};" /* get r5->lr, r4->r12 */
dinau 0:c68459544a17 98 "mov lr, r5;"
dinau 0:c68459544a17 99 "mov r12, r4;"
dinau 0:c68459544a17 100
dinau 0:c68459544a17 101 "sub r0, r0, #40;" /* set offset for r4, 40 = 10reg * 4byte */
dinau 0:c68459544a17 102 "ldmia r0!, {r4-r7};" /* get r7,r6,r5,r4 */
dinau 0:c68459544a17 103 /**** end ldmia converted by Cortex-M0 instructions */
dinau 0:c68459544a17 104 "msr msp, r12;" /* use main stack */
dinau 0:c68459544a17 105 "bx lr;"
dinau 0:c68459544a17 106 );
dinau 0:c68459544a17 107 #else
dinau 0:c68459544a17 108 /* for Cortex-m3 or ARM code cpu */
dinau 0:c68459544a17 109 asm (
dinau 0:c68459544a17 110 "mov r0, r5;" /* r5 = new task func */
dinau 0:c68459544a17 111 "blx r4;" /* r4 = helper func */
dinau 0:c68459544a17 112 "mov r0, #1;"
dinau 0:c68459544a17 113 "bl coopSchedule;"
dinau 0:c68459544a17 114 "ldmia r0, {r4-r12, lr};"
dinau 0:c68459544a17 115 "mov sp, r12;"
dinau 0:c68459544a17 116 "bx lr;"
dinau 0:c68459544a17 117 );
dinau 0:c68459544a17 118 #endif
dinau 0:c68459544a17 119 }
dinau 0:c68459544a17 120
dinau 0:c68459544a17 121 static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) {
dinau 0:c68459544a17 122 #if _ARM_CM0_
dinau 0:c68459544a17 123 /* for Cortex-m0 */
dinau 0:c68459544a17 124 __asm (
dinau 0:c68459544a17 125 "mrs r12, msp;"
dinau 0:c68459544a17 126 /**** "stmia r0, {r4-r12, lr};" */
dinau 0:c68459544a17 127 "stmia r0!, {r4-r7};" /* first store r4-r7 data */
dinau 0:c68459544a17 128
dinau 0:c68459544a17 129 "mov r4, r8;"
dinau 0:c68459544a17 130 "mov r5, r9;"
dinau 0:c68459544a17 131 "mov r6, r10;"
dinau 0:c68459544a17 132 "mov r7, r11;"
dinau 0:c68459544a17 133 "stmia r0!, {r4-r7};" /* store r8-r11 */
dinau 0:c68459544a17 134
dinau 0:c68459544a17 135 "mov r4, r12;"
dinau 0:c68459544a17 136 "mov r5, lr;"
dinau 0:c68459544a17 137 "stmia r0!, {r4,r5};" /* store r12, lr */
dinau 0:c68459544a17 138 /**** end stmia converted by cortex-m0 instructions */
dinau 0:c68459544a17 139
dinau 0:c68459544a17 140 "mov r0, #0;"
dinau 0:c68459544a17 141 "bl coopSchedule;"
dinau 0:c68459544a17 142
dinau 0:c68459544a17 143 /**** "ldmia r0, {r4-r12, lr};" */
dinau 0:c68459544a17 144 "add r0, r0, #16;" /* set offset for r8 */
dinau 0:c68459544a17 145 "ldmia r0!, {r4-r7};" /* get r7->r11, r6->r10, r5->r9, r4->r8 */
dinau 0:c68459544a17 146 "mov r11, r7;"
dinau 0:c68459544a17 147 "mov r10, r6;"
dinau 0:c68459544a17 148 "mov r9, r5;"
dinau 0:c68459544a17 149 "mov r8 , r4;"
dinau 0:c68459544a17 150
dinau 0:c68459544a17 151 "ldmia r0!, {r4-r5};" /* get r5->lr, r4->r12 */
dinau 0:c68459544a17 152 "mov lr, r5;"
dinau 0:c68459544a17 153 "mov r12, r4;"
dinau 0:c68459544a17 154
dinau 0:c68459544a17 155 "sub r0, r0, #40;" /* set offset for r4, 40 = 10reg * 4byte */
dinau 0:c68459544a17 156 "ldmia r0!, {r4-r7};"
dinau 0:c68459544a17 157 /**** end ldmia converted by Cortex-M0 instructions */
dinau 0:c68459544a17 158
dinau 0:c68459544a17 159 "msr msp, r12;"
dinau 0:c68459544a17 160 "bx lr;"
dinau 0:c68459544a17 161 );
dinau 0:c68459544a17 162 #else
dinau 0:c68459544a17 163 /* for Cortex-m3 or ARM code cpu */
dinau 0:c68459544a17 164 __asm (
dinau 0:c68459544a17 165 "mov r12, sp;"
dinau 0:c68459544a17 166 "stmia r0, {r4-r12, lr};"
dinau 0:c68459544a17 167 "mov r0, #0;"
dinau 0:c68459544a17 168 "bl coopSchedule;"
dinau 0:c68459544a17 169 "ldmia r0, {r4-r12, lr};"
dinau 0:c68459544a17 170 "mov sp, r12;"
dinau 0:c68459544a17 171 "bx lr;"
dinau 0:c68459544a17 172 );
dinau 0:c68459544a17 173 #endif
dinau 0:c68459544a17 174
dinau 0:c68459544a17 175 }
dinau 0:c68459544a17 176
dinau 0:c68459544a17 177 static int coopInit(void) {
dinau 0:c68459544a17 178 CoopTask* task;
dinau 0:c68459544a17 179 #if _USE_MALLOC_
dinau 0:c68459544a17 180 # ifdef __cplusplus
dinau 0:c68459544a17 181 task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
dinau 0:c68459544a17 182 # else
dinau 0:c68459544a17 183 task = (CoopTask *)(malloc(sizeof(CoopTask)));
dinau 0:c68459544a17 184 # endif
dinau 0:c68459544a17 185 if (!task)
dinau 0:c68459544a17 186 return 0;
dinau 0:c68459544a17 187 #else
dinau 0:c68459544a17 188 task = &tcb[ id ];
dinau 0:c68459544a17 189 #endif
dinau 0:c68459544a17 190 task->next = task;
dinau 0:c68459544a17 191 task->prev = task;
dinau 0:c68459544a17 192 task->stackPtr = 0;
dinau 0:c68459544a17 193 cur = task;
dinau 0:c68459544a17 194
dinau 0:c68459544a17 195 return 1;
dinau 0:c68459544a17 196 }
dinau 0:c68459544a17 197
dinau 0:c68459544a17 198 static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz) {
dinau 0:c68459544a17 199 #if _USE_MALLOC_
dinau 0:c68459544a17 200 uint8_t *stack = (uint8_t*)malloc(stackSz);
dinau 0:c68459544a17 201 if (!stack)
dinau 0:c68459544a17 202 return 0;
dinau 0:c68459544a17 203 # ifdef __cplusplus
dinau 0:c68459544a17 204 CoopTask *task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
dinau 0:c68459544a17 205 # else
dinau 0:c68459544a17 206 CoopTask *task = (CoopTask *)(malloc(sizeof(CoopTask)));
dinau 0:c68459544a17 207 # endif
dinau 0:c68459544a17 208 if (!task) {
dinau 0:c68459544a17 209 free(stack);
dinau 0:c68459544a17 210 return 0;
dinau 0:c68459544a17 211 }
dinau 0:c68459544a17 212 #else
dinau 0:c68459544a17 213 uint8_t *stack = task_stack[ id ];
dinau 0:c68459544a17 214 id++;
dinau 0:c68459544a17 215 CoopTask *task = &tcb[ id ];
dinau 0:c68459544a17 216 #endif
dinau 0:c68459544a17 217 task->stackPtr = stack;
dinau 0:c68459544a17 218 task->regs[0] = (uint32_t) taskF; /* Helper func: r4 */
dinau 0:c68459544a17 219 task->regs[1] = (uint32_t) taskData; /* New task : r5 */
dinau 0:c68459544a17 220 #if _USE_MALLOC_
dinau 0:c68459544a17 221 task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7;
dinau 0:c68459544a17 222 #else
dinau 0:c68459544a17 223 task->regs[8] = ((uint32_t)(stack + MIN_STACK_SIZE)) & ~7; /* r12 */
dinau 0:c68459544a17 224 #endif
dinau 0:c68459544a17 225 task->regs[9] = (uint32_t) & coopTaskStart; /* lr */
dinau 0:c68459544a17 226
dinau 0:c68459544a17 227 task->prev = cur;
dinau 0:c68459544a17 228 task->next = cur->next;
dinau 0:c68459544a17 229 cur->next->prev = task;
dinau 0:c68459544a17 230 cur->next = task;
dinau 0:c68459544a17 231
dinau 0:c68459544a17 232 // These are here so compiler is sure that function is
dinau 0:c68459544a17 233 // referenced in both variants (cancels a warning)
dinau 0:c68459544a17 234 if (stackSz == 0xFFFFFFFF)
dinau 0:c68459544a17 235 coopSchedule(0);
dinau 0:c68459544a17 236 if (stackSz == 0xFFFFFFFE)
dinau 0:c68459544a17 237 coopSchedule(1);
dinau 0:c68459544a17 238
dinau 0:c68459544a17 239 return 1;
dinau 0:c68459544a17 240 }
dinau 0:c68459544a17 241
dinau 0:c68459544a17 242 void yield(void) {
dinau 0:c68459544a17 243 coopDoYield(cur);
dinau 0:c68459544a17 244 }
dinau 0:c68459544a17 245
dinau 0:c68459544a17 246 void wait(uint32_t ms) {
dinau 0:c68459544a17 247 uint32_t start = millis();
dinau 0:c68459544a17 248 while (millis() - start < ms)
dinau 0:c68459544a17 249 yield();
dinau 0:c68459544a17 250 }
dinau 0:c68459544a17 251
dinau 0:c68459544a17 252 #ifdef __cplusplus
dinau 0:c68459544a17 253 }; // extern "C"
dinau 0:c68459544a17 254 #endif
dinau 0:c68459544a17 255
dinau 0:c68459544a17 256 void scheduler_init( void ) {
dinau 0:c68459544a17 257 coopInit();
dinau 0:c68459544a17 258 }
dinau 0:c68459544a17 259
dinau 0:c68459544a17 260 static void startLoopHelper(void *taskData) {
dinau 0:c68459544a17 261 #ifdef __cplusplus
dinau 0:c68459544a17 262 SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
dinau 0:c68459544a17 263 #else
dinau 0:c68459544a17 264 SchedulerTask task = (SchedulerTask)(taskData);
dinau 0:c68459544a17 265 #endif
dinau 0:c68459544a17 266 while (1){
dinau 0:c68459544a17 267 task();
dinau 0:c68459544a17 268 }
dinau 0:c68459544a17 269 }
dinau 0:c68459544a17 270
dinau 0:c68459544a17 271 static void startTaskHelper(void *taskData) {
dinau 0:c68459544a17 272 #ifdef __cplusplus
dinau 0:c68459544a17 273 SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
dinau 0:c68459544a17 274 #else
dinau 0:c68459544a17 275 SchedulerTask task = (SchedulerTask)(taskData);
dinau 0:c68459544a17 276 #endif
dinau 0:c68459544a17 277 task();
dinau 0:c68459544a17 278 }
dinau 0:c68459544a17 279
dinau 0:c68459544a17 280 void scheduler_start( SchedulerTask task ) {
dinau 0:c68459544a17 281 coopSpawn( startTaskHelper, (void *)(task), MIN_STACK_SIZE );
dinau 0:c68459544a17 282 }
dinau 0:c68459544a17 283
dinau 0:c68459544a17 284 void scheduler_startLoop( SchedulerTask task) {
dinau 0:c68459544a17 285 coopSpawn( startLoopHelper, (void *)(task), MIN_STACK_SIZE );
dinau 0:c68459544a17 286 }
dinau 0:c68459544a17 287
dinau 0:c68459544a17 288 #ifdef __cplusplus
dinau 0:c68459544a17 289 SchedulerClass::SchedulerClass() {
dinau 0:c68459544a17 290 coopInit();
dinau 0:c68459544a17 291 }
dinau 0:c68459544a17 292
dinau 0:c68459544a17 293 void SchedulerClass::startLoop(SchedulerTask task, uint32_t stackSize) {
dinau 0:c68459544a17 294 coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize);
dinau 0:c68459544a17 295 }
dinau 0:c68459544a17 296
dinau 0:c68459544a17 297 void SchedulerClass::start(SchedulerTask task, uint32_t stackSize) {
dinau 0:c68459544a17 298 coopSpawn(startTaskHelper, reinterpret_cast<void *>(task), stackSize);
dinau 0:c68459544a17 299 }
dinau 0:c68459544a17 300
dinau 0:c68459544a17 301 void SchedulerClass::start(SchedulerParametricTask task, void *taskData, uint32_t stackSize) {
dinau 0:c68459544a17 302 coopSpawn(task, taskData, stackSize);
dinau 0:c68459544a17 303 }
dinau 0:c68459544a17 304
dinau 0:c68459544a17 305 SchedulerClass Scheduler;
dinau 0:c68459544a17 306 #endif
dinau 0:c68459544a17 307