/*************************************************************************** * __________ __ ___. * Open \______ \ ____ ____ | | _\_ |__ _______ ___ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ * \/ \/ \/ \/ \/ * $Id$ * * Copyright (C) 2002 by Ulf Ralberg * * All files in this archive are subject to the GNU General Public License. * See the file COPYING in the source tree root for full license agreement. * * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY * KIND, either express or implied. * ****************************************************************************/ #include "config.h" #include #include "thread.h" #include "panic.h" #include "system.h" #include "kernel.h" #include "cpu.h" #ifdef CPU_COLDFIRE struct regs { unsigned int macsr; /* EMAC status register */ unsigned int d[6]; /* d2-d7 */ unsigned int a[5]; /* a2-a6 */ void *sp; /* Stack pointer (a7) */ void *start; /* Thread start address, or NULL when started */ }; #elif CONFIG_CPU == SH7034 struct regs { unsigned int r[7]; /* Registers r8 thru r14 */ void *sp; /* Stack pointer (r15) */ void *pr; /* Procedure register */ void *start; /* Thread start address, or NULL when started */ }; #elif defined(CPU_ARM) struct regs { unsigned int r[8]; /* Registers r4-r11 */ void *sp; /* Stack pointer (r13) */ unsigned int lr; /* r14 (lr) */ void *start; /* Thread start address, or NULL when started */ }; #elif CONFIG_CPU == TCC730 struct regs { void *sp; /* Stack pointer (a15) */ void *start; /* Thread start address */ int started; /* 0 when not started */ }; #endif #define DEADBEEF ((unsigned int)0xdeadbeef) /* Cast to the the machine int type, whose size could be < 4. */ int num_threads[NUM_CORES]; static volatile int num_sleepers[NUM_CORES]; static int current_thread[NUM_CORES]; static struct regs thread_contexts[NUM_CORES][MAXTHREADS] IBSS_ATTR; const char *thread_name[NUM_CORES][MAXTHREADS]; void *thread_stack[NUM_CORES][MAXTHREADS]; int thread_stack_size[NUM_CORES][MAXTHREADS]; static const char main_thread_name[] = "main"; extern int stackbegin[]; extern int stackend[]; #ifdef CPU_PP #ifndef BOOTLOADER extern int cop_stackbegin[]; extern int cop_stackend[]; #else /* The coprocessor stack is not set up in the bootloader code, but the threading is. No threads are run on the coprocessor, so set up some dummy stack */ int *cop_stackbegin = stackbegin; int *cop_stackend = stackend; #endif #endif void switch_thread(void) ICODE_ATTR; static inline void store_context(void* addr) __attribute__ ((always_inline)); static inline void load_context(const void* addr) __attribute__ ((always_inline)); #ifdef RB_PROFILE #include void profile_thread(void) { profstart(current_thread); } #endif #if defined(CPU_ARM) /*--------------------------------------------------------------------------- * Store non-volatile context. *--------------------------------------------------------------------------- */ static inline void store_context(void* addr) { asm volatile( "stmia %0, { r4-r11, sp, lr }\n" : : "r" (addr) ); } /*--------------------------------------------------------------------------- * Load non-volatile context. *--------------------------------------------------------------------------- */ static inline void load_context(const void* addr) { asm volatile( "ldmia %0, { r4-r11, sp, lr }\n" /* load regs r4 to r14 from context */ "ldr r0, [%0, #40] \n" /* load start pointer */ "mov r1, #0 \n" "cmp r0, r1 \n" /* check for NULL */ "strne r1, [%0, #40] \n" /* if it's NULL, we're already running */ "movne pc, r0 \n" /* not already running, so jump to start */ : : "r" (addr) : "r0", "r1" ); } #elif defined(CPU_COLDFIRE) /*--------------------------------------------------------------------------- * Store non-volatile context. *--------------------------------------------------------------------------- */ static inline void store_context(void* addr) { asm volatile ( "move.l %%macsr,%%d0 \n" "movem.l %%d0/%%d2-%%d7/%%a2-%%a7,(%0) \n" : : "a" (addr) : "d0" /* only! */ ); } /*--------------------------------------------------------------------------- * Load non-volatile context. *--------------------------------------------------------------------------- */ static inline void load_context(const void* addr) { asm volatile ( "movem.l (%0),%%d0/%%d2-%%d7/%%a2-%%a7 \n" /* Load context */ "move.l %%d0,%%macsr \n" "move.l (52,%0),%%d0 \n" /* Get start address */ "beq.b .running \n" /* NULL -> already running */ "clr.l (52,%0) \n" /* Clear start address.. */ "move.l %%d0,%0 \n" "jmp (%0) \n" /* ..and start the thread */ ".running: \n" : : "a" (addr) : "d0" /* only! */ ); } #elif CONFIG_CPU == SH7034 /*--------------------------------------------------------------------------- * Store non-volatile context. *--------------------------------------------------------------------------- */ static inline void store_context(void* addr) { asm volatile ( "add #36,%0 \n" "sts.l pr, @-%0 \n" "mov.l r15,@-%0 \n" "mov.l r14,@-%0 \n" "mov.l r13,@-%0 \n" "mov.l r12,@-%0 \n" "mov.l r11,@-%0 \n" "mov.l r10,@-%0 \n" "mov.l r9, @-%0 \n" "mov.l r8, @-%0 \n" : : "r" (addr) ); } /*--------------------------------------------------------------------------- * Load non-volatile context. *--------------------------------------------------------------------------- */ static inline void load_context(const void* addr) { asm volatile ( "mov.l @%0+,r8 \n" "mov.l @%0+,r9 \n" "mov.l @%0+,r10 \n" "mov.l @%0+,r11 \n" "mov.l @%0+,r12 \n" "mov.l @%0+,r13 \n" "mov.l @%0+,r14 \n" "mov.l @%0+,r15 \n" "lds.l @%0+,pr \n" "mov.l @%0,r0 \n" /* Get start address */ "tst r0,r0 \n" "bt .running \n" /* NULL -> already running */ "lds r0,pr \n" "mov #0,r0 \n" "rts \n" /* Start the thread */ "mov.l r0,@%0 \n" /* Clear start address */ ".running: \n" : : "r" (addr) : "r0" /* only! */ ); } #elif CONFIG_CPU == TCC730 /*--------------------------------------------------------------------------- * Store non-volatile context. *--------------------------------------------------------------------------- */ #define store_context(addr) \ __asm__ volatile ( \ "push r0,r1\n\t" \ "push r2,r3\n\t" \ "push r4,r5\n\t" \ "push r6,r7\n\t" \ "push a8,a9\n\t" \ "push a10,a11\n\t" \ "push a12,a13\n\t" \ "push a14\n\t" \ "ldw @[%0+0], a15\n\t" : : "a" (addr) ); /*--------------------------------------------------------------------------- * Load non-volatile context. *--------------------------------------------------------------------------- */ #define load_context(addr) \ { \ if (!(addr)->started) { \ (addr)->started = 1; \ __asm__ volatile ( \ "ldw a15, @[%0+0]\n\t" \ "ldw a14, @[%0+4]\n\t" \ "jmp a14\n\t" : : "a" (addr) \ ); \ } else \ __asm__ volatile ( \ "ldw a15, @[%0+0]\n\t" \ "pop a14\n\t" \ "pop a13,a12\n\t" \ "pop a11,a10\n\t" \ "pop a9,a8\n\t" \ "pop r7,r6\n\t" \ "pop r5,r4\n\t" \ "pop r3,r2\n\t" \ "pop r1,r0\n\t" : : "a" (addr) \ ); \ \ } #endif /*--------------------------------------------------------------------------- * Switch thread in round robin fashion. *--------------------------------------------------------------------------- */ void switch_thread(void) { #ifdef RB_PROFILE profile_thread_stopped(current_thread); #endif int current; unsigned int *stackptr; #ifdef SIMULATOR /* Do nothing */ #else while (num_sleepers[CURRENT_CORE] == num_threads[CURRENT_CORE]) { /* Enter sleep mode, woken up on interrupt */ #ifdef CPU_COLDFIRE asm volatile ("stop #0x2000"); #elif CONFIG_CPU == SH7034 and_b(0x7F, &SBYCR); asm volatile ("sleep"); #elif CONFIG_CPU == PP5020 /* This should sleep the CPU. It appears to wake by itself on interrupts */ CPU_CTL = 0x80000000; #elif CONFIG_CPU == TCC730 /* Sleep mode is triggered by the SYS instr on CalmRisc16. * Unfortunately, the manual doesn't specify which arg to use. __asm__ volatile ("sys #0x0f"); 0x1f seems to trigger a reset; 0x0f is the only one other argument used by Archos. */ #elif CONFIG_CPU == S3C2440 CLKCON |= 2; #endif } #endif current = current_thread[CURRENT_CORE]; store_context(&thread_contexts[CURRENT_CORE][current]); #if CONFIG_CPU != TCC730 /* Check if the current thread stack is overflown */ stackptr = thread_stack[CURRENT_CORE][current]; if(stackptr[0] != DEADBEEF) panicf("Stkov %s", thread_name[CURRENT_CORE][current]); #endif if (++current >= num_threads[CURRENT_CORE]) current = 0; current_thread[CURRENT_CORE] = current; load_context(&thread_contexts[CURRENT_CORE][current]); #ifdef RB_PROFILE profile_thread_started(current_thread); #endif } void sleep_thread(void) { ++num_sleepers[CURRENT_CORE]; switch_thread(); } void wake_up_thread(void) { num_sleepers[CURRENT_CORE] = 0; } /*--------------------------------------------------------------------------- * Create thread on the current core. * Return ID if context area could be allocated, else -1. *--------------------------------------------------------------------------- */ int create_thread(void (*function)(void), void* stack, int stack_size, const char *name) { return create_thread_on_core(CURRENT_CORE, function, stack, stack_size, name); } /*--------------------------------------------------------------------------- * Create thread on a specific core. * Return ID if context area could be allocated, else -1. *--------------------------------------------------------------------------- */ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack, int stack_size, const char *name) { unsigned int i; unsigned int stacklen; unsigned int *stackptr; struct regs *regs; if (num_threads[core] >= MAXTHREADS) return -1; /* Munge the stack to make it easy to spot stack overflows */ stacklen = stack_size / sizeof(int); stackptr = stack; for(i = 0;i < stacklen;i++) { stackptr[i] = DEADBEEF; } /* Store interesting information */ thread_name[core][num_threads[core]] = name; thread_stack[core][num_threads[core]] = stack; thread_stack_size[core][num_threads[core]] = stack_size; regs = &thread_contexts[core][num_threads[core]]; #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) || defined(CPU_ARM) /* Align stack to an even 32 bit boundary */ regs->sp = (void*)(((unsigned int)stack + stack_size) & ~3); #elif CONFIG_CPU == TCC730 /* Align stack on word boundary */ regs->sp = (void*)(((unsigned long)stack + stack_size - 2) & ~1); regs->started = 0; #endif regs->start = (void*)function; wake_up_thread(); return num_threads[core]++; /* return the current ID, e.g for remove_thread() */ } /*--------------------------------------------------------------------------- * Remove a thread on the current core from the scheduler. * Parameter is the ID as returned from create_thread(). *--------------------------------------------------------------------------- */ void remove_thread(int threadnum) { remove_thread_on_core(CURRENT_CORE, threadnum); } /*--------------------------------------------------------------------------- * Remove a thread on the specified core from the scheduler. * Parameters are the core and the ID as returned from create_thread(). *--------------------------------------------------------------------------- */ void remove_thread_on_core(unsigned int core, int threadnum) { int i; if(threadnum >= num_threads[core]) return; num_threads[core]--; for (i=threadnum; i threadnum) /* within the moved positions? */ current_thread[core]--; /* adjust it, point to same context again */ } void init_threads(void) { unsigned int core = CURRENT_CORE; num_threads[core] = 1; /* We have 1 thread to begin with */ current_thread[core] = 0; /* The current thread is number 0 */ thread_name[core][0] = main_thread_name; /* In multiple core setups, each core has a different stack. There is probably a much better way to do this. */ if(core == CPU) { thread_stack[CPU][0] = stackbegin; thread_stack_size[CPU][0] = (int)stackend - (int)stackbegin; } else { #if NUM_CORES > 1 /* This code path will not be run on single core targets */ thread_stack[COP][0] = cop_stackbegin; thread_stack_size[COP][0] = (int)cop_stackend - (int)cop_stackbegin; #endif } #if CONFIG_CPU == TCC730 thread_contexts[core][0].started = 1; #else thread_contexts[core][0].start = 0; /* thread 0 already running */ #endif num_sleepers[core] = 0; } int thread_stack_usage(int threadnum){ return thread_stack_usage_on_core(CURRENT_CORE, threadnum); } int thread_stack_usage_on_core(unsigned int core, int threadnum) { unsigned int i; unsigned int *stackptr = thread_stack[core][threadnum]; if(threadnum >= num_threads[core]) return -1; for(i = 0;i < thread_stack_size[core][threadnum]/sizeof(int);i++) { if(stackptr[i] != DEADBEEF) break; } return ((thread_stack_size[core][threadnum] - i * sizeof(int)) * 100) / thread_stack_size[core][threadnum]; }