/*************************************************************************** * __________ __ ___. * Open \______ \ ____ ____ | | _\_ |__ _______ ___ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ * \/ \/ \/ \/ \/ * $Id$ * * Copyright (C) 2007 by Daniel Ankers * * PP5002 and PP502x SoC threading support * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY * KIND, either express or implied. * ****************************************************************************/ #if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64 /* Support a special workaround object for large-sector disks */ #define IF_NO_SKIP_YIELD(...) __VA_ARGS__ #endif #if NUM_CORES > 1 extern uintptr_t cpu_idlestackbegin[]; extern uintptr_t cpu_idlestackend[]; extern uintptr_t cop_idlestackbegin[]; extern uintptr_t cop_idlestackend[]; static uintptr_t * const idle_stacks[NUM_CORES] = { [CPU] = cpu_idlestackbegin, [COP] = cop_idlestackbegin }; #if CONFIG_CPU == PP5002 /* Bytes to emulate the PP502x mailbox bits */ struct core_semaphores { volatile uint8_t intend_wake; /* 00h */ volatile uint8_t stay_awake; /* 01h */ volatile uint8_t intend_sleep; /* 02h */ volatile uint8_t unused; /* 03h */ }; static struct core_semaphores core_semaphores[NUM_CORES] IBSS_ATTR; #endif /* CONFIG_CPU == PP5002 */ #endif /* NUM_CORES */ #if CONFIG_CORELOCK == SW_CORELOCK /* Software core locks using Peterson's mutual exclusion algorithm */ /*--------------------------------------------------------------------------- * Initialize the corelock structure. *--------------------------------------------------------------------------- */ void corelock_init(struct corelock *cl) { memset(cl, 0, sizeof (*cl)); } #if 1 /* Assembly locks to minimize overhead */ /*--------------------------------------------------------------------------- * Wait for the corelock to become free and acquire it when it does. *--------------------------------------------------------------------------- */ void corelock_lock(struct corelock *cl) __attribute__((naked)); void corelock_lock(struct corelock *cl) { /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */ asm volatile ( "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ "ldrb r1, [r1] \n" "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */ "eor r2, r1, #0xff \n" /* r2 = othercore */ "strb r2, [r0, #2] \n" /* cl->turn = othercore */ "1: \n" "ldrb r3, [r0, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */ "cmp r3, #0 \n" /* yes? lock acquired */ "bxeq lr \n" "ldrb r3, [r0, #2] \n" /* || cl->turn == core ? */ "cmp r3, r1 \n" "bxeq lr \n" /* yes? lock acquired */ "b 1b \n" /* keep trying */ : : "i"(&PROCESSOR_ID) ); (void)cl; } /*--------------------------------------------------------------------------- * Try to aquire the corelock. If free, caller gets it, otherwise return 0. *--------------------------------------------------------------------------- */ int corelock_try_lock(struct corelock *cl) __attribute__((naked)); int corelock_try_lock(struct corelock *cl) { /* Relies on the fact that core IDs are complementary bitmasks (0x55,0xaa) */ asm volatile ( "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ "ldrb r1, [r1] \n" "mov r3, r0 \n" "strb r1, [r0, r1, lsr #7] \n" /* cl->myl[core] = core */ "eor r2, r1, #0xff \n" /* r2 = othercore */ "strb r2, [r0, #2] \n" /* cl->turn = othercore */ "ldrb r0, [r3, r2, lsr #7] \n" /* cl->myl[othercore] == 0 ? */ "eors r0, r0, r2 \n" /* yes? lock acquired */ "bxne lr \n" "ldrb r0, [r3, #2] \n" /* || cl->turn == core? */ "ands r0, r0, r1 \n" "streqb r0, [r3, r1, lsr #7] \n" /* if not, cl->myl[core] = 0 */ "bx lr \n" /* return result */ : : "i"(&PROCESSOR_ID) ); return 0; (void)cl; } /*--------------------------------------------------------------------------- * Release ownership of the corelock *--------------------------------------------------------------------------- */ void corelock_unlock(struct corelock *cl) __attribute__((naked)); void corelock_unlock(struct corelock *cl) { asm volatile ( "mov r1, %0 \n" /* r1 = PROCESSOR_ID */ "ldrb r1, [r1] \n" "mov r2, #0 \n" /* cl->myl[core] = 0 */ "strb r2, [r0, r1, lsr #7] \n" "bx lr \n" : : "i"(&PROCESSOR_ID) ); (void)cl; } #else /* C versions for reference */ /*--------------------------------------------------------------------------- * Wait for the corelock to become free and aquire it when it does. *--------------------------------------------------------------------------- */ void corelock_lock(struct corelock *cl) { const unsigned int core = CURRENT_CORE; const unsigned int othercore = 1 - core; cl->myl[core] = core; cl->turn = othercore; for (;;) { if (cl->myl[othercore] == 0 || cl->turn == core) break; } } /*--------------------------------------------------------------------------- * Try to aquire the corelock. If free, caller gets it, otherwise return 0. *--------------------------------------------------------------------------- */ int corelock_try_lock(struct corelock *cl) { const unsigned int core = CURRENT_CORE; const unsigned int othercore = 1 - core; cl->myl[core] = core; cl->turn = othercore; if (cl->myl[othercore] == 0 || cl->turn == core) { return 1; } cl->myl[core] = 0; return 0; } /*--------------------------------------------------------------------------- * Release ownership of the corelock *--------------------------------------------------------------------------- */ void corelock_unlock(struct corelock *cl) { cl->myl[CURRENT_CORE] = 0; } #endif /* ASM / C selection */ #endif /* CONFIG_CORELOCK == SW_CORELOCK */ /*--------------------------------------------------------------------------- * Put core in a power-saving state if waking list wasn't repopulated and if * no other core requested a wakeup for it to perform a task. *--------------------------------------------------------------------------- */ #ifdef CPU_PP502x #if NUM_CORES == 1 static inline void core_sleep(void) { sleep_core(CURRENT_CORE); enable_irq(); } #else static inline void core_sleep(unsigned int core) { #if 1 asm volatile ( "mov r0, #4 \n" /* r0 = 0x4 << core */ "mov r0, r0, lsl %[c] \n" "str r0, [%[mbx], #4] \n" /* signal intent to sleep */ "ldr r1, [%[mbx], #0] \n" /* && !(MBX_MSG_STAT & (0x10< 1 /*--------------------------------------------------------------------------- * Switches to a stack that always resides in the Rockbox core. * * Needed when a thread suicides on a core other than the main CPU since the * stack used when idling is the stack of the last thread to run. This stack * may not reside in the core firmware in which case the core will continue * to use a stack from an unloaded module until another thread runs on it. *--------------------------------------------------------------------------- */ static inline void switch_to_idle_stack(const unsigned int core) { asm volatile ( "str sp, [%0] \n" /* save original stack pointer on idle stack */ "mov sp, %0 \n" /* switch stacks */ : : "r"(&idle_stacks[core][IDLE_STACK_WORDS-1])); (void)core; } /*--------------------------------------------------------------------------- * Perform core switch steps that need to take place inside switch_thread. * * These steps must take place while before changing the processor and after * having entered switch_thread since switch_thread may not do a normal return * because the stack being used for anything the compiler saved will not belong * to the thread's destination core and it may have been recycled for other * purposes by the time a normal context load has taken place. switch_thread * will also clobber anything stashed in the thread's context or stored in the * nonvolatile registers if it is saved there before the call since the * compiler's order of operations cannot be known for certain. */ static void core_switch_blk_op(unsigned int core, struct thread_entry *thread) { /* Flush our data to ram */ cpucache_flush(); /* Stash thread in r4 slot */ thread->context.r[0] = (uint32_t)thread; /* Stash restart address in r5 slot */ thread->context.r[1] = thread->context.start; /* Save sp in context.sp while still running on old core */ thread->context.sp = idle_stacks[core][IDLE_STACK_WORDS-1]; } /*--------------------------------------------------------------------------- * Machine-specific helper function for switching the processor a thread is * running on. Basically, the thread suicides on the departing core and is * reborn on the destination. Were it not for gcc's ill-behavior regarding * naked functions written in C where it actually clobbers non-volatile * registers before the intended prologue code, this would all be much * simpler. Generic setup is done in switch_core itself. */ /*--------------------------------------------------------------------------- * This actually performs the core switch. */ static void __attribute__((naked)) switch_thread_core(unsigned int core, struct thread_entry *thread) { /* Pure asm for this because compiler behavior isn't sufficiently predictable. * Stack access also isn't permitted until restoring the original stack and * context. */ asm volatile ( "stmfd sp!, { r4-r11, lr } \n" /* Stack all non-volatile context on current core */ "ldr r2, =idle_stacks \n" /* r2 = &idle_stacks[core][IDLE_STACK_WORDS] */ "ldr r2, [r2, r0, lsl #2] \n" "add r2, r2, %0*4 \n" "stmfd r2!, { sp } \n" /* save original stack pointer on idle stack */ "mov sp, r2 \n" /* switch stacks */ "adr r2, 1f \n" /* r2 = new core restart address */ "str r2, [r1, #40] \n" /* thread->context.start = r2 */ "ldr pc, =switch_thread \n" /* r0 = thread after call - see load_context */ "1: \n" "ldr sp, [r0, #32] \n" /* Reload original sp from context structure */ "mov r1, #0 \n" /* Clear start address */ "str r1, [r0, #40] \n" "ldr r0, =cpucache_invalidate \n" /* Invalidate new core's cache */ "mov lr, pc \n" "bx r0 \n" "ldmfd sp!, { r4-r11, pc } \n" /* Restore non-volatile context to new core and return */ ".ltorg \n" /* Dump constant pool */ : : "i"(IDLE_STACK_WORDS) ); (void)core; (void)thread; } /*--------------------------------------------------------------------------- * Do any device-specific inits for the threads and synchronize the kernel * initializations. *--------------------------------------------------------------------------- */ static void core_thread_init(unsigned int core) INIT_ATTR; static void core_thread_init(unsigned int core) { if (core == CPU) { /* Wake up coprocessor and let it initialize kernel and threads */ #ifdef CPU_PP502x MBX_MSG_CLR = 0x3f; #endif wake_core(COP); /* Sleep until COP has finished */ sleep_core(CPU); } else { /* Wake the CPU and return */ wake_core(CPU); } } #endif /* NUM_CORES */