/********************************************************************************************************* * ------------------------------------------------------------------------------------------------------ * file description * ------------------------------------------------------------------------------------------------------ * \file coroutine.c * \unit coroutine * \brief This is a C language coroutine library * \author Lamdonn * \version v0.2.0 * \license GPL-2.0 * \copyright Copyright (C) 2025 Lamdonn. ********************************************************************************************************/ #include #include "coroutine.h" #if (COROUTINE_STACK_DEFAULT_SIZE < 32) #error "COROUTINE_STACK_DEFAULT_SIZE must be greater than 32" #endif /** * \brief Platform-specific stack get and atomic switch-and-run primitives * \param p Pointer to the stack for COROUTINE_GET_SP * \param new_sp The new stack pointer address for COROUTINE_SWITCH * \param func The task entry function pointer (CoTaskRun_t) * \param arg The scheduler pointer argument (CoScheduler*) */ #if defined(__i386__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %%esp, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ __asm__ volatile ( \ "mov %0, %%esp\n\t" \ "sub $12, %%esp\n\t" \ "push %2\n\t" \ "call *%1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (arg) \ : "memory" \ ); \ } while (0) #elif defined(__x86_64__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %%rsp, %0" : "=r" (p) : : "memory") #if defined(_WIN32) #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("rcx") = (arg); \ __asm__ volatile ( \ "mov %0, %%rsp\n\t" \ "sub $32, %%rsp\n\t" \ "call *%1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "memory" \ ); \ } while (0) #else #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("rdi") = (arg); \ __asm__ volatile ( \ "mov %0, %%rsp\n\t" \ "call *%1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "memory" \ ); \ } while (0) #endif #elif defined(__arm__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0, sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r0") = (arg); \ __asm__ volatile ( \ "mov sp, %0\n\t" \ "blx %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "lr", "memory" \ ); \ } while (0) #elif defined(__aarch64__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0, sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("x0") = (arg); \ __asm__ volatile ( \ "mov sp, %0\n\t" \ "blr %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "x30", "memory" \ ); \ } while (0) #elif defined(__mips__) || defined(__mips64__) #define COROUTINE_GET_SP(p) __asm__ volatile(".set noreorder\n\t" "move %0, $sp\n\t" ".set reorder" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("$4") = (arg); \ __asm__ volatile ( \ ".set noreorder\n\t" \ "move $sp, %0\n\t" \ "jalr %1\n\t" \ "nop\n\t" \ ".set reorder\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "$31", "memory" \ ); \ } while (0) #elif defined(__riscv__) || defined(__riscv) #define COROUTINE_GET_SP(p) __asm__ volatile("mv %0, sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("a0") = (arg); \ __asm__ volatile ( \ "mv sp, %0\n\t" \ "jalr %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "ra", "memory" \ ); \ } while (0) #elif defined(__powerpc__) || defined(__powerpc64__) #define COROUTINE_GET_SP(p) __asm__ volatile("mr %0, r1" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r3") = (arg); \ __asm__ volatile ( \ "mr 1, %0\n\t" \ "mtctr %1\n\t" \ "bctrl\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "lr", "ctr", "memory" \ ); \ } while (0) #elif defined(__s390x__) #define COROUTINE_GET_SP(p) __asm__ volatile("lgr %0, %%r15" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r2") = (arg); \ __asm__ volatile ( \ "lgr 15, %0\n\t" \ "basr 14, %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "r14", "memory" \ ); \ } while (0) #elif defined(__s390__) #define COROUTINE_GET_SP(p) __asm__ volatile("lr %0, %%r15" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r2") = (arg); \ __asm__ volatile ( \ "lr 15, %0\n\t" \ "basr 14, %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "r14", "memory" \ ); \ } while (0) #elif defined(__sparc__) || defined(__sparcv9__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %%sp, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("%o0") = (arg); \ __asm__ volatile ( \ "mov %0, %%sp\n\t" \ "call %1\n\t" \ "nop\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "%o7", "memory" \ ); \ } while (0) #elif defined(__tile__) || defined(__tilegx__) #define COROUTINE_GET_SP(p) __asm__ volatile("move %0, sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r00") = (arg); \ __asm__ volatile ( \ "move sp, %0\n\t" \ "jalr %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "lr", "memory" \ ); \ } while (0) #elif defined(__hppa__) || defined(__hppa64__) || defined(__parisc__) || defined(__parisc64__) #define COROUTINE_GET_SP(p) __asm__ volatile("copy %%r30, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r26") = (arg); \ __asm__ volatile ( \ "copy %0, %%r30\n\t" \ "bve,l (%1), %%r2\n\t" \ "nop\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "r2", "memory" \ ); \ } while (0) #elif defined(__ia64__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0=sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r32") = (arg); \ __asm__ volatile ( \ "mov sp=%0\n\t" \ "mov b6=%1\n\t" \ "br.call.sptk.many b0=b6\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "b0", "b6", "memory" \ ); \ } while (0) #elif defined(__loongarch__) #define COROUTINE_GET_SP(p) __asm__ volatile("move %0, $sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("$r4") = (arg); \ __asm__ volatile ( \ "move $sp, %0\n\t" \ "jirl $ra, %1, 0\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "$ra", "memory" \ ); \ } while (0) #elif defined(__m68k__) #define COROUTINE_GET_SP(p) __asm__ volatile("move.l %%sp, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ __asm__ volatile ( \ "move.l %0, %%sp\n\t" \ "pea (%2)\n\t" \ "jsr (%1)\n\t" \ : \ : "r" (new_sp), "a" (func), "r" (arg) \ : "memory" \ ); \ } while (0) #elif defined(__sh__) || defined(__sh64__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov r15, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r4") = (arg); \ __asm__ volatile ( \ "mov %0, r15\n\t" \ "jsr @%1\n\t" \ "nop\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "pr", "memory" \ ); \ } while (0) #elif defined(__xtensa__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0, a1" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("a2") = (arg); \ __asm__ volatile ( \ "mov a1, %0\n\t" \ "callx0 %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "a0", "memory" \ ); \ } while (0) #elif defined(__CSKY__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0, r14" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r0") = (arg); \ __asm__ volatile ( \ "mov r14, %0\n\t" \ "jsr %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "r15", "memory" \ ); \ } while (0) #elif defined(__MSP430__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov r1, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r12") = (arg); \ __asm__ volatile ( \ "mov %0, r1\n\t" \ "call %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "memory" \ ); \ } while (0) #elif defined(__ARC__) || defined(__arc__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0, sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("r0") = (arg); \ __asm__ volatile ( \ "mov sp, %0\n\t" \ "jl %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "blink", "memory" \ ); \ } while (0) #elif defined(__nds32__) #define COROUTINE_GET_SP(p) __asm__ volatile("mov %0, $sp" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("$r0") = (arg); \ __asm__ volatile ( \ "mov $sp, %0\n\t" \ "jral %1\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "$lp", "memory" \ ); \ } while (0) #elif defined(__alpha__) || defined(__alpha) #define COROUTINE_GET_SP(p) __asm__ volatile("bis $30, $30, %0" : "=r" (p) : : "memory") #define COROUTINE_SWITCH(new_sp, func, arg) \ do { \ register void *__arg0 __asm__("$16") = (arg); \ __asm__ volatile ( \ "bis %0, %0, $30\n\t" \ "jsr $26, (%1)\n\t" \ : \ : "r" (new_sp), "r" (func), "r" (__arg0) \ : "$26", "memory" \ ); \ } while (0) #else #error "Unsupported platform, please implement `COROUTINE_GET_SP` and `COROUTINE_SWITCH`" #endif /** * \brief: Coroutine scheduler state * \note: Coroutine scheduler state is used to indicate the current state of the coroutine scheduler. * The coroutine scheduler state is updated periodically with the given tick interval. */ #define COSCHEDULER_STATE_INTI (0) /**< Coroutine scheduler state initialize */ #define COSCHEDULER_STATE_EXIT (1) /**< Coroutine scheduler state exit */ #define COSCHEDULER_STATE_START (2) /**< Coroutine scheduler state start */ #define COSCHEDULER_STATE_RUNNING (3) /**< Coroutine scheduler state running */ #define COSCHEDULER_STATE_SCHEDULE (4) /**< Coroutine scheduler state schedule */ /** * \brief: Coroutine task state * \note: Coroutine task state is used to indicate the current state of the coroutine task. * The coroutine task state is updated periodically with the given tick interval. */ #define COTASK_STATE_INIT (0) /**< Coroutine task state initialize */ #define COTASK_STATE_READY (1) /**< Coroutine task state ready */ #define COTASK_STATE_RUNNING (2) /**< Coroutine task state running */ #define COTASK_STATE_SUSPEND (3) /**< Coroutine task state suspend */ #define COTASK_STATE_DELETED (4) /**< Coroutine task state deleted */ /** * \brief: Coroutine task flag * \note: Coroutine task flag is used to indicate the current flag of the coroutine task. * The coroutine task flag is updated periodically with the given tick interval. */ #define COTASK_FLAG_ALLOCED (0x01) /**< Coroutine task flag alloced */ #define COTASK_FLAG_DYNC_TCB (0x02) /**< Coroutine task flag dynamic tcb */ #define COTASK_FLAG_DYNC_STACK (0x04) /**< Coroutine task flag dynamic stack */ #define COTASK_FLAG_MANUAL (0x08) /**< Coroutine task flag manual control */ #define COTASK_FLAG_SHARED_STACK (0x10) /**< Coroutine task flag shared stack */ /** * \brief: Coroutine task stack mark * \note: Coroutine task stack mark is used to mark the end of the coroutine task stack. * The coroutine task stack mark is used to check the stack overflow. * During stack counting, it is determined whether the stack address is used without this MARK defined by the stack value. */ #define COTASK_STACK_MARK (0xAAAAAAAA) /** * \brief: Coroutine scheduler state set * \param pScheduler: Coroutine scheduler * \param state: Coroutine scheduler state * \note: This function is used to set the coroutine scheduler state. */ #define COSCHEDULER_SET_STATE(pScheduler, state) longjmp((pScheduler)->env, (state)) /** * \brief: Coroutine scheduler state get * \param pScheduler: Coroutine scheduler * \return: Coroutine scheduler state * \note: This function is used to get the coroutine scheduler state. */ #define COSCHEDULER_GET_STATE(pScheduler) setjmp((pScheduler)->env) /** * \brief: Coroutine scheduler */ typedef struct CoScheduler CoScheduler; /** * \brief: Coroutine task run function * \param pScheduler: Coroutine scheduler * \note: This function must be called in the coroutine context, no matter * whether the coroutine is running or not. */ typedef void (*CoTaskRun_t)(CoScheduler *pScheduler); /** * \brief: Coroutine task * \note: Coroutine task is a coroutine unit, each coroutine task has its own stack, * and can be scheduled by the coroutine scheduler. */ struct CoTask { CoTask_t next; /**< Double-link list, next coroutine task */ CoTask_t prev; /**< Double-link list, previous coroutine task */ CoScheduler *pScheduler; /**< Coroutine scheduler, point to the scheduler that created this coroutine task */ uint8_t state; /**< Coroutine task state */ void *stackBase; /**< Coroutine task stack base address */ size_t stackSize; /**< Coroutine task stack size */ #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) size_t stackMaxUsed; /**< Coroutine task stack max used size */ #endif uint64_t nextRunTick; /**< When the coroutine task is suspended, coroutine task next run tick */ uint32_t flag; /**< Coroutine task flag */ CoTaskEntry_t entry; /**< Coroutine task entry function */ void *arg; /**< Coroutine task argument address */ CoEvent *pEvent; /**< Coroutine task event, point to the event that the coroutine task is waiting for */ jmp_buf env; /**< Coroutine task environment, used for context switch */ jmp_buf callerEnv; /**< Caller environment for direct CoTask_Resume / CoTask_Yield */ #if (COROUTINE_SHARED_STACK_SIZE > 0) void *saveStack; /**< Saved stack buffer when suspended */ size_t saveStackSize; /**< Size of saved stack buffer */ void *saveStackSP; /**< SP pointer when saved */ #endif }; /** * \brief: Coroutine timer * \note: Coroutine timer is a coroutine unit, each coroutine timer has its own period, * and can be scheduled by the coroutine scheduler. */ struct CoTimer { CoTimer_t next; /**< Double-link list, next coroutine timer */ CoTimer_t prev; /**< Double-link list, previous coroutine timer */ CoScheduler *pScheduler; /**< Coroutine scheduler, point to the scheduler that created this coroutine timer */ uint64_t periodTick; /**< Coroutine timer period tick */ uint64_t nextRunTick; /**< Coroutine timer next run tick */ uint32_t flag; /**< Coroutine timer flag */ CoTimerEntry_t entry; /**< Coroutine timer entry function */ }; /** * \brief: Coroutine scheduler * \note: Coroutine scheduler is a coroutine unit, each coroutine scheduler has its own coroutine task list and coroutine timer list, * and can be scheduled by the coroutine scheduler. */ struct CoScheduler { CoTask_t CoTaskCurrent; /**< Current coroutine task */ CoTask_t CoTaskList; /**< Coroutine task list head */ size_t CoTaskListSize; /**< Coroutine task list size */ CoTimer_t CoTimerCurrent; /**< Current coroutine timer */ CoTimer_t CoTimerList; /**< Coroutine timer list head */ size_t CoTimerListSize; /**< Coroutine timer list size */ void *stackTop; /**< Coroutine scheduler stack top address */ jmp_buf env; /**< Coroutine scheduler environment, used for context switch */ CoTick_t tick; /**< Coroutine scheduler tick function */ uint32_t tickInterval; /**< Coroutine scheduler tick interval, indicates how many us each tick is */ #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /** The results of each measurement period are added to this queue, * so the load measured at the last `COROUTINE_LOADING_CALCULATE_QSIZE` time point is kept * and averaged as the real-time load */ uint16_t loadQueueBase[COROUTINE_LOADING_CALCULATE_QSIZE]; /**< Coroutine scheduler loading queue base */ uint16_t loadQueueSize; /**< Coroutine scheduler loading queue size */ uint16_t loadQueueTail; /**< Coroutine scheduler loading queue tail */ uint16_t curLoad; /**< Coroutine scheduler current load */ uint16_t maxLoad; /**< Coroutine scheduler max load */ uint64_t uTick; /**< Coroutine scheduler measures and calculates the load once every `uTick` */ uint64_t sTick; /**< Coroutine scheduler tick that start measures */ uint64_t pTick; /**< Coroutine scheduler tick that previous start measurement task run */ uint64_t cTick; /**< Coroutine scheduler tick that current tick */ uint64_t rTick; /**< Coroutine scheduler tick that task running */ #endif CoTaskRun_t CoTaskRun; /**< Coroutine scheduler task run function, prevents function compilation from being optimized inline */ CoIdleHook_t idleHook; /**< Coroutine scheduler idle hook function */ CoWakeupHook_t wakeupHook; /**< Coroutine scheduler wakeup hook function */ volatile uint8_t exitReq; /**< Asynchronous request to exit scheduler loop */ #if (COROUTINE_SHARED_STACK_SIZE > 0) uint8_t sharedStackBuffer[COROUTINE_SHARED_STACK_SIZE] __attribute__((aligned(16))); /**< Physical shared stack buffer */ CoTask_t sharedStackOwner; /**< Current task whose stack frame is active in sharedStackBuffer */ #endif }; #if (COROUTINE_SCHEDULER_NUMBER > 0) static CoScheduler sCoScheduler[COROUTINE_SCHEDULER_NUMBER] = {0}; #else #error "COROUTINE_SCHEDULER_NUMBER must be greater than 0" #endif #if (COROUTINE_STATIC_TASK_MAX_NUMBER > 0) /**< Coroutine task static array */ struct CoTask sCoTasks[COROUTINE_STATIC_TASK_MAX_NUMBER] = {0}; #endif #if (COROUTINE_STATIC_TIMER_MAX_NUMBER > 0) /**< Coroutine timer static array */ struct CoTimer sCoTimers[COROUTINE_STATIC_TIMER_MAX_NUMBER] = {0}; #endif /** * \brief: Coroutine scheduler lock * \note: Coroutine scheduler lock is used to lock the coroutine scheduler. * The coroutine scheduler lock is used to prevent the coroutine scheduler from being accessed by multiple threads. */ #if (COROUTINE_SCHEDULER_NUMBER > 1) static CoLock_t sCoSchedulerLock = NULL; #else #define sCoSchedulerLock() #endif /** * \brief: Coroutine scheduler unlock * \note: Coroutine scheduler unlock is used to unlock the coroutine scheduler. * The coroutine scheduler unlock is used to release the lock of the coroutine scheduler. */ #if (COROUTINE_SCHEDULER_NUMBER > 1) static CoUnlock_t sCoSchedulerUnlock = NULL; #else #define sCoSchedulerUnlock() #endif /** * \brief: Coroutine scheduler malloc * \note: Coroutine scheduler malloc is used to malloc the coroutine scheduler. * The coroutine scheduler malloc is used to malloc the memory for the coroutine scheduler. */ static CoMalloc_t sCoSchedulerMalloc = NULL; /** * \brief: Coroutine scheduler free * \note: Coroutine scheduler free is used to free the coroutine scheduler. * The coroutine scheduler free is used to free the memory for the coroutine scheduler. */ static CoFree_t sCoSchedulerFree = NULL; /** * \brief: Coroutine scheduler lock * \note: Coroutine scheduler lock is used to lock the coroutine scheduler. * Only for a single scheduler. */ #if (COROUTINE_SCHEDULER_NUMBER > 1) static void CoScheduler_Lock(void) { } #endif /** * \brief: Coroutine scheduler unlock * \note: Coroutine scheduler unlock is used to unlock the coroutine scheduler. * Only for a single scheduler. */ #if (COROUTINE_SCHEDULER_NUMBER > 1) static void CoScheduler_Unlock(void) { } #endif static void EndlessLoop(void) { while (1) { /* Do nothing */ } } static void StackOverflowCheck(CoTask_t task) { if (task == NULL) return; #if (COROUTINE_SHARED_STACK_SIZE > 0) if (task->flag & COTASK_FLAG_SHARED_STACK) return; #endif if (*(uint32_t *)task->stackBase != COTASK_STACK_MARK) { EndlessLoop(); } } #if (COROUTINE_SHARED_STACK_SIZE > 0) static void CoTask_SaveStack(CoTask_t task) { if (task == NULL || !(task->flag & COTASK_FLAG_SHARED_STACK)) { return; } if (sCoSchedulerMalloc == NULL) { return; } char *sp = NULL; COROUTINE_GET_SP(sp); char *stackTop = (char *)((((uintptr_t)task->stackBase + task->stackSize) & ~15) - 16); if (stackTop > sp) { size_t used = (size_t)(stackTop - sp); if (task->saveStackSize < used) { if (task->saveStack != NULL && sCoSchedulerFree != NULL) { sCoSchedulerFree(task->saveStack); } task->saveStack = sCoSchedulerMalloc(used); task->saveStackSize = (task->saveStack != NULL) ? used : 0; } if (task->saveStack != NULL) { memcpy(task->saveStack, sp, used); task->saveStackSP = sp; } #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) if (used > task->stackMaxUsed) { task->stackMaxUsed = used; } #endif } } static void CoTask_RestoreStack(CoScheduler *pScheduler, CoTask_t task) { if (task == NULL || !(task->flag & COTASK_FLAG_SHARED_STACK)) { return; } if (pScheduler == NULL) { pScheduler = task->pScheduler; } /* Zero-copy stack cache hit! If the physical stack still holds this task's data, skip memcpy */ if (pScheduler != NULL && pScheduler->sharedStackOwner == task) { return; } if (task->saveStack != NULL && task->saveStackSP != NULL && task->saveStackSize > 0) { memcpy(task->saveStackSP, task->saveStack, task->saveStackSize); } if (pScheduler != NULL) { pScheduler->sharedStackOwner = task; } } #else #define CoTask_SaveStack(task) #define CoTask_RestoreStack(pScheduler, task) #endif static void Suspend(CoScheduler *pScheduler) { /* Block the coroutine task if the task stack be brokend */ StackOverflowCheck(pScheduler->CoTaskCurrent); #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Calculate the loading time */ pScheduler->cTick = pScheduler->tick(); /* Update the loading time */ if (pScheduler->pTick == 0) { pScheduler->pTick = pScheduler->cTick; } /* Update the running time */ if (pScheduler->cTick > pScheduler->pTick) { /* Add the running time */ pScheduler->rTick += (pScheduler->cTick - pScheduler->pTick); } #endif /* Save shared stack if this task uses shared stack */ CoTask_SaveStack(pScheduler->CoTaskCurrent); /* Yield to the next coroutine task */ COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_SCHEDULE); } static void Resume(CoScheduler *pScheduler) { /* Block the coroutine task if the task stack be brokend */ StackOverflowCheck(pScheduler->CoTaskCurrent); pScheduler->CoTaskCurrent->state = COTASK_STATE_RUNNING; /* Restore shared stack if this task uses shared stack */ CoTask_RestoreStack(pScheduler, pScheduler->CoTaskCurrent); #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Record the task cut time */ pScheduler->pTick = pScheduler->tick(); #endif /* Jump to the coroutine */ longjmp(pScheduler->CoTaskCurrent->env, COSCHEDULER_STATE_RUNNING); } static CoScheduler *GetCurCoScheduler(void) { void *sp = NULL; CoScheduler *pScheduler = NULL; sCoSchedulerLock(); for (int i = 0; i < COROUTINE_SCHEDULER_NUMBER; i++) { pScheduler = &sCoScheduler[i]; if (pScheduler->tick == NULL) continue; /* Check sp wether in the current scheduler stack */ COROUTINE_GET_SP(sp); if (pScheduler->CoTaskCurrent && pScheduler->CoTaskCurrent->stackBase <= sp && sp < (void *)((char *)pScheduler->CoTaskCurrent->stackBase + pScheduler->CoTaskCurrent->stackSize)) { break; } } sCoSchedulerUnlock(); return pScheduler; } static CoScheduler *DistributeCoScheduler(void) { CoScheduler *pScheduler = NULL; CoScheduler *minCoScheduler = NULL; sCoSchedulerLock(); for (int i = 0; i < COROUTINE_SCHEDULER_NUMBER; i++) { pScheduler = &sCoScheduler[i]; if (pScheduler->tick == NULL) continue; if (minCoScheduler == NULL) { minCoScheduler = pScheduler; } else if (pScheduler->CoTaskListSize < minCoScheduler->CoTaskListSize) { minCoScheduler = pScheduler; } } sCoSchedulerUnlock(); return minCoScheduler; } static CoScheduler *DistributeCoSchedulerForTimer(void) { CoScheduler *pScheduler = NULL; CoScheduler *minCoScheduler = NULL; sCoSchedulerLock(); for (int i = 0; i < COROUTINE_SCHEDULER_NUMBER; i++) { pScheduler = &sCoScheduler[i]; if (pScheduler->tick == NULL) continue; if (minCoScheduler == NULL) { minCoScheduler = pScheduler; } else if (pScheduler->CoTimerListSize < minCoScheduler->CoTimerListSize) { minCoScheduler = pScheduler; } } sCoSchedulerUnlock(); return minCoScheduler; } static CoTask_t CreateCoTask(CoScheduler *pScheduler) { #if (COROUTINE_STATIC_TASK_MAX_NUMBER > 0) /* Find the first unallocated coroutine task */ for (int i = 0; i < COROUTINE_STATIC_TASK_MAX_NUMBER; i++) { /* Check the coroutine task is allocated */ if (!(sCoTasks[i].flag & COTASK_FLAG_ALLOCED)) { /* Clear the flag */ sCoTasks[i].flag = 0; /* Set the flag as allocated */ sCoTasks[i].flag |= COTASK_FLAG_ALLOCED; return &sCoTasks[i]; } } #endif /* Allocate the coroutine task dynamically */ if (sCoSchedulerMalloc) { CoTask_t pCoroutine = (CoTask_t)sCoSchedulerMalloc(sizeof(struct CoTask)); if (pCoroutine) { memset(pCoroutine, 0, sizeof(struct CoTask)); /* Set the flag as allocated */ pCoroutine->flag = COTASK_FLAG_ALLOCED | COTASK_FLAG_DYNC_TCB; return pCoroutine; } } return NULL; } static void DeleteCoTask(CoScheduler *pScheduler, CoTask_t CoTaskCurrent) { #if (COROUTINE_SHARED_STACK_SIZE > 0) if (pScheduler != NULL && pScheduler->sharedStackOwner == CoTaskCurrent) { pScheduler->sharedStackOwner = NULL; } if (CoTaskCurrent->flag & COTASK_FLAG_SHARED_STACK) { if (CoTaskCurrent->saveStack != NULL && sCoSchedulerFree != NULL) { sCoSchedulerFree(CoTaskCurrent->saveStack); CoTaskCurrent->saveStack = NULL; CoTaskCurrent->saveStackSize = 0; CoTaskCurrent->saveStackSP = NULL; } } else #endif { /* Free the stack if it is dynamically allocated */ if (CoTaskCurrent->flag & COTASK_FLAG_DYNC_STACK) { sCoSchedulerFree(CoTaskCurrent->stackBase); } } #if (COROUTINE_STATIC_TASK_MAX_NUMBER > 0) /* Check if the coroutine task is statically allocated */ if (&sCoTasks[0] <= CoTaskCurrent && CoTaskCurrent <= &sCoTasks[COROUTINE_STATIC_TASK_MAX_NUMBER - 1]) { /* Clear the flag */ CoTaskCurrent->flag &= ~COTASK_FLAG_ALLOCED; return; } #endif /* Free the TCB if it is dynamically allocated */ if (CoTaskCurrent->flag & COTASK_FLAG_DYNC_TCB) { sCoSchedulerFree(CoTaskCurrent); } } static void InsertCoTaskList(CoScheduler *pScheduler, CoTask_t Task) { /* Check the coroutine task list is empty */ if (pScheduler->CoTaskList == NULL) { /* Set the coroutine task as the head and tail */ pScheduler->CoTaskList = Task; Task->next = Task; Task->prev = Task; } /* Insert the coroutine task to the tail of the list */ else { /* Get the head and tail of the list */ CoTask_t Head = pScheduler->CoTaskList; CoTask_t Tail = Head->prev; /* Insert the coroutine task to the tail */ Tail->next = Task; Task->prev = Tail; Head->prev = Task; Task->next = Head; } pScheduler->CoTaskListSize++; } static void EraseCoTaskList(CoScheduler *pScheduler, CoTask_t CoTaskCurrent) { /* Check the coroutine task is the head of the list */ if (CoTaskCurrent == pScheduler->CoTaskList) { /* Set the next coroutine task as the head */ pScheduler->CoTaskList = CoTaskCurrent->next; } /* Erase the coroutine task from the list */ CoTaskCurrent->prev->next = CoTaskCurrent->next; CoTaskCurrent->next->prev = CoTaskCurrent->prev; /* Decrease the number of coroutine tasks */ pScheduler->CoTaskListSize--; /* Check the coroutine task list is empty */ if (pScheduler->CoTaskListSize == 0) { pScheduler->CoTaskList = NULL; } } static void CoTaskRun(CoScheduler *pScheduler) { /* Set the coroutine task state as running */ pScheduler->CoTaskCurrent->state = COTASK_STATE_RUNNING; /* Run the coroutine task */ pScheduler->CoTaskCurrent->entry(pScheduler->CoTaskCurrent->arg); /* Block the coroutine task if the task stack be brokend */ StackOverflowCheck(pScheduler->CoTaskCurrent); /* Set the coroutine task state as deleted */ pScheduler->CoTaskCurrent->state = COTASK_STATE_DELETED; /* Return control to the scheduler so that it can delete the coroutine task */ Suspend(pScheduler); } #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) static void CoTask_StackInit(uint32_t *pStack, size_t u32Count) { /* Initialize the stack with the mark */ for (size_t i = 0; i < u32Count; i++) { pStack[i] = COTASK_STACK_MARK; } } #endif #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) static uint32_t CoTask_StackUsed(uint32_t *pStack, size_t u32Count) { uint32_t indexLeft = 0; uint32_t indexRight = u32Count; uint32_t indexCenter = u32Count / 2; /* Binary search the mark in the stack */ while (indexLeft < indexRight) { /* Check the mark is in the left half */ if (pStack[indexCenter] == COTASK_STACK_MARK) { indexLeft = indexCenter + 1; } /* Check the mark is in the right half */ else { indexRight = indexCenter; } /* Update the center index */ indexCenter = (indexLeft + indexRight) / 2; } /* Calculate the stack used size */ return (u32Count - indexLeft) * sizeof(uint32_t); } #endif CoTask_t CoTask_CreateP(CoTaskEntry_t entry, CoTaskCreatePara *pPara) { CoScheduler *pScheduler = NULL; CoTask_t Task = NULL; void *stackBase = NULL; size_t stackSize = 0; uint32_t flag = 0; CoTaskCreatePara para = {.pStack=NULL,.stackSize=0,.arg=NULL,.schedulerId=-1}; /* Check the entry function is valid */ if (entry == NULL) { return NULL; } /* Check the create parameter is valid */ if (pPara == NULL) { pPara = ¶ } /* Auto distribute the scheduler if the scheduler id is negative */ if (pPara->schedulerId < 0) { pScheduler = DistributeCoScheduler(); } /* Manual distribute the scheduler if the scheduler id is valid */ else { /* Check the scheduler id is valid */ if (pPara->schedulerId >= COROUTINE_SCHEDULER_NUMBER) { return NULL; } /* Get the scheduler by id */ pScheduler = &sCoScheduler[pPara->schedulerId]; } /* Check the scheduler is valid */ if (pScheduler->tick == NULL) { return NULL; } #if (COROUTINE_SHARED_STACK_SIZE > 0) if (pPara->sharedStack) { /* Shared stack strictly requires user-configured dynamic memory allocator */ if (sCoSchedulerMalloc == NULL || sCoSchedulerFree == NULL) { return NULL; } stackBase = pScheduler->sharedStackBuffer; stackSize = COROUTINE_SHARED_STACK_SIZE; } else #endif { stackBase = pPara->pStack; stackSize = pPara->stackSize; /* If stackBase is empty, an automatic allocation of stackBase is attempted */ if (stackBase == NULL) { if (stackSize == 0) { /* Check the stack size is valid, or use the defualt stack size */ stackSize = COROUTINE_STACK_DEFAULT_SIZE; } /* Check the malloc function is valid */ if (sCoSchedulerMalloc) { /* Allocate the stack memory */ stackBase = sCoSchedulerMalloc(stackSize); if (stackBase == NULL) { return NULL; } /* Set the flag as dynamic stack */ flag |= COTASK_FLAG_DYNC_STACK; } else { return NULL; } } else { /* Check the stack size is valid */ if (stackSize == 0) { return NULL; } } } /* Create the coroutine task */ Task = CreateCoTask(pScheduler); if (Task == NULL) { /* Free the dynamic stack memory if it is allocated */ if (flag & COTASK_FLAG_DYNC_STACK) { sCoSchedulerFree(stackBase); } return NULL; } /* Set the coroutine task parameters */ Task->entry = entry; Task->arg = pPara->arg; Task->stackBase = stackBase; Task->stackSize = stackSize; Task->next = NULL; Task->prev = NULL; Task->state = COTASK_STATE_INIT; Task->pEvent = NULL; Task->nextRunTick = 0; if (pPara->manual) { flag |= COTASK_FLAG_MANUAL; } #if (COROUTINE_SHARED_STACK_SIZE > 0) if (pPara->sharedStack) { flag |= COTASK_FLAG_SHARED_STACK; } #endif Task->flag |= flag; Task->pScheduler = pScheduler; #if (COROUTINE_SHARED_STACK_SIZE > 0) Task->saveStack = NULL; Task->saveStackSize = 0; Task->saveStackSP = NULL; #endif /* Insert the coroutine task into the scheduler if it is an automatic task */ if (!(Task->flag & COTASK_FLAG_MANUAL)) { InsertCoTaskList(pScheduler, Task); } #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) /* Initialize the stack */ Task->stackMaxUsed = 0; #if (COROUTINE_SHARED_STACK_SIZE > 0) if (!(Task->flag & COTASK_FLAG_SHARED_STACK)) #endif { CoTask_StackInit((uint32_t *)stackBase, stackSize / sizeof(uint32_t)); } #endif return Task; } int CoTask_Delete(CoTask_t CoTask) { /* Check the coroutine task is valid */ if (CoTask == NULL) { return COROUTINE_E_INVALID_PARAMETER; } CoScheduler *pScheduler = CoTask->pScheduler; if (pScheduler == NULL) { return COROUTINE_E_INVALID_PARAMETER; } CoTask->state = COTASK_STATE_DELETED; /* If it is a manual task */ if (CoTask->flag & COTASK_FLAG_MANUAL) { /* If a manual task deletes itself while running */ if (CoTask == pScheduler->CoTaskCurrent) { DeleteCoTask(pScheduler, CoTask); longjmp(CoTask->callerEnv, 2); } else { /* Deleted from outside: free its resources directly */ DeleteCoTask(pScheduler, CoTask); return COROUTINE_E_OK; } } else { /* Automatic task managed by scheduler */ /* If an automatic task deletes itself while running */ if (CoTask == pScheduler->CoTaskCurrent) { /* Return control to the scheduler so that it can clean up the task */ if (COSCHEDULER_STATE_RUNNING != setjmp(pScheduler->CoTaskCurrent->env)) { Suspend(pScheduler); } } else { /* Deleted from outside: remove from list and delete */ sCoSchedulerLock(); EraseCoTaskList(pScheduler, CoTask); DeleteCoTask(pScheduler, CoTask); sCoSchedulerUnlock(); return COROUTINE_E_OK; } } return COROUTINE_E_OK; } CoTask_t CoTask_Self(void) { /* Get the current coroutine scheduler */ CoScheduler *pScheduler = GetCurCoScheduler(); /* Return the current coroutine task */ return pScheduler->CoTaskCurrent; } static void CoTaskRun_Direct(void *arg) { CoTask_t task = (CoTask_t)arg; task->state = COTASK_STATE_RUNNING; /* Execute the coroutine entry function */ task->entry(task->arg); /* Check stack overflow */ StackOverflowCheck(task); task->state = COTASK_STATE_DELETED; /* Return to caller of CoTask_Resume with code 2 indicating task finished */ longjmp(task->callerEnv, 2); } int CoTask_Resume(CoTask_t task) { /* Enforce valid task pointer and strict manual mode contract */ if (task == NULL || !(task->flag & COTASK_FLAG_MANUAL)) { return COROUTINE_E_INVALID_PARAMETER; } if (task->state == COTASK_STATE_DELETED) { return 1; /* Already finished */ } CoTask_t prevTask = NULL; /* Update scheduler's current task pointer so CoTask_Self() works */ if (task->pScheduler != NULL) { prevTask = task->pScheduler->CoTaskCurrent; task->pScheduler->CoTaskCurrent = task; } int ret = setjmp(task->callerEnv); if (ret == 0) { if (task->state == COTASK_STATE_INIT) { #if (COROUTINE_SHARED_STACK_SIZE > 0) if ((task->flag & COTASK_FLAG_SHARED_STACK) && task->pScheduler != NULL) { task->pScheduler->sharedStackOwner = task; } #endif void *stackTop = (void *)((((uintptr_t)task->stackBase + task->stackSize) & ~15) - 16); COROUTINE_SWITCH(stackTop, CoTaskRun_Direct, task); } else if (task->state == COTASK_STATE_SUSPEND) { task->state = COTASK_STATE_RUNNING; CoTask_RestoreStack(task->pScheduler, task); longjmp(task->env, 1); } } /* Restore previous current task */ if (task->pScheduler != NULL) { task->pScheduler->CoTaskCurrent = prevTask; } if (ret == 2 || task->state == COTASK_STATE_DELETED) { return 1; /* Task completed */ } return COROUTINE_E_OK; /* Task yielded / suspended */ } int CoTask_Wakeup(CoTask_t task) { if (task == NULL || task->state == COTASK_STATE_DELETED) { return COROUTINE_E_INVALID_PARAMETER; } sCoSchedulerLock(); /* Clear remaining sleep ticks and mark ready for central scheduler */ task->nextRunTick = 0; task->state = COTASK_STATE_READY; sCoSchedulerUnlock(); /* Unblock the scheduler's idleHook if configured */ if (task->pScheduler != NULL && task->pScheduler->wakeupHook != NULL) { task->pScheduler->wakeupHook((uint32_t)(task->pScheduler - sCoScheduler)); } return COROUTINE_E_OK; } int CoTask_SchedulerId(void) { /* Get the current coroutine scheduler */ CoScheduler *pScheduler = GetCurCoScheduler(); /* Return the current coroutine scheduler id */ return (int)(pScheduler - sCoScheduler); } #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) size_t CoTask_StackMaxUsed(CoTask_t CoTask) { /* Check the coroutine task is valid */ if (CoTask == NULL) { return 0; } /* Check the coroutine task is initialized or deleted */ if (CoTask->state == COTASK_STATE_INIT || CoTask->state == COTASK_STATE_DELETED) { return 0; } #if (COROUTINE_SHARED_STACK_SIZE > 0) if (CoTask->flag & COTASK_FLAG_SHARED_STACK) { return CoTask->stackMaxUsed; } #endif /* Calculate the stack max used */ CoTask->stackMaxUsed = CoTask_StackUsed((uint32_t *)CoTask->stackBase, CoTask->stackSize / sizeof(uint32_t)); return CoTask->stackMaxUsed; } #endif #if (COROUTINE_ENABLE_STACK_CALCULATE > 0) size_t CoTask_StackCurUsed(CoTask_t CoTask) { char *sp = NULL; /* Get the current stack pointer */ COROUTINE_GET_SP(sp); /* Calculate the current stack used */ return (size_t)((char *)CoTask->stackBase + CoTask->stackSize - sp); } #endif #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) uint16_t CoScheduler_CurLoad(uint32_t CoSchedulerId) { CoScheduler *pScheduler = NULL; /* Check the coroutine scheduler is valid */ if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return 0; } /* Get the coroutine scheduler */ pScheduler = &sCoScheduler[CoSchedulerId]; if (pScheduler->tick == NULL) { return 0; } /* Return the current loading */ return pScheduler->curLoad; } #endif #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) uint16_t CoScheduler_MaxLoad(uint32_t CoSchedulerId) { CoScheduler *pScheduler = NULL; /* Check the coroutine scheduler is valid */ if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return 0; } /* Get the coroutine scheduler */ pScheduler = &sCoScheduler[CoSchedulerId]; if (pScheduler->tick == NULL) { return 0; } /* Return the maximum loading */ return pScheduler->maxLoad; } #endif void CoEvent_Init(CoEvent *pEvent) { /* Check the coroutine event is valid */ if (pEvent == NULL) { return; } /* Initialize the event flag */ pEvent->flag = 0; } uint32_t CoTask_WaitP(CoTaskWaitPara *pPara) { CoScheduler *pScheduler = NULL; uint32_t evs = 0; CoTaskWaitPara para = {.pEvent=NULL,.ms=0,.tick=0}; uint64_t tick = 0; /* Check the wait parameter */ if (pPara == NULL) { /* Use the default wait parameter, it will waiting nothing */ pPara = ¶ } /* Get the current coroutine scheduler */ pScheduler = GetCurCoScheduler(); if (pScheduler == NULL || pScheduler->CoTaskCurrent == NULL) { return 0; } CoTask_t task = pScheduler->CoTaskCurrent; /* Check the wait parameter */ /* Default wait time is 0, means no wait */ if (pPara->ms != 0 || pPara->tick != 0) { /* Check the wait time ms */ if (pPara->ms != 0 && pScheduler->tickInterval != 0) { tick = pPara->ms * 1000000 / pScheduler->tickInterval; } /* Check the wait time tick */ if (pPara->tick != 0) { /* Select the smaller one */ if (tick == 0 || tick > pPara->tick) { tick = pPara->tick; } } } /* Set the next run tick */ if (tick == 0 && pPara->pEvent != NULL) { task->nextRunTick = 0; } else if (pScheduler->tick != NULL) { task->nextRunTick = pScheduler->tick() + tick; } /* Set the event */ task->pEvent = pPara->pEvent; /* Set the coroutine task state as suspend */ task->state = COTASK_STATE_SUSPEND; /* If running in direct/manual mode (resumed via CoTask_Resume), yield back directly to caller */ if (task->flag & COTASK_FLAG_MANUAL) { if (setjmp(task->env) == 0) { CoTask_SaveStack(task); longjmp(task->callerEnv, 1); } task->state = COTASK_STATE_RUNNING; } else { /* Return control to the scheduler so that it can schedule other tasks */ if (COSCHEDULER_STATE_RUNNING != setjmp(task->env)) { Suspend(pScheduler); } } if (pPara->pEvent != NULL) { /* Get the event flag */ evs = pPara->pEvent->flag; /* Clear the event flag */ pPara->pEvent->flag = 0; } /* Return the event flag */ return evs; } void CoEvent_Notify(CoEvent *pEvent, uint32_t evs) { /* Check the coroutine event is valid */ if (pEvent == NULL || evs == 0) { return; } sCoSchedulerLock(); /* Set the event flag */ pEvent->flag |= evs; sCoSchedulerUnlock(); /* Unblock all schedulers that might have tasks waiting on this event */ for (int i = 0; i < COROUTINE_SCHEDULER_NUMBER; i++) { if (sCoScheduler[i].tick != NULL && sCoScheduler[i].wakeupHook != NULL) { sCoScheduler[i].wakeupHook(i); } } } static CoTimer_t CreateCoTimer(CoScheduler *pScheduler) { #if (COROUTINE_STATIC_TIMER_MAX_NUMBER > 0) /* Search the free coroutine timer */ for (int i = 0; i < COROUTINE_STATIC_TIMER_MAX_NUMBER; i++) { /* Check the coroutine timer is free */ if (!(sCoTimers[i].flag & COTASK_FLAG_ALLOCED)) { /* Allocate the coroutine timer */ sCoTimers[i].flag |= COTASK_FLAG_ALLOCED; return &sCoTimers[i]; } } #endif /* Check the dynamic coroutine timer malloc function */ if (sCoSchedulerMalloc) { /* Allocate the coroutine timer */ CoTimer_t pCoroutineTimer = (CoTimer_t)sCoSchedulerMalloc(sizeof(struct CoTimer)); if (pCoroutineTimer) { /* Initialize the coroutine timer */ pCoroutineTimer->flag = COTASK_FLAG_ALLOCED | COTASK_FLAG_DYNC_TCB; return pCoroutineTimer; } } return NULL; } static void DeleteCoTimer(CoScheduler *pScheduler, CoTimer_t CoTimerCurrent) { #if (COROUTINE_STATIC_TIMER_MAX_NUMBER > 0) /* Check the coroutine timer is static */ if (&sCoTimers[0] <= CoTimerCurrent && CoTimerCurrent <= &sCoTimers[COROUTINE_STATIC_TIMER_MAX_NUMBER - 1]) { /* Free the coroutine timer */ CoTimerCurrent->flag &= ~COTASK_FLAG_ALLOCED; return; } else #endif /* Check the coroutine timer is dynamic */ { /* Check the coroutine timer is dynamic */ if (CoTimerCurrent->flag & COTASK_FLAG_DYNC_TCB) { /* Free the coroutine timer */ sCoSchedulerFree(CoTimerCurrent); } } } static void InsertCoTimerList(CoScheduler *pScheduler, CoTimer_t NewCoTimer) { /* Check the coroutine timer list is empty */ if (pScheduler->CoTimerList == NULL) { pScheduler->CoTimerList = NewCoTimer; /* Initialize the coroutine timer list */ NewCoTimer->next = NewCoTimer; NewCoTimer->prev = NewCoTimer; } else { /* Insert the coroutine timer to the end of the list */ CoTimer_t Head = pScheduler->CoTimerList; CoTimer_t Tail = Head->prev; /* Insert the coroutine timer to the end of the list */ Tail->next = NewCoTimer; NewCoTimer->prev = Tail; Head->prev = NewCoTimer; NewCoTimer->next = Head; } /* Update the coroutine timer list size */ pScheduler->CoTimerListSize++; } static void EraseCoTimerList(CoScheduler *pScheduler, CoTimer_t CoTimerCurrent) { /* Check the coroutine timer is the head of the list */ if (CoTimerCurrent == pScheduler->CoTimerList) { /* Update the coroutine timer list head */ pScheduler->CoTimerList = CoTimerCurrent->next; } /* Erase the coroutine timer from the list */ CoTimerCurrent->prev->next = CoTimerCurrent->next; CoTimerCurrent->next->prev = CoTimerCurrent->prev; /* Update the coroutine timer list size */ pScheduler->CoTimerListSize--; /* Check the coroutine timer list is empty */ if (pScheduler->CoTimerListSize == 0) { pScheduler->CoTimerList = NULL; } } CoTimer_t CoTimer_CreateP(CoTimerEntry_t entry, CoTimerCreatePara *pPara) { CoScheduler *pScheduler = NULL; CoTimer_t Timer = NULL; uint64_t tick = 0; /* Check the coroutine timer entry is valid */ if (entry == NULL) { return NULL; } /* Check the coroutine timer parameter is valid */ if (pPara == NULL) { return NULL; } /* Check the coroutine timer period is valid */ if (pPara->ms == 0 && pPara->tick == 0) { return NULL; } /* Auto distribute the scheduler if the scheduler id is negative */ if (pPara->schedulerId < 0) { pScheduler = DistributeCoSchedulerForTimer(); } /* Manual distribute the scheduler if the scheduler id is valid */ else { /* Check the scheduler id is valid */ if (pPara->schedulerId >= COROUTINE_SCHEDULER_NUMBER) { return NULL; } /* Get the scheduler by id */ pScheduler = &sCoScheduler[pPara->schedulerId]; } /* Check the scheduler is valid */ if (pScheduler->tick == NULL) { return NULL; } /* Check the wait time ms */ if (pPara->ms != 0) { tick = pPara->ms * 1000000 / pScheduler->tickInterval; } /* Check the wait time tick */ if (pPara->tick != 0) { /* Select the smaller one */ if (tick == 0 || tick > pPara->tick) { tick = pPara->tick; } } /* Create the coroutine timer */ Timer = CreateCoTimer(pScheduler); if (Timer == NULL) { return NULL; } /* Initialize the coroutine timer */ Timer->entry = entry; Timer->periodTick = tick; Timer->nextRunTick = pScheduler->tick() + tick; Timer->next = NULL; Timer->prev = NULL; Timer->pScheduler = pScheduler; /* Insert the coroutine timer to the list */ InsertCoTimerList(pScheduler, Timer); return Timer; } void CoTimer_Delete(CoTimer_t Timer) { CoScheduler *pScheduler = NULL; /* Check the coroutine timer is valid */ if (Timer == NULL) { return; } pScheduler = Timer->pScheduler; /* Erase the coroutine timer from the list */ EraseCoTimerList(pScheduler, Timer); /* Free the coroutine timer */ DeleteCoTimer(pScheduler, Timer); } CoTimer_t CoTimer_Self(void) { CoTimer_t Self = NULL; CoScheduler *pScheduler = NULL; CoScheduler *minCoScheduler = NULL; void *sp = NULL; size_t minSize = 0; sCoSchedulerLock(); for (int i = 0; i < COROUTINE_SCHEDULER_NUMBER; i++) { pScheduler = &sCoScheduler[i]; if (pScheduler->tick == NULL) continue; /* Get the current stack pointer */ COROUTINE_GET_SP(sp); /* Check the coroutine scheduler stack top is above the current stack pointer */ if (pScheduler->stackTop - sp > 0) { /* Check the coroutine scheduler stack top is above the current stack pointer */ if (minSize == 0) { minSize = pScheduler->stackTop - sp; minCoScheduler = pScheduler; } /* Check the coroutine scheduler stack top is above the current stack pointer */ else { size_t size = pScheduler->stackTop - sp; if (pScheduler->stackTop - sp < minSize) { minSize = size; minCoScheduler = pScheduler; } } } /* Get the coroutine timer current of the closest scheduler */ Self = minCoScheduler->CoTimerCurrent; } sCoSchedulerUnlock(); return Self; } int CoTimer_SchedulerId(void) { CoTimer_t Timer = NULL; CoScheduler *pScheduler = NULL; /* Get the current coroutine timer */ Timer = CoTimer_Self(); if (Timer == NULL) { return -1; } /* Get the current coroutine scheduler */ pScheduler = Timer->pScheduler; /* Return the current coroutine scheduler id */ return (int)(pScheduler - sCoScheduler); } int Coroutine_InitP(CoroutineInitPara *pPara) { if (pPara == NULL) { return COROUTINE_E_INVALID_PARAMETER; } #if (COROUTINE_SCHEDULER_NUMBER > 1) if (pPara->lock && pPara->unlock) { sCoSchedulerLock = pPara->lock; sCoSchedulerUnlock = pPara->unlock; } else if (pPara->lock == NULL && pPara->unlock == NULL && sCoSchedulerLock == NULL) { sCoSchedulerLock = CoScheduler_Lock; sCoSchedulerUnlock = CoScheduler_Unlock; } #endif sCoSchedulerLock(); if (pPara->malloc && pPara->free) { sCoSchedulerMalloc = pPara->malloc; sCoSchedulerFree = pPara->free; } sCoSchedulerUnlock(); return COROUTINE_E_OK; } int Coroutine_HasMemHook(void) { return (sCoSchedulerMalloc != NULL && sCoSchedulerFree != NULL) ? 1 : 0; } void *Coroutine_Malloc(size_t size) { if (sCoSchedulerMalloc == NULL || size == 0) { return NULL; } return sCoSchedulerMalloc(size); } void Coroutine_Free(void *ptr) { if (sCoSchedulerFree != NULL && ptr != NULL) { sCoSchedulerFree(ptr); } } int CoScheduler_InitP(uint32_t CoSchedulerId, CoTick_t tick, uint32_t tickInterval, CoSchedulerInitPara *pPara) { CoSchedulerInitPara para = {.idleHook=NULL}; /* Check the coroutine scheduler is valid */ if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } /* Check the coroutine scheduler tick function is valid */ if (tick == NULL) { return COROUTINE_E_INVALID_TICK; } /* Check the coroutine scheduler tick interval is valid */ if (tickInterval == 0) { return COROUTINE_E_INVALID_TICK_INTERVAL; } /* Check the coroutine scheduler init parameter is valid */ if (pPara == NULL) { pPara = ¶ } /* Configure default lock if not yet configured */ #if (COROUTINE_SCHEDULER_NUMBER > 1) if (sCoSchedulerLock == NULL) { sCoSchedulerLock = CoScheduler_Lock; sCoSchedulerUnlock = CoScheduler_Unlock; } #endif /* Get the coroutine scheduler */ CoScheduler *pScheduler = &sCoScheduler[CoSchedulerId]; /* Initialize the coroutine scheduler */ pScheduler->tick = tick; pScheduler->tickInterval = tickInterval; #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Initialize the coroutine scheduler loading calculate */ pScheduler->uTick = COROUTINE_LOADING_CALCULATE_PERIOD * 1000000 / pScheduler->tickInterval; pScheduler->pTick = 0; pScheduler->sTick = 0; pScheduler->rTick = 0; pScheduler->curLoad = 0; pScheduler->maxLoad = 0; #endif pScheduler->CoTaskList = NULL; pScheduler->CoTaskCurrent = NULL; pScheduler->CoTaskListSize = 0; pScheduler->CoTimerList = NULL; pScheduler->CoTimerCurrent = NULL; pScheduler->CoTimerListSize = 0; pScheduler->stackTop = NULL; pScheduler->CoTaskRun = CoTaskRun; pScheduler->idleHook = pPara->idleHook; pScheduler->wakeupHook = pPara->wakeupHook; pScheduler->exitReq = 0; #if (COROUTINE_SHARED_STACK_SIZE > 0) pScheduler->sharedStackOwner = NULL; #endif return COROUTINE_E_OK; } int CoScheduler_SetIdleHook(uint32_t CoSchedulerId, CoIdleHook_t idleHook) { if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } sCoScheduler[CoSchedulerId].idleHook = idleHook; return COROUTINE_E_OK; } int CoScheduler_SetWakeupHook(uint32_t CoSchedulerId, CoWakeupHook_t wakeupHook) { if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } sCoScheduler[CoSchedulerId].wakeupHook = wakeupHook; return COROUTINE_E_OK; } int CoScheduler_Wakeup(uint32_t CoSchedulerId) { if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } if (sCoScheduler[CoSchedulerId].wakeupHook != NULL) { sCoScheduler[CoSchedulerId].wakeupHook(CoSchedulerId); } return COROUTINE_E_OK; } int CoScheduler_Start(uint32_t CoSchedulerId) { int state = COSCHEDULER_STATE_INTI; CoScheduler *pScheduler = NULL; if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } pScheduler = &sCoScheduler[CoSchedulerId]; state = COSCHEDULER_GET_STATE(pScheduler); /* Initialize the coroutine scheduler stack pointer */ pScheduler->stackTop = &state; if (COSCHEDULER_STATE_INTI == state) { if (pScheduler->tick != NULL) { /* Jump directly to the scheduling state */ COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_SCHEDULE); } else { /* Jump directly to the exit state */ COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_EXIT); } } else if (COSCHEDULER_STATE_START == state) { #if (COROUTINE_SHARED_STACK_SIZE > 0) if (pScheduler->CoTaskCurrent->flag & COTASK_FLAG_SHARED_STACK) { pScheduler->sharedStackOwner = pScheduler->CoTaskCurrent; } #endif /* Calculate the coroutine scheduler stack pointer */ /* Align both stackBase and stackSize downward to a strict 16-byte boundary for ARM64 / universal safety */ void *stackTop = (void *)((((uintptr_t)pScheduler->CoTaskCurrent->stackBase + pScheduler->CoTaskCurrent->stackSize) & ~15) - 16); /* Switch stack and run the task using atomic context switch primitive */ COROUTINE_SWITCH(stackTop, pScheduler->CoTaskRun, pScheduler); } else if (COSCHEDULER_STATE_EXIT == state) { /* Delete all coroutines in the list */ while (pScheduler->CoTaskList) { CoTask_t Task = pScheduler->CoTaskList; /* Remove the coroutine from the list */ EraseCoTaskList(pScheduler, Task); /* Delete the coroutine */ DeleteCoTask(pScheduler, Task); } /* Delete all coroutine timers in the list */ while (pScheduler->CoTimerList) { CoTimer_t Timer = pScheduler->CoTimerList; /* Erase the coroutine timer from the list */ EraseCoTimerList(pScheduler, Timer); /* Free the coroutine timer */ DeleteCoTimer(pScheduler, Timer); } /* De-initialize the coroutine scheduler */ pScheduler->tick = NULL; pScheduler->tickInterval = 0; pScheduler->CoTaskList = NULL; pScheduler->CoTaskCurrent = NULL; pScheduler->CoTaskListSize = 0; pScheduler->CoTimerList = NULL; pScheduler->CoTimerCurrent = NULL; pScheduler->CoTimerListSize = 0; pScheduler->stackTop = NULL; pScheduler->CoTaskRun = NULL; pScheduler->idleHook = NULL; } else if (COSCHEDULER_STATE_SCHEDULE == state) { /* Infinite loop to run the coroutine scheduler */ while (1) { if (pScheduler->exitReq) { pScheduler->exitReq = 0; COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_EXIT); } uint8_t hasActive = 0; uint64_t nextWakeTick = 0; #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Calculate the coroutine scheduler tick */ if (pScheduler->sTick == 0) { pScheduler->sTick = pScheduler->tick(); } else { pScheduler->cTick = pScheduler->tick(); /* Calculate the time difference between task cut-in and cut-out */ uint64_t diffTick = pScheduler->cTick - pScheduler->sTick; /* Check the time difference is greater than or equal to the tick interval */ if (diffTick >= pScheduler->uTick) { /* Calculate the loading percentage */ uint16_t tload = pScheduler->rTick * 10000 / diffTick; /* Add the loading percentage to the loading queue */ pScheduler->loadQueueBase[pScheduler->loadQueueTail] = tload; pScheduler->loadQueueTail = (pScheduler->loadQueueTail + 1) % COROUTINE_LOADING_CALCULATE_QSIZE; /* Update the loading queue size */ /* If the loading queue size is less than the queue size, increment the size */ if (pScheduler->loadQueueSize < COROUTINE_LOADING_CALCULATE_QSIZE) { pScheduler->loadQueueSize++; } /* If the loading queue size is equal to the queue size, calculate the average loading percentage */ else { uint32_t totalLoad = 0; /* Calculate the total loading percentage */ for (int i = 0; i < COROUTINE_LOADING_CALCULATE_QSIZE; i++) { totalLoad += pScheduler->loadQueueBase[i]; } /* Calculate the average loading percentage */ pScheduler->curLoad = (uint16_t)(totalLoad / COROUTINE_LOADING_CALCULATE_QSIZE); /* Update the maximum loading percentage */ if (pScheduler->curLoad > pScheduler->maxLoad) { pScheduler->maxLoad = pScheduler->curLoad; } } /* Update the scheduler tick */ /* Set the scheduler tick to the current tick */ pScheduler->sTick = pScheduler->cTick; /* Reset the running tick */ pScheduler->rTick = 0; } } #endif /* Check the coroutine scheduler has coroutine */ if (pScheduler->CoTaskList != NULL) { /* Check the current coroutine is NULL */ if (pScheduler->CoTaskCurrent == NULL) { /* Set the current coroutine to the first coroutine in the list */ pScheduler->CoTaskCurrent = pScheduler->CoTaskList; } /* Get the next coroutine */ CoTask_t tCoroutine = pScheduler->CoTaskCurrent->next; /* Loop through the coroutine list */ do { /* Check the coroutine is initialized */ if (tCoroutine->state == COTASK_STATE_INIT) { hasActive = 1; /* Set the current coroutine to the initialized coroutine */ pScheduler->CoTaskCurrent = tCoroutine; /* Jump to the coroutine */ COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_START); } /* Check the coroutine is ready */ else if (tCoroutine->state == COTASK_STATE_READY) { hasActive = 1; /* Set the current coroutine to the ready coroutine */ pScheduler->CoTaskCurrent = tCoroutine; /* Resume the coroutine */ Resume(pScheduler); } /* Check the coroutine is running */ else if (tCoroutine->state == COTASK_STATE_RUNNING) { hasActive = 1; tCoroutine->state = COTASK_STATE_INIT; continue; } /* Check the coroutine is suspend */ else if (tCoroutine->state == COTASK_STATE_SUSPEND) { /* Check the next run time is reached */ if (((tCoroutine->nextRunTick != 0 && pScheduler->tick() >= tCoroutine->nextRunTick) || /* Check the event is set */ (tCoroutine->pEvent != NULL && tCoroutine->pEvent->flag != 0))) { /* Set the coroutine to ready and immediately resume execution */ tCoroutine->state = COTASK_STATE_READY; hasActive = 1; pScheduler->CoTaskCurrent = tCoroutine; Resume(pScheduler); } else { /* Track earliest upcoming wake-up tick */ if (tCoroutine->nextRunTick != 0) { if (nextWakeTick == 0 || tCoroutine->nextRunTick < nextWakeTick) { nextWakeTick = tCoroutine->nextRunTick; } } } } /* Check the coroutine is deleted */ else if (tCoroutine->state == COTASK_STATE_DELETED) { hasActive = 1; CoTask_t nextTask = tCoroutine->next; /* Remove the coroutine from the list */ EraseCoTaskList(pScheduler, tCoroutine); /* Delete the coroutine */ DeleteCoTask(pScheduler, tCoroutine); if (pScheduler->CoTaskList == NULL) { pScheduler->CoTaskCurrent = NULL; break; } tCoroutine = nextTask; continue; } /* Check the coroutine is unknown */ else { /* Set the coroutine to initialized */ tCoroutine->state = COTASK_STATE_INIT; continue; } /* Check the next coroutine */ tCoroutine = tCoroutine->next; } while (tCoroutine != pScheduler->CoTaskCurrent->next); } /* Check the timer scheduler has timer */ if (pScheduler->CoTimerList != NULL) { /* Get the first timer */ CoTimer_t tTimer = pScheduler->CoTimerList; /* Loop through the timer list */ do { /* Check the timer is expired */ if (pScheduler->tick() >= tTimer->nextRunTick) { hasActive = 1; /* Set the current timer to the expired timer */ pScheduler->CoTimerCurrent = tTimer; #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Record the timer cut-in time */ uint64_t pTick = pScheduler->tick(); #endif tTimer->entry(); #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Record the timer cut-out time */ uint64_t cTick = pScheduler->tick(); #endif #if (COROUTINE_ENABLE_LOADING_CALCULATE > 0) /* Record the timer loading time */ pScheduler->rTick += cTick - pTick; #endif /* Set the timer to next run time */ pScheduler->CoTimerCurrent = NULL; tTimer->nextRunTick = pScheduler->tick() + tTimer->periodTick; } else { /* Track earliest upcoming timer wake-up tick */ if (tTimer->nextRunTick != 0) { if (nextWakeTick == 0 || tTimer->nextRunTick < nextWakeTick) { nextWakeTick = tTimer->nextRunTick; } } } /* Check the next timer */ tTimer = tTimer->next; } while (tTimer != pScheduler->CoTimerList); } /* If no task or timer executed in this pass, notify the idle hook if configured */ if (!hasActive && pScheduler->idleHook != NULL) { uint64_t nowTick = pScheduler->tick(); /* If an upcoming timer has already expired, do not sleep! Loop immediately to run it */ if (nextWakeTick != 0 && nextWakeTick <= nowTick) { continue; } uint32_t idleUs = 0; if (nextWakeTick > nowTick) { /* Convert tick difference to microseconds: diffTick * tickInterval(ns) / 1,000 */ uint64_t diffNs = (nextWakeTick - nowTick) * pScheduler->tickInterval; idleUs = (uint32_t)(diffNs / 1000); if (idleUs == 0 && diffNs > 0) { idleUs = 1; } } pScheduler->idleHook(CoSchedulerId, idleUs); } } } else { COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_EXIT); } return 0; } int CoScheduler_Exit(uint32_t CoSchedulerId) { CoScheduler *pScheduler = NULL; /* Check the coroutine scheduler is valid */ if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } /* Get the coroutine scheduler */ pScheduler = &sCoScheduler[CoSchedulerId]; if (pScheduler->tick == NULL) { return COROUTINE_E_INVALID_TICK; } /* If called from within this scheduler's own task, exit immediately */ if (pScheduler == GetCurCoScheduler()) { COSCHEDULER_SET_STATE(pScheduler, COSCHEDULER_STATE_EXIT); } else { /* Called from another thread/scheduler: set async exit request */ pScheduler->exitReq = 1; if (pScheduler->wakeupHook != NULL) { pScheduler->wakeupHook(CoSchedulerId); } } return COROUTINE_E_OK; } int CoScheduler_TaskCount(uint32_t CoSchedulerId) { CoScheduler *pScheduler = NULL; /* Check the coroutine scheduler is valid */ if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } /* Get the coroutine scheduler */ pScheduler = &sCoScheduler[CoSchedulerId]; if (pScheduler->tick == NULL) { return COROUTINE_E_INVALID_TICK; } return (int)pScheduler->CoTaskListSize; } int CoScheduler_TimerCount(uint32_t CoSchedulerId) { CoScheduler *pScheduler = NULL; /* Check the coroutine scheduler is valid */ if (CoSchedulerId >= COROUTINE_SCHEDULER_NUMBER) { return COROUTINE_E_INVALID_PARAMETER; } /* Get the coroutine scheduler */ pScheduler = &sCoScheduler[CoSchedulerId]; if (pScheduler->tick == NULL) { return COROUTINE_E_INVALID_TICK; } return (int)pScheduler->CoTimerListSize; }