admin/arduino-libs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
arduino-libs/arduino-cli/libraries/SCoop/SCoop.cpp
王立帮 c247dd07a6 初始化提交
2024-07-20 22:09:06 +08:00

970 lines
38 KiB
C++
Raw Permalink Blame History

/*****************************************************************************/
/* SCOOP LIBRARY / AUTHOR FABRICE OUDERT / GNU GPL V3 */
/* https://code.google.com/p/arduino-scoop-cooperative-scheduler-arm-avr/ */
/* VERSION 1.2 NEW YEAR PACK 10/1/2013 */
/* ENJOY AND USE AT YOUR OWN RISK :) */
/* SHOULD READ USER GUIDE FIRST (@\_/@) */
/*****************************************************************************/
#include "SCoop.h"
#define SCINM SCoopInstanceNickName
/********* GLOBAL VARIABLE *******/
SCoopEvent* SCoopFirstItem = NULL; // has to be initialized here. hold a pointer on the whole list of task/event/timer...
SCoopEvent* SCoopFirstTaskItem = NULL; // has to be initialized here. points to the first of all tasks registered in the list
uint8_t SCoopNumberTask = 0; // hold the number of task registered. used to calculate quantum in start(xxx)
SCoop SCoopInstanceNickName; // then we can use the library in the main sketch directly
#define SCINM SCoopInstanceNickName // just a local nickname...
#if SCoopANDROIDMODE >= 1
SCoop& ArduinoSchedulerNickName = SCINM; // this will create another identifier for the same object instance
#endif
/********* ASSEMBLY / LETS GET STARTED WITH THE COMPLEX THINGS **********/
// original idea for switching stack pointer taken out from ChibiOS.
// Credit to the author. now slightly modified.
// http://forum.pjrc.com/threads/540-ChibiOS-RTand-FreeRTOS-for-Teensy-3-0
//
// original idea for micros() optimization taken from CORE_TEENSY
// credit to Paul http://www.pjrc.com/teensy/
//************************************************************************/
#if defined(SCoop_ARM) && (SCoop_ARM == 1)
static void SCoopSwitch(uint8_t **newSP, uint8_t **oldSP) __attribute__((naked,noinline)) ;
static void SCoopSwitch(uint8_t **newSP, uint8_t **oldSP)
{ asm volatile ("push {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}" : : : "memory");
asm volatile ("str sp, [%[oldsp], #0] \n\t"
"ldr sp, [%[newsp], #0]" : : [newsp] "r" (newSP), [oldsp] "r" (oldSP));
asm volatile ("pop {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, pc}" : : : "memory");
};
static inline uint32_t SCoopGetSP() __attribute__ ((always_inline)) ;
uint32_t SCoopGetSP() { register uint32_t val; asm ("mov %[temp],sp" : [temp] "=r" (val)); return val; }
#define ARM_ATOMIC ASM_ATOMIC
#define AVR_ATOMIC
#define SCoopMicros() ((micros_t)micros()) // overloading the standard micros()
#endif
#if defined(SCoop_AVR) && (SCoop_AVR == 1)
static void SCoopSwitch(void *newSP, void *oldSP) __attribute__((naked,noinline));
static void SCoopSwitch(void *newSP, void *oldSP)
{ asm volatile ("push r2 \n\t push r3 \n\t push r4 \n\t push r5 \n\t push r6 \n\t push r7 \n\t push r8 \n\t push r9 \n\t push r10 \n\t"
"push r11 \n\t push r12 \n\t push r13 \n\t push r14 \n\t push r15 \n\t push r16 \n\t push r17 \n\t push r28 \n\t push r29");
asm volatile ("movw r28, %[oldsp]" : : [oldsp] "r" (oldSP));
asm volatile ("in r2, 0x3d"); // SPL
asm volatile ("in r3, 0x3e"); // SPH
asm volatile ("std Y+0, r2"); // store the current SP into the pointer oldSP
asm volatile ("std Y+1, r3");
asm volatile ("movw r28, %[newsp]" : : [newsp] "r" (newSP));
asm volatile ("ldd r2, Y+0"); // restore the SP from the pointer newSP
asm volatile ("ldd r3, Y+1");
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 ("pop r29 \n\t pop r28 \n\t pop r17 \n\t pop r16 \n\t pop r15 \n\t pop r14 \n\t pop r13 \n\t pop r12 \n\t pop r11 \n\t"
"pop r10 \n\t pop r9 \n\t pop r8 \n\t pop r7 \n\t pop r6 \n\t pop r5 \n\t pop r4 \n\t pop r3 \n\t pop r2");
asm volatile ("ret"); };
#define SCoopGetSP() (uint16_t)SP // direct read access to SP register is possible
#define AVR_ATOMIC for ( uint8_t sreg_save __attribute__((__cleanup__(__iRestore))) = SREG, __ToDo = __iCliRetVal() ; __ToDo ; __ToDo = 0 )
static inline void __iRestore(const uint8_t *__s) { SREG = *__s; asm volatile ("" ::: "memory"); }
static inline uint8_t __iCliRetVal(void) { noInterrupts(); return 1; }
#define ARM_ATOMIC
/******** NEW MICROS METHOD BASED ON CORE_TEENSY / LIMITED TO 16 BITS **********/
/* Copyright (c) 2008-2010 PJRC.COM, LLC
* for the Teensy and Teensy++
* http://www.pjrc.com/teensy/
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#if F_CPU == 16000000L
#define TIMER0_MICROS_INC 4
#elif F_CPU == 8000000L
#define TIMER0_MICROS_INC 8
#elif F_CPU == 4000000L
#define TIMER0_MICROS_INC 16
#elif F_CPU == 2000000L
#define TIMER0_MICROS_INC 32
#elif F_CPU == 1000000L
#define TIMER0_MICROS_INC 64
#else
#define SCoopMicros() ((micros_t)micros()) // using the standard micros()
#warning "CPU Frequence not supported by the new micros() function"
#endif
#if defined(CORE_TEENSY)
extern volatile unsigned long timer0_micros_count; // this variable is incremented by timer 0 overflow
static inline micros_t SCoopMicros16(void) __attribute__((always_inline));
static inline micros_t SCoopMicros16(void) // same as standrad PJRC micros, but in 16 bits and with inlining
{ register micros_t out;
asm volatile(
"in __tmp_reg__, __SREG__" "\n\t"
"cli" "\n\t"
"in %A0, %2" "\n\t"
"in __zero_reg__, %3" "\n\t"
"lds %B0, timer0_micros_count" "\n\t"
"out __SREG__, __tmp_reg__" "\n\t"
"sbrs __zero_reg__, %4" "\n\t"
"rjmp L_%=_skip" "\n\t"
"cpi %A0, 255" "\n\t"
"breq L_%=_skip" "\n\t"
"subi %B0, 256 - %1" "\n\t"
"L_%=_skip:" "\n\t"
"clr __zero_reg__" "\n\t"
"clr __tmp_reg__" "\n\t"
#if F_CPU == 16000000L || F_CPU == 8000000L || F_CPU == 4000000L
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
#if F_CPU == 8000000L || F_CPU == 4000000L
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
#endif
#if F_CPU == 4000000L
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
#endif
"or %B0, __tmp_reg__" "\n\t"
#endif
#if F_CPU == 1000000L || F_CPU == 2000000L
"lsr %A0" "\n\t"
"ror __tmp_reg__" "\n\t"
"lsr %A0" "\n\t"
"ror __tmp_reg__" "\n\t"
#if F_CPU == 2000000L
"lsr %A0" "\n\t"
"ror __tmp_reg__" "\n\t"
#endif
"or %B0, %A0" "\n\t"
"mov %A0, __tmp_reg__" "\n\t"
#endif
: "=r" (out)
: "M" (TIMER0_MICROS_INC),
"I" (_SFR_IO_ADDR(TCNT0)),
"I" (_SFR_IO_ADDR(TIFR0)),
"I" (TOV0) : "r0" ); return out; }
#define SCoopMicros() (SCoopMicros16()) // overloading the standard micros()
#else // end of CORE_TEENSY. Now same job for the Arduino wiring.c library
extern volatile unsigned long timer0_overflow_count; // use this variable which is incremented at each overflow
static inline micros_t SCoopMicros16(void) __attribute__((always_inline));
static inline micros_t SCoopMicros16(void) // same as standrad PJRC micros, but in 16 bits and with inlining
{ register micros_t out ;
asm volatile(
"in __tmp_reg__, __SREG__" "\n\t"
"cli" "\n\t"
"in %A0, %2" "\n\t"
"in __zero_reg__, %3" "\n\t"
"lds %B0, timer0_overflow_count" "\n\t"
"out __SREG__, __tmp_reg__" "\n\t"
"sbrs __zero_reg__, %4" "\n\t"
"rjmp L_%=_skip" "\n\t"
"cpi %A0, 255" "\n\t"
"breq L_%=_skip" "\n\t"
#if F_CPU == 16000000L
"subi %B0, 1" "\n\t"
#elif F_CPU == 8000000L
"subi %B0, 2" "\n\t"
#endif
"L_%=_skip:" "\n\t"
"clr __zero_reg__" "\n\t"
"clr __tmp_reg__" "\n\t"
#if F_CPU == 16000000L || F_CPU == 8000000L
"lsl %B0" "\n\t"
"lsl %B0" "\n\t"
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
#if F_CPU == 8000000L
"lsl %B0" "\n\t"
"lsl %A0" "\n\t"
"rol __tmp_reg__" "\n\t"
#endif
"or %B0, __tmp_reg__" "\n\t"
#endif
: "=r" (out)
: "M" (TIMER0_MICROS_INC),
"I" (_SFR_IO_ADDR(TCNT0)),
"I" (_SFR_IO_ADDR(TIFR0)),
"I" (TOV0)
: "r0" ); return out; }
#define SCoopMicros() (SCoopMicros16()) // overloading the standard micros()
#endif // core_teensy
#endif
/********* SCOOPEVENT METHODS *******/
SCoopEvent::SCoopEvent()
{ init(NULL); // initialize specific object variable
registerThis(SCoopEventType); } // add to list and set state to Constructed
void SCoopEvent::registerThis(uint8_t type)
{ pNext = SCoopFirstItem; // memorize the latest item registered
SCoopFirstItem = this; // point the latest item to this one
itemType = type; // just to memorize the object type, as we use polymorphism
state = SCoopCONSTRUCTED; } // we are in the list and ready for a formal "init", either in the skecth or as a constructor extension
SCoopEvent::SCoopEvent(SCoopFunc_t func)
{ init(func);
registerThis(SCoopEventType); }
SCoopEvent::~SCoopEvent() // destructor : remove item from the list
{ unregisterThis();
if (SCoopFirstTaskItem == this) SCoopFirstTaskItem = pNext; // we do not need to change this if this is not the first task
// below section should be in Task Destructor, but didnt work there, probleme with chaining... so I put it here...
if ((itemType == SCoopDynamicTask) || (itemType == SCoopTaskType)) {
SCINM.targetCycleMicros -= reinterpret_cast<SCoopTask*>(this)->quantumMicros; // reduce target cycle time
SCoopNumberTask--;
#if SCoopYIELDCYCLE == 0
if (SCoopNumberTask>0) { SCINM.quantumMicrosReal = quantumMicros / SCoopNumberTask; }
#endif
#if SCoopANDROIDMODE >=2
if (itemType == SCoopDynamicTask) {
free(reinterpret_cast<SCoopTask*>(this)->pStackAddr); }
#endif
}
} // remove from list
void SCoopEvent::unregisterThis() // remove item from SCoop list (needed for local objects)
{SCoopEvent * ptr = SCoopFirstItem; // lets try first one
if (ptr == this) SCoopFirstItem = ptr->pNext;// if this item is the last one registered
else
do { if (ptr->pNext==this) { // if the next is the one to remove
ptr->pNext = ptr->pNext->pNext; // skip it
break; } } // we are done
while ((ptr=ptr->pNext)); // try next item until we find the end of the list (NULL)
state = SCoopTERMINATED;
};
void SCoopEvent::init(SCoopFunc_t func) // called by constructor, after registration
{ userFunc = func; // hook call for the "run", if not overriden by a virtual void in child object
if (func != NULL) state = SCoopNEW; // this object is ready to get started or even launched (as launch() also call start())
};
void SCoopEvent::start() { // launched by scheduler when calling SCINM.start()
ifSCoopTRACE(2,"Event::start");
if (state >= SCoopNEW) {
setup(); // call the setup function, and do something only if a derived object has been created with this method.
state = SCoopRUNNABLE; };
}
bool SCoopEvent::launch() { // launch or switch into this item or derived
//ifSCoopTRACE(2,"Event::launch"); // removed. too much printing !
if (state & SCoopPAUSED) return false; // check if item is suspended or not
if (!(state & SCoopTRIGGER)) return false;
SCoopATOMIC {
state = SCoopRUNNING; // this also clear the trigger flag at the same time :)
run();
state = SCoopRUNNABLE; } // an event shouldnt pause itself so lets go with RUNNABLE
return true; // has been launched
};
#if SCoopTRACE > 0
void SCoopEvent::traceThis() { // declare the trace functions for debuging or printing some info by user
SCp("this=");SCphex((ptrInt)this & 0xFFFF);
int x = (ptrInt) SCoopGetSP(); // only place where we use SP register, just to see its value
SCp(" SP=");SCphex( x & 0xFFFF); }
void SCoopEvent::trace(char * xx) {
traceThis();SCp(" ");SCpln(xx); }
#endif
void SCoopEvent::pause() // pausing an event just set the state to PAUSED
{ if (state >= SCoopRUNNABLE) { state |= SCoopPAUSED; } };
void SCoopEvent::resume() // resuming an event just clear the flag PAUSED ... might be not enough for user code
{ if (state & SCoopPAUSED) state &= ~SCoopPAUSED; };
bool SCoopEvent::paused() // just return the pause flag status
{ if (state & SCoopPAUSED) return true; else return false; }
/********* SCoopDelay CLASS *******/ // a basic virtual timer implementation. sort of "timerdown"
// very small size code in each method. compiler will probably inline and clone everything
SCoopDelay::SCoopDelay()
{ reset(); }
SCoopDelay::SCoopDelay(SCDelay_t reload)
{ set(setReload(reload)); }
SCDelay_t SCoopDelay::setReload(SCDelay_t reload)
{ return (reloadValue = reload); }
SCDelay_t SCoopDelay::getReload()
{ return reloadValue; }
void SCoopDelay::initReload()
{ set(reloadValue); }
void SCoopDelay::reload()
{ timeValue += reloadValue; }
bool SCoopDelay::reloaded()
{ if (elapsed()) {
reload();
return true; }
return false; }
void SCoopDelay::reset()
{ set(0); }
SCDelay_t SCoopDelay::set(SCDelay_t time)
{ return (timeValue = time + SCoopDelayMillis()); };
SCDelay_t SCoopDelay::get()
{ register SCDelay_t temp =(timeValue - SCoopDelayMillis());
if (temp <0) return 0; else return temp; }
SCDelay_t SCoopDelay::add(SCDelay_t time)
{ timeValue += time; return time; }
SCDelay_t SCoopDelay::sub(SCDelay_t time)
{ timeValue -= time; return time; }
bool SCoopDelay::elapsed()
{ return (get() == 0); }
/********* SCoopDelayUS CLASS *******/ // a basic virtual timer implementation. sort of "timerdown"
// very small size code in each method. compiler will probably inline and clone everything
SCoopDelayus::SCoopDelayus()
{ reset(); }
SCoopDelayus::SCoopDelayus(micros_t reload)
{ set(setReload(reload)); }
micros_t SCoopDelayus::setReload(micros_t reload)
{ return (reloadValue = reload); }
micros_t SCoopDelayus::getReload()
{ return reloadValue; }
void SCoopDelayus::initReload()
{ set(reloadValue); }
void SCoopDelayus::reload()
{ timeValue += reloadValue; }
bool SCoopDelayus::reloaded()
{ if (elapsed()) {
reload();
return true; }
return false; }
void SCoopDelayus::reset()
{ set(0); }
micros_t SCoopDelayus::set(micros_t time)
{ return (timeValue = time + SCoopMicros()); };
micros_t SCoopDelayus::get()
{ register micros_t temp =(timeValue - SCoopMicros());
if (temp <0) return 0; else return temp; }
micros_t SCoopDelayus::add(micros_t time)
{ timeValue += time; return time; }
micros_t SCoopDelayus::sub(micros_t time)
{ timeValue -= time; return time; }
bool SCoopDelayus::elapsed()
{ return (get() == 0); }
/********* SCoopTimer METHODS *******/
SCoopTimer::SCoopTimer() : SCoopEvent()
{initBasic(); init(0,NULL); };
SCoopTimer::SCoopTimer(SCDelay_t period) : SCoopEvent()
{initBasic(); init(period, NULL); };
SCoopTimer::SCoopTimer(SCDelay_t period, SCoopFunc_t func) : SCoopEvent()
{initBasic(); init( period, func); }
void SCoopTimer::initBasic() {
counter = -1;
userFunc = NULL;
itemType = SCoopTimerType; };
void SCoopTimer::init(SCDelay_t period, SCoopFunc_t func) {
timer.setReload(period); timer.reset();
userFunc = func;
if (func != NULL)
state = SCoopNEW; } // we can use this NEW state as the user function is now defined
void SCoopTimer::start() {
ifSCoopTRACE(3,"Timer::start");
SCoopEvent::start();
timer.initReload(); // make sure the timer is starting with the reload period value
}
bool SCoopTimer::launch()
{ if ((counter == 0) || (timer.getReload() == 0)) return false;
if (timer.reloaded()) {
//ifSCoopTRACE(3,"Timer::launch/run"); // removed too much printing
state |= SCoopTRIGGER;
register bool launched = SCoopEvent::launch();
if ((launched ) && (counter > 0)) counter--;
return launched; } // rearm next run time so timers are NOT desynchronized by pause(). (my default choice)
return false; };
SCDelay_t SCoopTimer::getTimeToRun()
{ if ((counter == 0) || (timer.getReload() == 0)) return -1;
return timer.get(); };
void SCoopTimer::setTimeToRun(SCDelay_t time)
{ timer.set(time); };
void SCoopTimer::schedule(SCDelay_t time, SCoopTimerCount_t count)
{ timer.set(timer.setReload(time)); counter = count; };
void SCoopTimer::schedule(SCDelay_t time)
{ schedule(time,-1); };
/********* SOME BASIC FUNCTIONS *******/
void SCoopMemFill(uint8_t *startp, uint8_t *endp, uint8_t v)
{ if (startp) while (startp < endp) *startp++ = v; };
ptrInt SCoopMemSearch(uint8_t *startp, uint8_t *endp, uint8_t v)
{ uint8_t *ptr = startp;
while (ptr < endp) if (*ptr++ != v) break;
return ((ptrInt)ptr-(ptrInt)startp-1);
};
/********* SCoopTASK METHODS *******/
// CONSTRUCTORS
SCoopTask::SCoopTask() : SCoopEvent()
{ initBasic(); }
SCoopTask::SCoopTask(SCoopStack_t* stack, ptrInt size) : SCoopEvent()
{ initBasic(); init(stack,size); }
SCoopTask::SCoopTask(SCoopStack_t* stack, ptrInt size, SCoopFunc_t func) : SCoopEvent()
{ initBasic(); init(stack,size,func); }
void SCoopTask::initBasic() {
SCoopNumberTask++;
pStackAddr = NULL;
pStack = NULL;
userFunc = NULL;
register SCoopEvent* ptr = pNext; // point on the previous item registered in the standard item list (if any)
pNext = SCoopFirstTaskItem; // register in the task list
SCoopFirstTaskItem = this;
if (ptr != pNext) { // if there was another (non task) item in the list before the previous task
SCoopFirstItem = ptr; // mark this item as now being the first
while (ptr->pNext != pNext) ptr = ptr->pNext; // search last event or item
ptr->pNext = this; } // now points to the new task
itemType = SCoopTaskType; }
void SCoopTask::init(SCoopStack_t* stack, ptrInt size)
{ pStackAddr = (uint8_t*)stack;
pStack = (uint8_t*)stack + ((size-sizeof(SCoopStack_t)) // prepare task stack to the top of the space provided
#if defined(SCoop_ARM) && (SCoop_ARM == 1)
& ~7
#endif
);
SCoopMemFill((uint8_t*)stack, pStack, 0x55); // fill with 0x55 patern in order to calculate StackLeft later
};
void SCoopTask::init(SCoopStack_t* stack, ptrInt size, SCoopFunc_t func) // only this one can be called by user
{ init(stack,size);
userFunc = func;
if (func != NULL)
state = SCoopNEW; } // we have a stack and a user function so we can "start" later.
SCoopTask::~SCoopTask(){ } // destructor to remove task from list .. doesnt really work with "delete"
/********* ONLY USED IF SCoopTRACE DEFINED *******/
#if SCoopTRACE > 0
void SCoopTask::trace(char * xx) {
SCoopEvent::traceThis();
SCp(" @Stack=");SCphex((ptrInt)pStackAddr & 0xFFFF);
SCp(" pStack=");SCphex((ptrInt)pStack & 0xFFFF);
SCp(" ");SCpln(xx); }
#endif
/******** START and LAUNCH SECTION ****************/
void SCoopTask::start() {
ifSCoopTRACE(3,"Task::start");
if (pStack) { // sanity check if stack has been allocated by user or constructor ...
if ((state & SCoopNEW)) { // if the task context is not yet set
ASM_ATOMIC { // de activate interrupt so we can use the stack content for further copy/paste
SCoopSwitch(&SCINM.mainEnv,&SCINM.mainEnv); // simulate switching but with current context : back to same place !
if (state & SCoopRUNNABLE) { // this will be executed only when we comeback here with a backToTask the very first time
startFirstLoop(); // quite equivalent to a "setjmp" mechanism
}; // never come back here then.
{ register uint8_t* pEnd = SCINM.mainEnv; //
register uint8_t* pSource = (uint8_t*) SCoopGetSP(); // start from the stack
do { *pStack-- = *pSource--; } // we copy the stack context to the newly pStack space,
while (pSource != pEnd); } // this includes the previous return adress (@!@)
// so we ll endup just below the previous call to scoopswitch above (@\_/@)
}; // we can restore interrupts as we are finished with critical stack handling
}; // continue forward to launch setup
SCoopEvent::start(); // call the user setup function (if defined in derived object) and set object RUNNABLE
quantumMicros = SCINM.startQuantum; // initialize quantum time provided by start (xx) or by user or by default
SCINM.targetCycleMicros += quantumMicros; // cumulate time to calculate target cycle time
prevMicros = SCoopMicros(); // memorize time , to calculate time spent in the task and in the cycle
timer = 0; } // this will enable imediate user call to sleepSync to work properly
} // end start()
void SCoopTask::startFirstLoop() { // will execute this function the first call to backToTask() made by yield()
#if SCoopTIMEREPORT > 0
yieldMicros = 0; maxYieldMicros = 0;
#endif
state = SCoopRUNNING;
while (true) { // a SCoop task will never end ...
if (!(state & SCoopPAUSED)) {
loop(); // call user function (derived virtual loop or user adress)
yieldInline(quantumMicros); } // try to switch task if we reach the end of the user loop
else yield(0); // switch imediately to next task (or scheduler) if we are paused
}
}
bool SCoopTask::launch() {
// ifSCoopTRACE(3,"task::launch")
if (state & SCoopRUNNABLE) { // make sure the task context is setup first and start() has been called already
if (!(state & (SCoopPAUSED | SCoopKILLING)))
{ SCINM.Task = this; // we always can find a pointer to the current task in which we are running
SCoopSwitch(&pStack,&SCINM.mainEnv);
return true; } // return to scheduler / yield() or cycle()
else prevMicros = SCoopMicros(); // just to avoid jeopardizing the cycleMicros in fact
} else
if (state & SCoopNEW) start(); // initialize context if not done in the main arduino setup() section ...
return false; }
#if SCoopANDROIDMODE >= 2
void SCoopTask::kill()
{ if (itemType == SCoopDynamicTask)
state |= (SCoopKILLING );
if (mySCoop.Task == this) yieldSwitch(); // quick return to main scheduler for treating the situation
}
#endif
/******** YIELD SECTION ****************/
void SCoopTask::yield() // check if the quantum time alowed is elapsed, the switch to scheduler
{ yieldInline(quantumMicros); }
void SCoopTask::yield(micros_t quantum)
{ yieldInline(quantum); }
void SCoopTask::yieldInline(micros_t quantum)
{ if (quantum) {
register micros_t spent = SCoopMicros() - prevMicros;
if (spent >= quantum) yieldSpent(spent); } // switch makes sense
else if (!SCINM.Atomic) yieldSwitch();
};
void SCoopTask::yieldSpent(micros_t spent) // immediate jump back to scheduler
{ if (SCINM.Atomic) return; // do nothing if we are in a atomic section
#if SCoopTIMEREPORT > 0 // verifiy if we want to measure timing
if (spent > maxYieldMicros) maxYieldMicros = spent; // check max time to capture peak
yieldMicros += spent - (yieldMicros >> SCoopTIMEREPORT); // this cumulate the time spent in the task over the 2^x last cycles
#endif
yieldSwitch(); }
void SCoopTask::yieldSwitch() {
register SCoopEvent* temp;
if ((SCoopYIELDCYCLE == 1) && // optimize speed by directly switching next adjacent task
((temp=pNext) != NULL) && // only if possible, otherwise back to main loop
((temp->state & (SCoopRUNNABLE | SCoopPAUSED | SCoopKILLING)) == SCoopRUNNABLE)) {
SCINM.Current = temp;
SCINM.Task = (SCoopTask*)temp; // lets go next
SCoopSwitch(&(((SCoopTask*)temp)->pStack),&pStack); } // save our context and use next one
else { // systematically return to main loop or scheduler if using cycle()
SCINM.Task = NULL;
SCoopSwitch(&SCINM.mainEnv,&pStack); } // save context and return to main scheduler
// will return here by launch() from scheduler yield() or cycle()
prevMicros = SCoopMicros();
}; // come back into the task HERE / NOW
/******** SLEEP SECTION ****************/
void SCoopTask::sleep(SCDelay_t ms) // will replace your usual arduino "delay" function
{ sleepMs(ms,false); };
void SCoopTask::sleepSync(SCDelay_t ms) // same as Sleep but delays are not absolute but relative to previous call to SleepSync
{ sleepMs(ms, true); };
void SCoopTask::sleepMs(SCDelay_t ms, bool sync) {
ifSCoopTRACE(3,"Task::sleepms");
if (ms < 1) { timer.reset(); return; }
if (sync) timer.add(ms); else timer.set(ms);
state = SCoopWAITING;
while (timer) yield(0);
state = SCoopRUNNING; }
bool SCoopTask::sleepUntil(vbool& var, SCDelay_t timeOut) // just wait for an "external" variable to become true, with a timeout
{ register bool temp = false;
if (timeOut) {
timer.set(timeOut);
temp = true; }
return sleepUntilBool(var, temp); }
void SCoopTask::sleepUntil(vbool& var) // just wait for an "external" variable to become true
{ sleepUntilBool(var, false); }
bool SCoopTask::sleepUntilBool(vbool& var, bool checkTime) { // just wait for an "external" variable to become true
ifSCoopTRACE(3,"Task::sleepuntil");
state=SCoopWAITING;
while(!var) {
yield(0);
if (checkTime)
if (timer.elapsed()) { state = SCoopRUNNING; return false; }
}
state = SCoopRUNNING;
var=false; return true; }
ptrInt SCoopTask::stackLeft()
{ if (pStackAddr) { // sanity check if stack has been initialized
return SCoopMemSearch(pStackAddr, pStack, 0x55);}
else return 0;
};
/********* SCoop METHODS *******/
SCoop::SCoop() // constructor
{ startQuantum = SCoopDefaultQuantum; // every task will get the default value unless changed in their setup or with sart()
quantumMicros = SCoopDefaultQuantum; // the main loop also
#if SCoopYIELDCYCLE == 0
quantumMicrosReal = 0; // we do nt know yet the number of task registered
#endif
targetCycleMicros = 0; // we do nt know yet the total cycle time
#if SCoopTIMEREPORT > 0 // verifiy if we want to measure timing
cycleMicros = 0; maxCycleMicros = 0;
#endif
Current = NULL;
Task = NULL;
Atomic = 1; }; // ensure yield is not activated
void SCoop::start(micros_t cycleTime, micros_t mainLoop) // define the total length of the cycle and the main loop quantum
{ startQuantum = (cycleTime - mainLoop) / (SCoopNumberTask);
quantumMicros = mainLoop; // specific time for main loop
start(); }
void SCoop::start(micros_t cycleTime) // define the total length of the cycle for all tasks+1
{ startQuantum = cycleTime / (SCoopNumberTask+1); // consider the main loop as a task
quantumMicros = startQuantum; // time for the main loop will be the same then
start(); }
void SCoop::start() // start all objects in the list
{ targetCycleMicros = quantumMicros; // initialization
#if SCoopYIELDCYCLE == 0
quantumMicrosReal = quantumMicros / SCoopNumberTask; // divide time in slice, as we come back here at each task switch...
#endif
#if SCoopTRACE > 1
SCp("starting scheduler : ");SCp(SCoopNumberTask);SCp("+1 tasks, quantum = "); SCp(startQuantum);SCp(", main loop quantum = ");SCp(quantumMicros);
#endif
Current = SCoopFirstItem; // take the first
while (Current) {
Current->start(); // start this task (initialize environement and call setup())
Current = Current->pNext; } // take next ptr
#if SCoopTRACE > 1
SCp(", target cycle time = ");SCpln(targetCycleMicros); // this is calculated by the task::start()
#endif
SCINM.Atomic=0; // ready for switchiching task with "yield"
};
// this is the main code for the Scheduler, relying on yield() method as a state machine
void SCoop::yield0() // can be called from where ever in order to Force the switch to next task
{ if (Task) Task->yield(0); else SCoop::yield(); }
void SCoop::yield() // can be called from where ever in order to Force the switch to next task
{ if (Task) Task->yield(); // we ve been called from a task context lets yield from there
else {
if (Atomic) return; // self explaining
register SCoopEvent* temp = SCoopFirstItem;
while (temp != SCoopFirstTaskItem) { temp->launch(); temp = temp->pNext; } // launch all events
register micros_t time;
if (Current == NULL) { // a cycle is completed
temp = SCoopFirstTaskItem;
if (temp == NULL) return; // no tasks in the list !
// check overall target cycle time before launching first task
#if SCoopTIMEREPORT > 0 // verifiy if we want to measure timing , then calculate average cycle time
time = SCoopMicros() - reinterpret_cast<SCoopTask*>(temp)->prevMicros; // mesure whole cycle length based on first task information
if (quantumMicros) { // check if we are supposed to spend some time in the main loop or not
if (time < targetCycleMicros) return; } // back in main loop() until we reach the expected target cycle time
Current = temp; // we can launch this first task
if (time > maxCycleMicros) maxCycleMicros = time;
cycleMicros += (time - (cycleMicros>> SCoopTIMEREPORT));
#else
if (quantumMicros) { // check if we are supposed to spend some time in the main loop or not
time = SCoopMicros() - reinterpret_cast<SCoopTask*>(temp)->prevMicros; // mesure whole cycle length based on first task information
if (time < targetCycleMicros) return; } // back in main loop() until we reach the expected target cycle time
Current = temp; // we can launch this first task
#endif
}
else { // lets check intertask timing before launching the next
#if SCoopYIELDCYCLE == 0
if (quantumMicrosReal) { // check if we should spend quite some time in the main loop between 2 tasks
time = SCoopMicros()
- reinterpret_cast<SCoopTask*>(Current)->prevMicros // give the time since last task executed
- reinterpret_cast<SCoopTask*>(Current)->quantumMicros; // mesure whole cycle length based on first task information
if (time < quantumMicrosReal) return; // back in main loop() until we reach the expected target cycle time
}
#endif
};
do { temp = Current;
temp->launch(); // now launch tasks from the list, and may be all in a single launch()
Current = temp->pNext;
#if SCoopANDROIDMODE >= 2 // check if we autorize the killme
if ((temp->state & SCoopKILLING) &&
(temp->itemType == SCoopDynamicTask)) {
delete temp;
} // killing done
#endif
#if SCoopYIELDCYCLE == 0 // back to main task if we are not in yield cycle mode
return;
#endif // otherwise we just go back into the main loop
} while (Current);
}
}
void SCoop::cycle() { // execute a complete cycle across all tasks & events
#if SCoopTRACE > 1
SCpln("start scheduler cycle()");
#endif
do { yield(); } while (Current); };
void SCoop::sleep(SCDelay_t time)
{ SCoopDelay SCoopSleepTimer;
SCoopSleepTimer = time; while (SCoopSleepTimer) SCINM.yield(); }
void SCoop::delay(uint32_t ms) {
uint32_t end = millis() + ms;
while (millis() < end) yield();
}
#if SCoopANDROIDMODE >= 1
SCoopTask* SCoop::startLoop(SCoopFunc_t func, uint32_t stackSize) {
uint8_t *stack = (uint8_t*)malloc(stackSize);
if (!stack) return NULL;
SCoopTask *task = new SCoopTask((SCoopStack_t*)stack,stackSize,func);
if (!task) {
free(stack);
return NULL; }
// object now exist and is registered in the list!
task->itemType = SCoopDynamicTask;
return task;
};
#endif
/*************** OVERLOAD STANDARD YIELD *****************/
#if SCoopOVERLOADYIELD == 1
void yield(void) { SCINM.yield(); }; // overload standard arduino yield
void yield0(void){ SCINM.yield0(); }; // overload standard arduino yield
#endif
void __attribute__((weak)) sleep (SCDelay_t time)
{ SCINM.sleep(time); }
/*************** FIFO *****************/
SCoopFifo::SCoopFifo(void * fifo, const uint8_t itemSize, const uint16_t itemNumber)
{ this->itemSize = itemSize;
ptrMin = (uint8_t*)fifo;
ptrIn = (uint8_t*)fifo;
ptrOut = (uint8_t*)fifo;
ptrMax = (uint8_t*)fifo + (itemNumber * itemSize); }
uint16_t SCoopFifo::count() { // return the number of item currently in the fifo
register int16_t temp;
AVR_ATOMIC { temp = (ptrIn-ptrOut); }
return (temp<0 ? (temp + ptrMax-ptrMin) : temp); }
bool SCoopFifo::put(void* var) // put a record in the pifo and return true if ok or false if fifo is full
{ register uint8_t N = itemSize;
register uint8_t* dest;
register uint8_t* source;
AVR_ATOMIC { dest = ptrIn; }
register uint8_t* post = dest + N;
if (post >= ptrMax) { post = ptrMin; }
AVR_ATOMIC { source = ptrOut; }
if (post != source) { // no overload
source = (uint8_t*)var;
do { *dest++ = *source++; } while (--N);
AVR_ATOMIC { ptrIn = post; }
return true; // ok
} else return false; } // fifo was full
bool SCoopFifo::putChar(const uint8_t value) {
uint8_t X=value; return put(&X); }
bool SCoopFifo::putInt(const uint16_t value) {
uint16_t X=value; return put(&X); }
bool SCoopFifo::putLong(const uint32_t value) {
uint32_t X=value; return put(&X); }
bool SCoopFifo::get(void* var) // retreive one record from the fifo, if not empty otherwise return false
{ register uint8_t* In;
register uint8_t* source;
AVR_ATOMIC { In=ptrIn; source=ptrOut; }
if (In != source) {
register uint8_t N = itemSize;
register uint8_t* dest = (uint8_t*)var;
do { *dest++ = *source++; } while (--N) ;
if (source >= ptrMax) { source = ptrMin; }
AVR_ATOMIC { ptrOut = source; }
return true; // ok
} else return false; // fifo was empty
}
void SCoopFifo::getYield(void* var) // same as get(var) but wait until fifo is not empty, and calls yield()
{ register uint8_t* In;
register uint8_t* Out;
while (true) {
AVR_ATOMIC { In=ptrIn; Out=ptrOut; }
if (In == Out) yield(); else break; } ;
get(var); }
uint8_t SCoopFifo::getChar()
{ uint8_t result8;
getYield(&result8); return result8; }
uint16_t SCoopFifo::getInt()
{ uint16_t result16;
getYield(&result16); return result16; }
uint32_t SCoopFifo::getLong()
{ uint32_t result32;
getYield(&result32); return result32; }
uint16_t SCoopFifo::flush() { // empty the fifo
ASM_ATOMIC { // this will de activate interrupts
ptrIn = ptrMin;
ptrOut = ptrMin; } // this will ACTIVATE interrupts
return (ptrMax-ptrMin); }
uint16_t SCoopFifo::flushNonAtomic() { // empty the fifo
ptrIn = ptrMin;
ptrOut = ptrMin;
return (ptrMax-ptrMin); }
Reference in New Issue View Git Blame Copy Permalink