From 6302b453bbe59325671f9113e0bd5be408da08d8 Mon Sep 17 00:00:00 2001 From: Asuki Kono Date: Mon, 14 Nov 2016 22:48:41 +0900 Subject: [PATCH 1/2] Replace tab to 2 spaces --- src/Scheduler.cpp | 262 +++++++++++++++++++++++----------------------- src/Scheduler.h | 14 +-- 2 files changed, 138 insertions(+), 138 deletions(-) diff --git a/src/Scheduler.cpp b/src/Scheduler.cpp index 6d7d676..d398d78 100644 --- a/src/Scheduler.cpp +++ b/src/Scheduler.cpp @@ -18,190 +18,190 @@ extern "C" { -#define NUM_REGS 10 // r4-r11, sp, pc +#define NUM_REGS 10 // r4-r11, sp, pc typedef struct CoopTask { - uint32_t regs[NUM_REGS]; - void* stackPtr; - struct CoopTask* next; - struct CoopTask* prev; + uint32_t regs[NUM_REGS]; + void* stackPtr; + struct CoopTask* next; + struct CoopTask* prev; } CoopTask; static CoopTask *cur = 0; static CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) { - CoopTask* next = cur->next; - - if (taskDied) { - // Halt if last task died. - if (next == cur) - while (1) - ; - - // Delete task - if (cur->stackPtr) - free(cur->stackPtr); - cur->next->prev = cur->prev; - cur->prev->next = cur->next; - free(cur); - } - cur = next; - return next; + CoopTask* next = cur->next; + + if (taskDied) { + // Halt if last task died. + if (next == cur) + while (1) + ; + + // Delete task + if (cur->stackPtr) + free(cur->stackPtr); + cur->next->prev = cur->prev; + cur->prev->next = cur->next; + free(cur); + } + cur = next; + return next; } static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) { - asm ( - "mov r0, r5;" - "blx r4;" - /* schedule. */ - "mov r0, #1;" /* returned from task func: task done */ - "bl coopSchedule;" - /* r0 holds address of next task context */ + asm ( + "mov r0, r5;" + "blx r4;" + /* schedule. */ + "mov r0, #1;" /* returned from task func: task done */ + "bl coopSchedule;" + /* r0 holds address of next task context */ #if defined(ARDUINO_ARCH_SAMD) - /* for cortex m0, ldm and stm are restricted to low registers */ - /* load high registers */ - "add r0, #16;" /* they are 4 words higher in memory */ - "ldmia r0, {r1-r6};" /* load them in low registers first... */ - "mov r8, r1;" /* ...and move them into high registers... */ - "mov r9, r2;" - "mov r10, r3;" - "mov r11, r4;" - "mov r12, r5;" - "mov lr, r6;" - /* load low registers */ - "sub r0, r0, #40;" /* back to begin of context */ - "ldmia r0, {r4-r7};" + /* for cortex m0, ldm and stm are restricted to low registers */ + /* load high registers */ + "add r0, #16;" /* they are 4 words higher in memory */ + "ldmia r0, {r1-r6};" /* load them in low registers first... */ + "mov r8, r1;" /* ...and move them into high registers... */ + "mov r9, r2;" + "mov r10, r3;" + "mov r11, r4;" + "mov r12, r5;" + "mov lr, r6;" + /* load low registers */ + "sub r0, r0, #40;" /* back to begin of context */ + "ldmia r0, {r4-r7};" #else - "ldmia r0, {r4-r12, lr};" + "ldmia r0, {r4-r12, lr};" #endif - /* restore task stack */ - "mov sp, r12;" - /* resume task */ - "bx lr;" - ); + /* restore task stack */ + "mov sp, r12;" + /* resume task */ + "bx lr;" + ); } static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) { - asm ( - "mov r12, sp;" + asm ( + "mov r12, sp;" #if defined(ARDUINO_ARCH_SAMD) - /* store low registers */ - "stmia r0, {r4-r7};" - /* store high registers */ - "mov r1, r8;" /* move them to low registers first. */ - "mov r2, r9;" - "mov r3, r10;" - "mov r4, r11;" - "mov r5, r12;" - "mov r6, lr;" - "stmia r0, {r1-r6};" + /* store low registers */ + "stmia r0, {r4-r7};" + /* store high registers */ + "mov r1, r8;" /* move them to low registers first. */ + "mov r2, r9;" + "mov r3, r10;" + "mov r4, r11;" + "mov r5, r12;" + "mov r6, lr;" + "stmia r0, {r1-r6};" #else - "stmia r0, {r4-r12, lr};" + "stmia r0, {r4-r12, lr};" #endif - /* schedule. */ - "mov r0, #0;" /* previous task did not complete */ - "bl coopSchedule;" - /* r0 holds address of next task context */ + /* schedule. */ + "mov r0, #0;" /* previous task did not complete */ + "bl coopSchedule;" + /* r0 holds address of next task context */ #if defined(ARDUINO_ARCH_SAMD) - /* for cortex m0, ldm and stm are restricted to low registers */ - /* load high registers */ - "add r0, #16;" /* they are 4 words higher in memory */ - "ldmia r0, {r1-r6};" /* load them in low registers first... */ - "mov r8, r1;" /* ...and move them into high registers... */ - "mov r9, r2;" - "mov r10, r3;" - "mov r11, r4;" - "mov r12, r5;" - "mov lr, r6;" - /* load low registers */ - "sub r0, r0, #40;" /* back to begin of context */ - "ldmia r0, {r4-r7};" + /* for cortex m0, ldm and stm are restricted to low registers */ + /* load high registers */ + "add r0, #16;" /* they are 4 words higher in memory */ + "ldmia r0, {r1-r6};" /* load them in low registers first... */ + "mov r8, r1;" /* ...and move them into high registers... */ + "mov r9, r2;" + "mov r10, r3;" + "mov r11, r4;" + "mov r12, r5;" + "mov lr, r6;" + /* load low registers */ + "sub r0, r0, #40;" /* back to begin of context */ + "ldmia r0, {r4-r7};" #else - "ldmia r0, {r4-r12, lr};" + "ldmia r0, {r4-r12, lr};" #endif - /* restore task stack */ - "mov sp, r12;" - /* resume task */ - "bx lr;" - ); + /* restore task stack */ + "mov sp, r12;" + /* resume task */ + "bx lr;" + ); } static int coopInit(void) { - CoopTask* task; + CoopTask* task; - task = reinterpret_cast(malloc(sizeof(CoopTask))); - if (!task) - return 0; - task->next = task; - task->prev = task; - task->stackPtr = 0; - cur = task; + task = reinterpret_cast(malloc(sizeof(CoopTask))); + if (!task) + return 0; + task->next = task; + task->prev = task; + task->stackPtr = 0; + cur = task; - return 1; + return 1; } static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz) { - uint8_t *stack = (uint8_t*)malloc(stackSz); - if (!stack) - return 0; - - CoopTask *task = reinterpret_cast(malloc(sizeof(CoopTask))); - if (!task) { - free(stack); - return 0; - } - task->stackPtr = stack; - task->regs[0] = (uint32_t) taskF; - task->regs[1] = (uint32_t) taskData; - task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7; - task->regs[9] = (uint32_t) & coopTaskStart; - - task->prev = cur; - task->next = cur->next; - cur->next->prev = task; - cur->next = task; - - // These are here so compiler is sure that function is - // referenced in both variants (cancels a warning) - if (stackSz == 0xFFFFFFFF) - coopSchedule(0); - if (stackSz == 0xFFFFFFFE) - coopSchedule(1); - - return 1; + uint8_t *stack = (uint8_t*)malloc(stackSz); + if (!stack) + return 0; + + CoopTask *task = reinterpret_cast(malloc(sizeof(CoopTask))); + if (!task) { + free(stack); + return 0; + } + task->stackPtr = stack; + task->regs[0] = (uint32_t) taskF; + task->regs[1] = (uint32_t) taskData; + task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7; + task->regs[9] = (uint32_t) & coopTaskStart; + + task->prev = cur; + task->next = cur->next; + cur->next->prev = task; + cur->next = task; + + // These are here so compiler is sure that function is + // referenced in both variants (cancels a warning) + if (stackSz == 0xFFFFFFFF) + coopSchedule(0); + if (stackSz == 0xFFFFFFFE) + coopSchedule(1); + + return 1; } void yield(void) { - coopDoYield(cur); + coopDoYield(cur); } }; // extern "C" SchedulerClass::SchedulerClass() { - coopInit(); + coopInit(); } static void startLoopHelper(void *taskData) { - SchedulerTask task = reinterpret_cast(taskData); - while (true) - task(); + SchedulerTask task = reinterpret_cast(taskData); + while (true) + task(); } void SchedulerClass::startLoop(SchedulerTask task, uint32_t stackSize) { - coopSpawn(startLoopHelper, reinterpret_cast(task), stackSize); + coopSpawn(startLoopHelper, reinterpret_cast(task), stackSize); } static void startTaskHelper(void *taskData) { - SchedulerTask task = reinterpret_cast(taskData); - task(); + SchedulerTask task = reinterpret_cast(taskData); + task(); } void SchedulerClass::start(SchedulerTask task, uint32_t stackSize) { - coopSpawn(startTaskHelper, reinterpret_cast(task), stackSize); + coopSpawn(startTaskHelper, reinterpret_cast(task), stackSize); } void SchedulerClass::start(SchedulerParametricTask task, void *taskData, uint32_t stackSize) { - coopSpawn(task, taskData, stackSize); + coopSpawn(task, taskData, stackSize); } SchedulerClass Scheduler; diff --git a/src/Scheduler.h b/src/Scheduler.h index 9a45cbb..f2c51df 100644 --- a/src/Scheduler.h +++ b/src/Scheduler.h @@ -20,18 +20,18 @@ #include extern "C" { - typedef void (*SchedulerTask)(void); - typedef void (*SchedulerParametricTask)(void *); + typedef void (*SchedulerTask)(void); + typedef void (*SchedulerParametricTask)(void *); } class SchedulerClass { public: - SchedulerClass(); - static void startLoop(SchedulerTask task, uint32_t stackSize = 1024); - static void start(SchedulerTask task, uint32_t stackSize = 1024); - static void start(SchedulerParametricTask task, void *data, uint32_t stackSize = 1024); + SchedulerClass(); + static void startLoop(SchedulerTask task, uint32_t stackSize = 1024); + static void start(SchedulerTask task, uint32_t stackSize = 1024); + static void start(SchedulerParametricTask task, void *data, uint32_t stackSize = 1024); - static void yield() { ::yield(); }; + static void yield() { ::yield(); }; }; extern SchedulerClass Scheduler; From cfa226070704e89f6a269b9384572022bbffa967 Mon Sep 17 00:00:00 2001 From: Asuki Kono Date: Tue, 15 Nov 2016 00:03:11 +0900 Subject: [PATCH 2/2] Merged SchedulerAVRARM --- library.properties | 6 +-- src/Scheduler.cpp | 103 +++++++++++++++++++++++++++++++++++++++++++-- src/Scheduler.h | 35 +++++++++++++-- 3 files changed, 134 insertions(+), 10 deletions(-) diff --git a/library.properties b/library.properties index c9ee1d5..6dce358 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=Scheduler -version=0.4.4 +version=0.4.5 author=Arduino maintainer=Arduino -sentence=Allows multiple tasks to run at the same time, without interrupting each other. For Arduino sam and samd architectures only (Due, Zero...). +sentence=Allows multiple tasks to run at the same time, without interrupting each other. Except for atmega256x series (Arduino MEGA etc). paragraph=The Scheduler library enables the Arduino to run multiple functions at the same time. This allows tasks to happen without interrupting each other.
This is a cooperative scheduler in that the CPU switches from one task to another. The library includes methods for passing control between tasks. category=Other url=http://www.arduino.cc/en/Reference/Scheduler -architectures=sam,samd +architectures=sam,samd,avr diff --git a/src/Scheduler.cpp b/src/Scheduler.cpp index d398d78..51335ff 100644 --- a/src/Scheduler.cpp +++ b/src/Scheduler.cpp @@ -18,10 +18,20 @@ extern "C" { -#define NUM_REGS 10 // r4-r11, sp, pc +#if defined(__arm__) +#define NUM_REGS (8+1+1) // r4-r11, sp, pc +#elif defined(__AVR_3_BYTE_PC__) +#error "not compatible with atmega256x" +#else +#define NUM_REGS (8+1+1+1) // r2-r17, sp, pc, r28-r29 +#endif typedef struct CoopTask { +#ifdef __arm__ uint32_t regs[NUM_REGS]; +#else + uint16_t regs[NUM_REGS]; +#endif void* stackPtr; struct CoopTask* next; struct CoopTask* prev; @@ -29,7 +39,7 @@ typedef struct CoopTask { static CoopTask *cur = 0; -static CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) { +static CoopTask* __attribute__((noinline)) __attribute__((used)) coopSchedule(char taskDied) { CoopTask* next = cur->next; if (taskDied) { @@ -46,10 +56,46 @@ static CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) { free(cur); } cur = next; - return next; +#ifdef __AVR__ + asm volatile ("movw r30, %[reg]" : : [reg] "r" (next)); + asm volatile ("ldd r5, Z+3"); + asm volatile ("ldd r6, Z+4"); + asm volatile ("ldd r7, Z+5"); + asm volatile ("ldd r8, Z+6"); + asm volatile ("ldd r9, Z+7"); + asm volatile ("ldd r10, Z+8"); + asm volatile ("ldd r11, Z+9"); + asm volatile ("ldd r12, Z+10"); + asm volatile ("ldd r13, Z+11"); + asm volatile ("ldd r14, Z+12"); + asm volatile ("ldd r15, Z+13"); + asm volatile ("ldd r16, Z+14"); + asm volatile ("ldd r17, Z+15"); + + asm volatile ("ldd r2, Z+16"); // restore SP + asm volatile ("ldd r3, Z+17"); + asm volatile ("in r4, 0x3f"); // save SREG + asm volatile ("cli "); // just to be safe on playing with stack ptr :) (useless with xmega) + asm volatile ("out 0x3e, r3"); // SPH + asm volatile ("out 0x3d, r2"); // SPL + asm volatile ("out 0x3f, r4"); // restore SREG asap (same approach as in setjmp.S credit to Marek Michalkiewicz) + + asm volatile ("ldd r2, Z+0"); + asm volatile ("ldd r3, Z+1"); + asm volatile ("ldd r4, Z+2"); + asm volatile ("ldd r28, Z+20"); // get previous R28,R29 + asm volatile ("ldd r29, Z+21"); + asm volatile ("ldd __tmp_reg__, Z+18"); // return low + asm volatile ("ldd r31, Z+19"); // return hi + asm volatile ("mov r30,__tmp_reg__"); + asm volatile ("ijmp"); // jump back to task return adress +#endif + + return cur; // just to avoid warning ! } static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) { +#ifdef __arm__ asm ( "mov r0, r5;" "blx r4;" @@ -79,9 +125,17 @@ static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) /* resume task */ "bx lr;" ); +#else + asm volatile ("movw r30,r2"); // get original taskF parameter + asm volatile ("movw r24,r4"); // get original taskData parameter + asm volatile ("icall"); // call taskF + asm volatile ("ldi r24,1"); // task died + asm volatile ("rjmp coopSchedule"); // get next task pointer +#endif } static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) { +#ifdef __arm__ asm ( "mov r12, sp;" #if defined(ARDUINO_ARCH_SAMD) @@ -100,6 +154,8 @@ static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTas #endif /* schedule. */ "mov r0, #0;" /* previous task did not complete */ + "stmia r0, {r4-r12, lr};" + "mov r0, #0;" "bl coopSchedule;" /* r0 holds address of next task context */ #if defined(ARDUINO_ARCH_SAMD) @@ -124,6 +180,38 @@ static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTas /* resume task */ "bx lr;" ); +#else + asm volatile ("movw r30, %[cur]" : : [cur] "r" (curTask)); + asm volatile ("std Z+0, r2"); // save register in current cooptask structure + asm volatile ("std Z+1, r3"); + asm volatile ("std Z+2, r4"); + asm volatile ("std Z+3, r5"); + asm volatile ("std Z+4, r6"); + asm volatile ("std Z+5, r7"); + asm volatile ("std Z+6, r8"); + asm volatile ("std Z+7, r9"); + asm volatile ("std Z+8, r10"); + asm volatile ("std Z+9, r11"); + asm volatile ("std Z+10, r12"); + asm volatile ("std Z+11, r13"); + asm volatile ("std Z+12, r14"); + asm volatile ("std Z+13, r15"); + asm volatile ("std Z+14, r16"); + asm volatile ("std Z+15, r17"); + asm volatile ("pop r3"); // return adresse = yield caller /// NOT COMPATIBLE 2560 + asm volatile ("pop r2"); + asm volatile ("std Z+18, r2"); // store return adress + asm volatile ("std Z+19, r3"); + asm volatile ("in r2, 0x3d"); // SPL + asm volatile ("in r3, 0x3e"); // SPH + asm volatile ("std Z+16, r2"); + asm volatile ("std Z+17, r3"); + asm volatile ("std Z+20, r28"); + asm volatile ("std Z+21, r29"); + + asm volatile ("ldi r24, 0"); + asm volatile ("rjmp coopSchedule"); // get next task pointer +#endif } static int coopInit(void) { @@ -153,7 +241,11 @@ static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t sta task->stackPtr = stack; task->regs[0] = (uint32_t) taskF; task->regs[1] = (uint32_t) taskData; +#ifdef __arm__ task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7; +#else + task->regs[8] = ((uint32_t)(stack + stackSz)); +#endif task->regs[9] = (uint32_t) & coopTaskStart; task->prev = cur; @@ -203,6 +295,11 @@ void SchedulerClass::start(SchedulerTask task, uint32_t stackSize) { void SchedulerClass::start(SchedulerParametricTask task, void *taskData, uint32_t stackSize) { coopSpawn(task, taskData, stackSize); } +// added for some level of compatibility with Arduino < 150 +void SchedulerClass::delay(uint32_t ms) { + uint32_t end = millis() + ms; + while (millis() < end) yield(); +} SchedulerClass Scheduler; diff --git a/src/Scheduler.h b/src/Scheduler.h index f2c51df..59e15d3 100644 --- a/src/Scheduler.h +++ b/src/Scheduler.h @@ -22,14 +22,42 @@ extern "C" { typedef void (*SchedulerTask)(void); typedef void (*SchedulerParametricTask)(void *); + +#if ARDUINO < 150 + void yield(void); // define a global yield function +#warning "library calls to Arduino delay doesnt contain yield() and might block scheduler during this delay" +#endif + } +#ifndef yieldPROTECT +#define yieldPROTECT() static uint8_t __yieldProtect = 0; \ +uint8_t* __temp __attribute__((__cleanup__(__yieldUnprotect))) = & __yieldProtect; \ +while (__yieldProtect) yield(); __yieldProtect = 1; +void inline __yieldUnprotect(uint8_t* *__s) { uint8_t* staticFlag = *__s; *staticFlag = 0; }; +#endif + +#ifndef yieldATOMIC +#define yieldATOMIC for ( uint8_t __temp __attribute__((__cleanup__(__decYieldAtomic))) = __incYieldAtomic(); __temp ; __temp = 0 ) +static volatile uint8_t __yieldAtomic = 0; +void inline __decYieldAtomic(const uint8_t *__s) { --__yieldAtomic; } +uint8_t inline __incYieldAtomic(void) { ++__yieldAtomic; return 1; } +#endif + +#ifdef __AVR__ +#define SchedulerDefaultStack 256 // proposed value, might be too low fo complex application or too high if not needed ... to be experimented by user +#else +#define SchedulerDefaultStack 1024 // original value, probably too much for most usage, but lot of memory on SAM +#endif + + class SchedulerClass { public: SchedulerClass(); - static void startLoop(SchedulerTask task, uint32_t stackSize = 1024); - static void start(SchedulerTask task, uint32_t stackSize = 1024); - static void start(SchedulerParametricTask task, void *data, uint32_t stackSize = 1024); + static void startLoop(SchedulerTask task, uint32_t stackSize = SchedulerDefaultStack); + static void start(SchedulerTask task, uint32_t stackSize = SchedulerDefaultStack); + static void start(SchedulerParametricTask task, void *data, uint32_t stackSize = SchedulerDefaultStack); + static void delay(uint32_t ms); static void yield() { ::yield(); }; }; @@ -37,4 +65,3 @@ class SchedulerClass { extern SchedulerClass Scheduler; #endif -