diff options
Diffstat (limited to 'src/gos/gos_x_threads.c')
| -rw-r--r-- | src/gos/gos_x_threads.c | 672 |
1 files changed, 672 insertions, 0 deletions
diff --git a/src/gos/gos_x_threads.c b/src/gos/gos_x_threads.c new file mode 100644 index 00000000..8a781b21 --- /dev/null +++ b/src/gos/gos_x_threads.c @@ -0,0 +1,672 @@ +/* + * This file is subject to the terms of the GFX License. If a copy of + * the license was not distributed with this file, you can obtain one at: + * + * http://ugfx.org/license.html + */ + +#include "gfx.h" + +#if GOS_NEED_X_THREADS + +/********************************************************* + * Semaphores and critical region functions + *********************************************************/ + +#if !defined(INTERRUPTS_OFF) || !defined(INTERRUPTS_ON) + #define INTERRUPTS_OFF() + #define INTERRUPTS_ON() +#endif + +void gfxSystemLock(void) { + INTERRUPTS_OFF(); +} + +void gfxSystemUnlock(void) { + INTERRUPTS_ON(); +} + +void gfxMutexInit(gfxMutex *pmutex) { + pmutex[0] = 0; +} + +void gfxMutexEnter(gfxMutex *pmutex) { + INTERRUPTS_OFF(); + while (pmutex[0]) { + INTERRUPTS_ON(); + gfxYield(); + INTERRUPTS_OFF(); + } + pmutex[0] = 1; + INTERRUPTS_ON(); +} + +void gfxMutexExit(gfxMutex *pmutex) { + pmutex[0] = 0; +} + +void gfxSemInit(gfxSem *psem, semcount_t val, semcount_t limit) { + psem->cnt = val; + psem->limit = limit; +} + +bool_t gfxSemWait(gfxSem *psem, delaytime_t ms) { + systemticks_t starttm, delay; + + // Convert our delay to ticks + starttm = 0; + switch (ms) { + case TIME_IMMEDIATE: + delay = TIME_IMMEDIATE; + break; + case TIME_INFINITE: + delay = TIME_INFINITE; + break; + default: + delay = gfxMillisecondsToTicks(ms); + if (!delay) delay = 1; + starttm = gfxSystemTicks(); + } + + INTERRUPTS_OFF(); + while (psem->cnt <= 0) { + INTERRUPTS_ON(); + // Check if we have exceeded the defined delay + switch (delay) { + case TIME_IMMEDIATE: + return FALSE; + case TIME_INFINITE: + break; + default: + if (gfxSystemTicks() - starttm >= delay) + return FALSE; + break; + } + gfxYield(); + INTERRUPTS_OFF(); + } + psem->cnt--; + INTERRUPTS_ON(); + return TRUE; +} + +bool_t gfxSemWaitI(gfxSem *psem) { + if (psem->cnt <= 0) + return FALSE; + psem->cnt--; + return TRUE; +} + +void gfxSemSignal(gfxSem *psem) { + INTERRUPTS_OFF(); + gfxSemSignalI(psem); + INTERRUPTS_ON(); +} + +void gfxSemSignalI(gfxSem *psem) { + if (psem->cnt < psem->limit) + psem->cnt++; +} + +/********************************************************* + * Sleep functions + *********************************************************/ + +void gfxSleepMilliseconds(delaytime_t ms) { + systemticks_t starttm, delay; + + // Safety first + switch (ms) { + case TIME_IMMEDIATE: + return; + case TIME_INFINITE: + while(1) + gfxYield(); + return; + } + + // Convert our delay to ticks + delay = gfxMillisecondsToTicks(ms); + starttm = gfxSystemTicks(); + + do { + gfxYield(); + } while (gfxSystemTicks() - starttm < delay); +} + +void gfxSleepMicroseconds(delaytime_t ms) { + systemticks_t starttm, delay; + + // Safety first + switch (ms) { + case TIME_IMMEDIATE: + return; + case TIME_INFINITE: + while(1) + gfxYield(); + return; + } + + // Convert our delay to ticks + delay = gfxMillisecondsToTicks(ms/1000); + starttm = gfxSystemTicks(); + + do { + gfxYield(); + } while (gfxSystemTicks() - starttm < delay); +} + +/********************************************************* + * Threading functions + *********************************************************/ + +/** For each scheduler the following need to be defined... + * + * void _gfxThreadsInit(void); - Initialise the scheduler + * void _gfxStartThread(thread *oldt, thread *newt); - Start a new thread + * void _gfxTaskSwitch(thread *oldt, thread *newt); - Switch to a different thread + * + */ + +typedef struct thread { + struct thread * next; // Next thread + int flags; // Flags + #define FLG_THD_ALLOC 0x0001 + #define FLG_THD_MAIN 0x0002 + #define FLG_THD_DEAD 0x0004 + #define FLG_THD_WAIT 0x0008 + size_t size; // Size of the thread stack (including this structure) + threadreturn_t (*fn)(void *param); // Thread function + void * param; // Parameter for the thread function + void * cxt; // The current thread context. + } thread; + +typedef struct threadQ { + thread *head; + thread *tail; +} threadQ; + +static threadQ readyQ; // The list of ready threads +static threadQ deadQ; // Where we put threads waiting to be deallocated +static thread * current; // The current running thread +static thread mainthread; // The main thread context + +#if GFX_CPU == GFX_CPU_UNKNOWN + + #include <string.h> // Prototype for memcpy() + #include <setjmp.h> + + /** + * Some compilers define a _setjmp() and a setjmp(). + * The difference between them is that setjmp() saves the signal masks. + * That is of no use to us so we prefer to use the _setjmp() methods. + * If they don't exist compile them to be the standard setjmp() function. + * Similarly for longjmp(). + */ + #if (!defined(setjmp) && !defined(_setjmp)) || defined(__KEIL__) || defined(__C51__) + #define CXT_SAVE setjmp + #else + #define CXT_SAVE _setjmp + #endif + #if (!defined(longjmp) && !defined(_longjmp)) || defined(__KEIL__) || defined(__C51__) + #define CXT_RESTORE longjmp + #else + #define CXT_RESTORE _longjmp + #endif + + // A place to store the main thread context. + // All other threads will store the context directly after the thread structure (as part of the stack space). + static jmp_buf maincxt; + + /** + * There are some compilers we know how they store the jmpbuf. For those + * we can use the constant macro definitions. For others we have to "auto-detect". + * Auto-detection is hairy and there is no guarantee it will work on all architectures. + * For those it doesn't - read the compiler manuals and the library source code to + * work out the correct macro values. + * You can use the debugger to work out the values for your compiler and put them here. + * Defining these macros as constant values makes the system behaviour guaranteed but also + * makes your code compiler and cpu architecture dependent. It also saves a heap of code + * and a few bytes of RAM. + * + * MACROS: + * + * AUTO_DETECT_STACKFRAME TRUE/FALSE - TRUE to auto-detect stack frame structure + * STACK_DIR_UP Macro/bool_t - TRUE if the stack grows up instead of down + * MASK1 Macro/uint32_t - The 1st mask of jmp_buf elements that need relocation + * MASK2 Macro/uint32_t - The 2nd mask of jmp_buf elements that need relocation + * STACK_BASE Macro/size_t - The base of the stack frame relative to the local variables + * _gfxThreadsInit() Macro/Function - Initialise the scheduler + * + */ + #if GFX_COMPILER == GFX_COMPILER_MINGW32 + + #define AUTO_DETECT_STACKFRAME FALSE + #define STACK_DIR_UP FALSE + #define MASK1 0x00000011 + #define MASK2 0x00000000 + #define STACK_BASE 12 + #define _gfxThreadsInit() mainthread.cxt = maincxt + + #else + + // Use auto-detection of the stack frame format + // Assumes all the relevant stuff to be relocated is in the first 256 bytes of the jmpbuf. + #define AUTO_DETECT_STACKFRAME TRUE + #define STACK_DIR_UP stackdirup // TRUE if the stack grow up instead of down + #define MASK1 jmpmask1 // The 1st mask of jmp_buf elements that need relocation + #define MASK2 jmpmask2 // The 2nd mask of jmp_buf elements that need relocation + #define STACK_BASE stackbase // The base of the stack frame relative to the local variables + + // The structure for the saved stack frame information + typedef struct saveloc { + char * localptr; + jmp_buf cxt; + } saveloc; + + static bool_t stackdirup; + static uint32_t jmpmask1; + static uint32_t jmpmask2; + static size_t stackbase; + static saveloc *pframeinfo; + + // These two functions are not static to prevent the compiler removing them as functions + void _gfxGetStackState(void) { + char *c; + pframeinfo->localptr = (char *)&c; + CXT_SAVE(pframeinfo->cxt); + } + void _gfxGetStackStateInFn(void) { + pframeinfo++; + _gfxGetStackState(); + pframeinfo--; + } + static void _gfxThreadsInit(void) { + uint32_t i; + char ** pout; + char ** pin; + size_t diff; + char * framebase; + saveloc tmpsaveloc[2]; + + // Create the main thread context + mainthread.cxt = maincxt; + + // Allocate a buffer to store our test data + pframeinfo = tmpsaveloc; + + // Get details of the stack frame from within a function + _gfxGetStackStateInFn(); + + // Get details of the stack frame outside the function + _gfxGetStackState(); + + /* Work out the frame entries to relocate by treating the jump buffer as an array of pointers */ + stackdirup = pframeinfo[1].localptr > pframeinfo[0].localptr; + pout = (char **)pframeinfo[0].cxt; + pin = (char **)pframeinfo[1].cxt; + diff = pframeinfo[0].localptr - pframeinfo[1].localptr; + framebase = pframeinfo[0].localptr; + jmpmask1 = jmpmask2 = 0; + for (i = 0; i < sizeof(jmp_buf)/sizeof(char *); i++, pout++, pin++) { + if ((size_t)(*pout - *pin) == diff) { + if (i < 32) + jmpmask1 |= 1 << i; + else + jmpmask2 |= 1 << (i-32); + + if (stackdirup) { + if (framebase > *pout) + framebase = *pout; + } else { + if (framebase < *pout) + framebase = *pout; + } + } + } + stackbase = stackdirup ? (pframeinfo[0].localptr - framebase) : (framebase - pframeinfo[0].localptr); + } + + #endif + + // Move the stack frame and relocate the context data + static void _gfxAdjustCxt(thread *t) { + char ** s; + char * nf; + int diff; + uint32_t i; + + // Copy the stack frame + #if AUTO_DETECT_STACKFRAME + if (STACK_DIR_UP) { // Stack grows up + nf = (char *)(t) + sizeof(thread) + sizeof(jmp_buf) + stackbase; + memcpy(t+1, (char *)&s - stackbase, stackbase+sizeof(char *)); + } else { // Stack grows down + nf = (char *)(t) + t->size - (stackbase + sizeof(char *)); + memcpy(nf, &s, stackbase+sizeof(char *)); + } + #elif STACK_DIR_UP + // Stack grows up + nf = (char *)(t) + sizeof(thread) + sizeof(jmp_buf) + stackbase; + memcpy(t+1, (char *)&s - stackbase, stackbase+sizeof(char *)); + #else + // Stack grows down + nf = (char *)(t) + t->size - (stackbase + sizeof(char *)); + memcpy(nf, &s, stackbase+sizeof(char *)); + #endif + + // Relocate the context data + s = (char **)(t->cxt); + diff = nf - (char *)&s; + + // Relocate the elements we know need to be relocated + for (i = MASK1; i ; i >>= 1, s++) { + if ((i & 1)) + *s += diff; + } + #ifdef MASK2 + s = (char **)(t->cxt)+32; + for (i = MASK2; i ; i >>= 1, s++) { + if ((i & 1)) + *s += diff; + } + #endif + } + static void _gfxXSwitch(thread *oldt, thread *newt, bool_t doBuildFrame) { + + // Save the old context + if (CXT_SAVE(oldt->cxt)) return; + + // Do we need to build a new context? + if (doBuildFrame) { + + // Save our existing context as a starting point for the new context + newt->cxt = newt+1; + if (CXT_SAVE(newt->cxt)) { + + // We are now running the new thread + + // We can't use any of the above function parameters here + // as we are on a different stack. + + // Run the users function. + gfxThreadExit(current->fn(current->param)); + + // We never get here as gfxThreadExit() never returns + } + + // Adjust the new context so the stack references are correct + _gfxAdjustCxt(newt); + } + + // Start the new context + CXT_RESTORE(newt->cxt, 1); + } + + #define _gfxTaskSwitch(oldt, newt) _gfxXSwitch(oldt, newt, FALSE) + #define _gfxStartThread(oldt, newt) _gfxXSwitch(oldt, newt, TRUE) + +#elif GFX_CPU == GFX_CPU_CORTEX_M0 || GFX_CPU == GFX_CPU_CORTEX_M1 + + // Use the EABI calling standard (ARM's AAPCS) - Save r4 - r11 + // The context is saved at the current stack location and a pointer is maintained in the thread structure. + + #define _gfxThreadsInit() + + static __attribute__((pcs("aapcs"),naked)) void _gfxTaskSwitch(thread *oldt, thread *newt) { + __asm__ volatile ( "push {r4, r5, r6, r7, lr} \n\t" + "mov r4, r8 \n\t" + "mov r5, r9 \n\t" + "mov r6, r10 \n\t" + "mov r7, r11 \n\t" + "push {r4, r5, r6, r7} \n\t" + "str sp, %[oldtcxt] \n\t" + "ldr sp, %[newtcxt] \n\t" + "pop {r4, r5, r6, r7} \n\t" + "mov r8, r4 \n\t" + "mov r9, r5 \n\t" + "mov r10, r6 \n\t" + "mov r11, r7 \n\t" + "pop {r4, r5, r6, r7, pc} \n\t" + : [newtcxt] "=m" (newt->cxt) + : [oldtcxt] "m" (oldt->cxt) + : "memory"); + } + + static __attribute__((pcs("aapcs"),naked)) void _gfxStartThread(thread *oldt, thread *newt) { + newt->cxt = (char *)newt + newt->size; + __asm__ volatile ( "push {r4, r5, r6, r7, r8, r9, r10, r11, lr} \n\t" // save current context + "str sp, %[oldtcxt] \n\t" // save context pointer + "ldr sp, %[newtcxt] \n\t" // load new context pointer + : [newtcxt] "=m" (newt->cxt) + : [oldtcxt] "m" (oldt->cxt) + : "memory"); + + // Run the users function + gfxThreadExit(current->fn(current->param)); + } + +#elif GFX_CPU == GFX_CPU_CORTEX_M3 || GFX_CPU == GFX_CPU_CORTEX_M4 || GFX_CPU == GFX_CPU_CORTEX_M7 + + // Use the EABI calling standard (ARM's AAPCS) - Save r4 - r11 + // The context is saved at the current stack location and a pointer is maintained in the thread structure. + + #if CORTEX_USE_FPU + #warning "GOS Threads: You have specified GFX_CPU=GFX_CPU_CORTX_M? with no hardware floating point support but CORTEX_USE_FPU is TRUE. Try using GFX_CPU_GFX_CPU_CORTEX_M?_FP instead" + #endif + + #define _gfxThreadsInit() + + static __attribute__((pcs("aapcs"),naked)) void _gfxTaskSwitch(thread *oldt, thread *newt) { + __asm__ volatile ( "push {r4, r5, r6, r7, r8, r9, r10, r11, lr} \n\t" + "str sp, %[oldtcxt] \n\t" + "ldr sp, %[newtcxt] \n\t" + "pop {r4, r5, r6, r7, r8, r9, r10, r11, pc} \n\t" + : [newtcxt] "=m" (newt->cxt) + : [oldtcxt] "m" (oldt->cxt) + : "memory"); + } + + static __attribute__((pcs("aapcs"),naked)) void _gfxStartThread(thread *oldt, thread *newt) { + newt->cxt = (char *)newt + newt->size; + __asm__ volatile ( "push {r4, r5, r6, r7, r8, r9, r10, r11, lr} \n\t" + "str sp, %[oldtcxt] \n\t" + "ldr sp, %[newtcxt] \n\t" + : [newtcxt] "=m" (newt->cxt) + : [oldtcxt] "m" (oldt->cxt) + : "memory"); + + // Run the users function + gfxThreadExit(current->fn(current->param)); + } + +#elif GFX_CPU == GFX_CPU == GFX_CPU_CORTEX_M4_FP || GFX_CPU == GFX_CPU_CORTEX_M7_FP + + // Use the EABI calling standard (ARM's AAPCS) - Save r4 - r11 and floating point + // The context is saved at the current stack location and a pointer is maintained in the thread structure. + + #if !CORTEX_USE_FPU + #warning "GOS Threads: You have specified GFX_CPU=GFX_CPU_CORTX_M?_FP with hardware floating point support but CORTEX_USE_FPU is FALSE. Try using GFX_CPU_GFX_CPU_CORTEX_M? instead" + #endif + + #define _gfxThreadsInit() + + static __attribute__((pcs("aapcs-vfp"),naked)) void _gfxTaskSwitch(thread *oldt, thread *newt) { + __asm__ volatile ( "push {r4, r5, r6, r7, r8, r9, r10, r11, lr} \n\t" + "vpush {s16-s31} \n\t" + "str sp, %[oldtcxt] \n\t" + "ldr sp, %[newtcxt] \n\t" + "vpop {s16-s31} \n\t" + "pop {r4, r5, r6, r7, r8, r9, r10, r11, pc} \n\t" + : [newtcxt] "=m" (newt->cxt) + : [oldtcxt] "m" (oldt->cxt) + : "memory"); + } + + static __attribute__((pcs("aapcs-vfp"),naked)) void _gfxStartThread(thread *oldt, thread *newt) { + newt->cxt = (char *)newt + newt->size; + __asm__ volatile ( "push {r4, r5, r6, r7, r8, r9, r10, r11, lr} \n\t" + "vpush {s16-s31} \n\t" + "str sp, %[oldtcxt] \n\t" + "ldr sp, %[newtcxt] \n\t" + : [newtcxt] "=m" (newt->cxt) + : [oldtcxt] "m" (oldt->cxt) + : "memory"); + + // Run the users function + gfxThreadExit(current->fn(current->param)); + } + +#else + #error "GOS Threads: Unsupported Scheduler. Try setting GFX_CPU = GFX_CPU_UNKNOWN" +#endif + +static void Qinit(threadQ * q) { + q->head = q->tail = 0; +} + +static void Qadd(threadQ * q, thread *t) { + t->next = 0; + if (q->head) { + q->tail->next = t; + q->tail = t; + } else + q->head = q->tail = t; +} + +static thread *Qpop(threadQ * q) { + struct thread * t; + + if (!q->head) + return 0; + t = q->head; + q->head = t->next; + return t; +} + +void _gosThreadsInit(void) { + Qinit(&readyQ); + + mainthread.next = 0; + mainthread.size = sizeof(thread); + mainthread.flags = FLG_THD_MAIN; + mainthread.fn = 0; + mainthread.param = 0; + + _gfxThreadsInit(); + + current = &mainthread; +} + +gfxThreadHandle gfxThreadMe(void) { + return (gfxThreadHandle)current; +} + +// Check if there are dead processes to deallocate +static void cleanUpDeadThreads(void) { + thread *p; + + while ((p = Qpop(&deadQ))) + gfxFree(p); +} + +void gfxYield(void) { + thread *me; + + // Clean up zombies + cleanUpDeadThreads(); + + // Is there another thread to run? + if (!readyQ.head) + return; + + Qadd(&readyQ, me = current); + current = Qpop(&readyQ); + _gfxTaskSwitch(me, current); +} + +// This routine is not currently public - but it could be. +void gfxThreadExit(threadreturn_t ret) { + thread *me; + + // Save the results in case someone is waiting + me = current; + me->param = (void *)ret; + me->flags |= FLG_THD_DEAD; + + // Add us to the dead list if we need deallocation as we can't free ourselves. + // If someone is waiting on the thread they will do the cleanup. + if ((me->flags & (FLG_THD_ALLOC|FLG_THD_WAIT)) == FLG_THD_ALLOC) + Qadd(&deadQ, me); + + // Set the next thread. Exit if it was the last thread + if (!(current = Qpop(&readyQ))) + gfxExit(); + + // Switch to the new thread + _gfxTaskSwitch(me, current); + + // We never get back here as we didn't re-queue ourselves +} + +gfxThreadHandle gfxThreadCreate(void *stackarea, size_t stacksz, threadpriority_t prio, DECLARE_THREAD_FUNCTION((*fn),p), void *param) { + thread * t; + thread * me; + (void) prio; + + // Ensure we have a minimum stack size + if (stacksz < sizeof(thread)+64) { + stacksz = sizeof(thread)+64; + stackarea = 0; + } + + if (stackarea) { + t = (thread *)stackarea; + t->flags = 0; + } else { + t = (thread *)gfxAlloc(stacksz); + if (!t) + return 0; + t->flags = FLG_THD_ALLOC; + } + t->size = stacksz; + t->fn = fn; + t->param = param; + + // Add the current thread to the queue because we are starting a new thread. + me = current; + Qadd(&readyQ, me); + current = t; + + _gfxStartThread(me, t); + + // Return the new thread handle + return t; +} + +threadreturn_t gfxThreadWait(gfxThreadHandle th) { + thread * t; + + t = th; + if (t == current) + return -1; + + // Mark that we are waiting + t->flags |= FLG_THD_WAIT; + + // Wait for the thread to die + while(!(t->flags & FLG_THD_DEAD)) + gfxYield(); + + // Unmark + t->flags &= ~FLG_THD_WAIT; + + // Clean up resources if needed + if (t->flags & FLG_THD_ALLOC) + gfxFree(t); + + // Return the status left by the dead process + return (threadreturn_t)t->param; +} + +#endif /* GFX_USE_OS_RAW32 */ |