Scheduler.cpp

00001 /*
00002  * Copyright (C) 2012-2013 audin
00003  * This program is licensed under the Apache License, Version 2.0.
00004  * 2013/08: Modify for mbed for LPC1114FN28
00005  * Modified 2012/10: For working on Cortex-M0 and M3.
00006  *                   Defined static tcb option for the cpu that has quite a less SRAM < 8kbyte.  
00007  *
00008  * Original file is arduino-1.5\hardware\arduino\sam\libraries\Scheduler
00009  */
00010 
00011 /*
00012  * Copyright (C) 2012 The Android Open Source Project
00013  *
00014  * Licensed under the Apache License, Version 2.0 (the "License");
00015  * you may not use this file except in compliance with the License.
00016  * You may obtain a copy of the License at
00017  *
00018  *      http://www.apache.org/licenses/LICENSE-2.0
00019  *
00020  * Unless required by applicable law or agreed to in writing, software
00021  * distributed under the License is distributed on an "AS IS" BASIS,
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00023  * See the License for the specific language governing permissions and
00024  * limitations under the License.
00025  */
00026 
00027 #include <stdint.h>
00028 #include "mbed.h"
00029 #include "Scheduler.h"
00030 
00031 #define _ARM_CM0_           1       /* 1: Cortex-M0 or M3,  0: Cortex-M3        */
00032 #define _USE_MALLOC_        0       /* 1: 8kbyte >= SRAM,   0: 8kbyte < SRAM    */
00033 
00034 
00035 #if _USE_MALLOC_
00036 #   include <stdlib.h>
00037 #endif
00038 
00039 #ifdef __cplusplus
00040 extern "C" {
00041 #endif
00042 uint32_t millis(void);
00043 
00044 #define NUM_REGS 10 // r4-r11, sp, lr
00045 
00046 typedef struct CoopTask {
00047     uint32_t regs[NUM_REGS];
00048     void* stackPtr;
00049     struct CoopTask* next;
00050     struct CoopTask* prev;
00051 } CoopTask;
00052 
00053 #if  !_USE_MALLOC_
00054 static uint32_t id = 0;
00055 static CoopTask tcb[ COOP_TASK_NUM_MAX ];
00056 static uint8_t task_stack[ COOP_TASK_NUM_MAX - 1 ][ MIN_STACK_SIZE ];
00057 #endif
00058 
00059 static CoopTask *cur = 0;
00060 
00061 CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) {
00062     CoopTask* next = cur->next;
00063 
00064 #if _USE_MALLOC_
00065     if (taskDied) {
00066         // Halt if last task died.
00067         if (next == cur)
00068             while (1)
00069                 ;
00070 
00071         // Delete task
00072         if (cur->stackPtr)
00073             free(cur->stackPtr);
00074         cur->next->prev = cur->prev;
00075         cur->prev->next = cur->next;
00076         free(cur);
00077     }
00078 #endif
00079     cur = next;
00080     return next;
00081 }
00082 #ifdef __CC_ARM 
00083 __asm static void coopTaskStart(void) {
00084     import coopSchedule
00085 #if _ARM_CM0_
00086     /* for Cortex-m0 */
00087         mov   r0, r5;
00088         blx   r4;
00089         movs   r0, #1;
00090         bl    coopSchedule;
00091     /****   ldmia r0, {r4-r12, lr}; */
00092         adds r0, r0, #16;
00093         ldmia r0!, {r4-r7};     /* get r7->r11, r6->r10, r5->r9, r4->r8     */
00094         mov r11, r7;
00095         mov r10, r6;
00096         mov r9,  r5;
00097         mov r8 , r4;
00098 
00099         ldmia r0!, {r4-r5};     /* get r5->lr, r4->r12                      */
00100         mov lr,  r5;
00101         mov r12, r4;
00102 
00103         subs r0, r0, #40;           /* set offset for r4, 40 = 10reg * 4byte    */
00104         ldmia  r0!, {r4-r7};        /* get r7,r6,r5,r4                          */
00105     /**** end ldmia converted by Cortex-M0 instructions */
00106         msr   msp, r12;         /* use main stack                           */
00107         bx    lr;
00108 #else
00109     /* for Cortex-m3 or ARM code cpu */
00110         mov   r0, r5;                       /* r5 = new task func */
00111         blx   r4;                           /* r4 = helper func   */
00112         movs   r0, #1;                      
00113         bl    coopSchedule;
00114         ldmia r0, {r4-r12, lr};
00115         mov   sp, r12;
00116         bx    lr;
00117 #endif
00118 }
00119 #else
00120 static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) {
00121 #if _ARM_CM0_
00122     /* for Cortex-m0 */
00123     __asm (
00124         "mov   r0, r5;"
00125         "blx   r4;"
00126         "mov   r0, #1;"
00127         "bl    coopSchedule;"
00128     /****   "ldmia r0, {r4-r12, lr};"   */
00129         "add r0, r0, #16;"
00130         "ldmia r0!, {r4-r7};"       /* get r7->r11, r6->r10, r5->r9, r4->r8     */
00131         "mov r11, r7;"
00132         "mov r10, r6;"
00133         "mov r9,  r5;"
00134         "mov r8 , r4;"
00135 
00136         "ldmia r0!, {r4-r5};"       /* get r5->lr, r4->r12                      */
00137         "mov lr,  r5;"
00138         "mov r12, r4;"
00139 
00140         "sub r0, r0, #40;"          /* set offset for r4, 40 = 10reg * 4byte    */
00141         "ldmia  r0!, {r4-r7};"      /* get r7,r6,r5,r4                          */
00142     /**** end ldmia converted by Cortex-M0 instructions */
00143         "msr   msp, r12;"           /* use main stack                           */
00144         "bx    lr;"
00145     );
00146 #else
00147     /* for Cortex-m3 or ARM code cpu */
00148     asm (
00149         "mov   r0, r5;"                     /* r5 = new task func */
00150         "blx   r4;"                         /* r4 = helper func   */
00151         "mov   r0, #1;"                     
00152         "bl    coopSchedule;"
00153         "ldmia r0, {r4-r12, lr};"
00154         "mov   sp, r12;"
00155         "bx    lr;"
00156     );
00157 #endif
00158 }
00159 #endif
00160 
00161 #ifdef __CC_ARM
00162 __asm static void coopDoYield(CoopTask* curTask) {
00163 #if _ARM_CM0_
00164     /* for Cortex-m0 */
00165             mrs   r12, msp;
00166         /**** stmia r0, {r4-r12, lr}; */
00167             stmia r0!, {r4-r7};     /* first store r4-r7 data               */
00168 
00169             mov r4, r8;
00170             mov r5, r9;
00171             mov r6, r10;
00172             mov r7, r11;
00173             stmia r0!, {r4-r7};     /* store r8-r11                         */
00174 
00175             mov r4, r12;
00176             mov r5, lr;
00177             stmia r0!, {r4,r5};     /* store r12, lr                        */
00178         /**** end stmia converted by cortex-m0 instructions */
00179 
00180             movs   r0, #0;
00181             bl    coopSchedule;
00182 
00183         /**** ldmia r0, {r4-r12, lr};   */
00184             adds r0, r0, #16;           /* set offset for r8                    */
00185             ldmia r0!, {r4-r7};     /* get r7->r11, r6->r10, r5->r9, r4->r8 */
00186             mov r11, r7;
00187             mov r10, r6;
00188             mov r9,  r5;
00189             mov r8 , r4;
00190 
00191             ldmia r0!, {r4-r5};     /* get r5->lr, r4->r12                  */
00192             mov lr,  r5;
00193             mov r12, r4;
00194 
00195             subs r0, r0, #40;           /* set offset for r4, 40 = 10reg * 4byte  */
00196             ldmia  r0!, {r4-r7};
00197         /**** end ldmia converted by Cortex-M0 instructions */
00198 
00199             msr   msp, r12;
00200             bx    lr;
00201 #else
00202     /* for Cortex-m3 or ARM code cpu */
00203             mov   r12, sp;
00204             stmia r0, {r4-r12, lr};
00205             movs   r0, #0;
00206             bl    coopSchedule;
00207             ldmia r0, {r4-r12, lr};
00208             mov   sp, r12;
00209             bx    lr;
00210 #endif
00211 }
00212 
00213 #else
00214 static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) {
00215 #if _ARM_CM0_
00216     /* for Cortex-m0 */
00217     __asm (
00218             "mrs   r12, msp;"
00219         /**** "stmia r0, {r4-r12, lr};" */
00220             "stmia r0!, {r4-r7};"       /* first store r4-r7 data               */
00221 
00222             "mov r4, r8;"
00223             "mov r5, r9;"
00224             "mov r6, r10;"
00225             "mov r7, r11;"
00226             "stmia r0!, {r4-r7};"       /* store r8-r11                         */
00227 
00228             "mov r4, r12;"
00229             "mov r5, lr;"
00230             "stmia r0!, {r4,r5};"       /* store r12, lr                        */
00231         /**** end stmia converted by cortex-m0 instructions */
00232 
00233             "mov   r0, #0;"
00234             "bl    coopSchedule;"
00235 
00236         /**** "ldmia r0, {r4-r12, lr};" */
00237             "add r0, r0, #16;"          /* set offset for r8                    */
00238             "ldmia r0!, {r4-r7};"       /* get r7->r11, r6->r10, r5->r9, r4->r8 */
00239             "mov r11, r7;"
00240             "mov r10, r6;"
00241             "mov r9,  r5;"
00242             "mov r8 , r4;"
00243 
00244             "ldmia r0!, {r4-r5};"       /* get r5->lr, r4->r12                  */
00245             "mov lr,  r5;"
00246             "mov r12, r4;"
00247 
00248             "sub r0, r0, #40;"          /* set offset for r4, 40 = 10reg * 4byte  */
00249             "ldmia  r0!, {r4-r7};"
00250         /**** end ldmia converted by Cortex-M0 instructions */
00251 
00252             "msr   msp, r12;"
00253             "bx    lr;"
00254         );
00255 #else
00256     /* for Cortex-m3 or ARM code cpu */
00257     __asm (
00258             "mov   r12, sp;"
00259             "stmia r0, {r4-r12, lr};"
00260             "mov   r0, #0;"
00261             "bl    coopSchedule;"
00262             "ldmia r0, {r4-r12, lr};"
00263             "mov   sp, r12;"
00264             "bx    lr;"
00265         );
00266 #endif
00267 
00268 }
00269 #endif
00270 static int coopInit(void) {
00271     CoopTask* task;
00272 #if _USE_MALLOC_
00273 #   ifdef __cplusplus
00274         task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
00275 #   else
00276         task = (CoopTask *)(malloc(sizeof(CoopTask)));
00277 #   endif
00278     if (!task)
00279         return 0;
00280 #else
00281     task = &tcb[ id ];
00282 #endif
00283     task->next = task;
00284     task->prev = task;
00285     task->stackPtr = 0;
00286     cur = task;
00287 
00288     return 1;
00289 }
00290 
00291 static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz) {
00292 #if _USE_MALLOC_
00293     uint8_t *stack = (uint8_t*)malloc(stackSz);
00294     if (!stack)
00295         return 0;
00296 #   ifdef __cplusplus
00297         CoopTask *task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
00298 #   else
00299         CoopTask *task = (CoopTask *)(malloc(sizeof(CoopTask)));
00300 #   endif
00301     if (!task) {
00302         free(stack);
00303         return 0;
00304     }
00305 #else
00306     uint8_t *stack = task_stack[ id ];
00307     id++;
00308     CoopTask *task = &tcb[ id ];
00309 #endif
00310     task->stackPtr = stack;
00311     task->regs[0] = (uint32_t) taskF;                           /* Helper func: r4  */
00312     task->regs[1] = (uint32_t) taskData;                        /* New task :   r5  */
00313 #if _USE_MALLOC_
00314     task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7;
00315 #else
00316     task->regs[8] = ((uint32_t)(stack + MIN_STACK_SIZE)) & ~7;      /* r12  */
00317 #endif
00318     task->regs[9] = (uint32_t) & coopTaskStart;                 /* lr   */
00319 
00320     task->prev = cur;
00321     task->next = cur->next;
00322     cur->next->prev = task;
00323     cur->next = task;
00324 
00325     // These are here so compiler is sure that function is
00326     // referenced in both variants (cancels a warning)
00327     if (stackSz == 0xFFFFFFFF)
00328         coopSchedule(0);
00329     if (stackSz == 0xFFFFFFFE)
00330         coopSchedule(1);
00331 
00332     return 1;
00333 }
00334 
00335 void yield(void) {
00336     coopDoYield(cur);
00337 }
00338 
00339 #ifdef MBED_H
00340 void taskWait(uint32_t ms) {
00341     uint32_t start = us_ticker_read();
00342     while ((us_ticker_read() - start) < (uint32_t)(ms*1000)){
00343         yield();
00344     }
00345 }
00346 #else
00347 void wait(uint32_t ms) {
00348     uint32_t start = millis();
00349     while (millis() - start < ms){
00350         yield();
00351     }
00352 }
00353 #endif
00354 
00355 #ifdef __cplusplus
00356 }; // extern "C"
00357 #endif
00358 
00359 void scheduler_init( void ) {
00360     coopInit();
00361 }
00362 
00363 static void startLoopHelper(void *taskData) {
00364 #ifdef __cplusplus
00365     SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
00366 #else
00367     SchedulerTask task = (SchedulerTask)(taskData);
00368 #endif
00369     while (1){
00370         task();
00371     }
00372 }
00373 
00374 static void startTaskHelper(void *taskData) {
00375 #ifdef __cplusplus
00376     SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
00377 #else
00378     SchedulerTask task = (SchedulerTask)(taskData);
00379 #endif
00380     task();
00381 }
00382 
00383 void scheduler_start( SchedulerTask task ) {
00384     coopSpawn( startTaskHelper, (void *)(task), MIN_STACK_SIZE );
00385 }
00386 
00387 void scheduler_startLoop( SchedulerTask task) {
00388     coopSpawn( startLoopHelper, (void *)(task), MIN_STACK_SIZE );
00389 }
00390 
00391 #ifdef __cplusplus
00392 SchedulerClass::SchedulerClass() {
00393     coopInit();
00394 }
00395 
00396 void SchedulerClass::startLoop(SchedulerTask task, uint32_t stackSize) {
00397     coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize);
00398 }
00399 
00400 void SchedulerClass::start(SchedulerTask task, uint32_t stackSize) {
00401     coopSpawn(startTaskHelper, reinterpret_cast<void *>(task), stackSize);
00402 }
00403 
00404 void SchedulerClass::start(SchedulerParametricTask task, void *taskData, uint32_t stackSize) {
00405     coopSpawn(task, taskData, stackSize);
00406 }
00407 
00408 SchedulerClass Scheduler;
00409 #endif
00410