From a8b388fb86096b265780ac682adddcec22005f79 Mon Sep 17 00:00:00 2001 From: Michael Sevakis Date: Tue, 27 Nov 2007 01:20:26 +0000 Subject: Enable dualcore for the pp5002 processor by adding the needed cache handling and sleep/wakeup sync to the kernel. Refine some handling of fw/bl startup for all. git-svn-id: svn://svn.rockbox.org/rockbox/trunk@15827 a1c6a512-1295-4272-9138-f99709370657 --- firmware/app.lds | 16 ++-- firmware/export/config.h | 4 +- firmware/export/pp5002.h | 18 ++++ firmware/target/arm/crt0-pp-bl.S | 113 +++++++++++++----------- firmware/target/arm/crt0-pp.S | 84 ++++++++++-------- firmware/target/arm/system-pp5002.c | 71 +++++++++++---- firmware/target/arm/system-target.h | 18 ++-- firmware/thread.c | 170 ++++++++++++++++++++++++++++++++---- 8 files changed, 355 insertions(+), 139 deletions(-) diff --git a/firmware/app.lds b/firmware/app.lds index e3f6ef2e50..d88ce9fd60 100644 --- a/firmware/app.lds +++ b/firmware/app.lds @@ -369,14 +369,6 @@ SECTIONS } > IRAM #if defined(CPU_COLDFIRE) || defined(CPU_ARM) - .stack : - { - *(.stack) - stackbegin = .; - . += 0x2000; - stackend = .; - } > IRAM - #ifdef CPU_PP .idle_stacks : { @@ -392,6 +384,14 @@ SECTIONS } > IRAM #endif + .stack : + { + *(.stack) + stackbegin = .; + . += 0x2000; + stackend = .; + } > IRAM + #else /* TRICK ALERT! We want 0x2000 bytes of stack, but we set the section size smaller, and allow the stack to grow into the .iram copy */ diff --git a/firmware/export/config.h b/firmware/export/config.h index 6a2c02cffd..f377697b70 100644 --- a/firmware/export/config.h +++ b/firmware/export/config.h @@ -423,7 +423,7 @@ #define IDLE_STACK_SIZE 0x80 #define IDLE_STACK_WORDS 0x20 -#if !defined(FORCE_SINGLE_CORE) && CONFIG_CPU != PP5002 +#if !defined(FORCE_SINGLE_CORE) #define NUM_CORES 2 #define CURRENT_CORE current_core() @@ -436,7 +436,7 @@ #define IF_COP_VOID(...) __VA_ARGS__ #define IF_COP_CORE(core) core -#if CONFIG_CPU == PP5020 +#if CONFIG_CPU == PP5020 || CONFIG_CPU == PP5002 #define CONFIG_CORELOCK SW_CORELOCK /* SWP(B) is broken */ #else #define CONFIG_CORELOCK CORELOCK_SWAP diff --git a/firmware/export/pp5002.h b/firmware/export/pp5002.h index 6076703f1a..8882f296c3 100644 --- a/firmware/export/pp5002.h +++ b/firmware/export/pp5002.h @@ -141,14 +141,32 @@ #define PP_VER3 (*(volatile unsigned long *)(0xcf004038)) #define PP_VER4 (*(volatile unsigned long *)(0xcf00403c)) +/* Processors Control */ +#define PROC_STAT (*(volatile unsigned long *)(0xcf004050)) #define CPU_CTL (*(volatile unsigned char *)(0xcf004054)) #define COP_CTL (*(volatile unsigned char *)(0xcf004058)) +#define CPU_SLEEPING 0x8000 +#define COP_SLEEPING 0x4000 +#define PROC_SLEEPING(core) (0x8000 >> (core)) + #define PROC_CTL(core) ((&CPU_CTL)[(core)*4]) #define PROC_SLEEP 0xca #define PROC_WAKE 0xce +/* Cache Control */ +#define CACHE_CTL (*(volatile unsigned long *)(0xcf004024)) +#define CACHE_RUN 0x1 +#define CACHE_INIT 0x2 + +#define CACHE_MASK (*(volatile unsigned long *)(0xf000f020)) +#define CACHE_OPERATION (*(volatile unsigned long *)(0xf000f024)) +#define CACHE_FLUSH_BASE (*(volatile unsigned long *)(0xf000c000)) +#define CACHE_INVALIDATE_BASE (*(volatile unsigned long *)(0xf0004000)) +#define CACHE_SIZE 0x2000 /* PP5002 has 8KB cache */ + +#define CACHE_OP_UNKNOWN1 (1<<11) /* 0x800 */ #define DEV_EN (*(volatile unsigned long *)(0xcf005000)) #define DEV_RS (*(volatile unsigned long *)(0xcf005030)) diff --git a/firmware/target/arm/crt0-pp-bl.S b/firmware/target/arm/crt0-pp-bl.S index 9ab33a78d3..7aabd2b06a 100644 --- a/firmware/target/arm/crt0-pp-bl.S +++ b/firmware/target/arm/crt0-pp-bl.S @@ -32,23 +32,31 @@ start: * */ #if CONFIG_CPU == PP5002 - .equ PROC_ID, 0xc4000000 - .equ COP_CTRL, 0xcf004058 - .equ COP_STATUS, 0xcf004050 - .equ IIS_CONFIG, 0xc0002500 - .equ SLEEP, 0xca - .equ WAKE, 0xce - .equ SLEEPING, 0x4000 + .equ PROC_ID, 0xc4000000 + .equ CPU_CTRL, 0xcf004054 + .equ CPU_STATUS, 0xcf004050 + .equ COP_CTRL, 0xcf004058 + .equ COP_STATUS, 0xcf004050 + .equ IIS_CONFIG, 0xc0002500 + .equ SLEEP, 0xca + .equ WAKE, 0xce + .equ CPUSLEEPING, 0x8000 + .equ COPSLEEPING, 0x4000 + .equ CACHE_CTRL, 0xcf004024 + .equ CACHE_ENAB, 0x2 /* Actually the CACHE_INIT flag */ #else - .equ PROC_ID, 0x60000000 - .equ COP_CTRL, 0x60007004 - .equ COP_STATUS, 0x60007004 - .equ IIS_CONFIG, 0x70002800 - .equ SLEEP, 0x80000000 - .equ WAKE, 0x0 - .equ SLEEPING, 0x80000000 - .equ CACHE_CTRL, 0x6000c000 - .equ CACHE_ENAB, 0x1 + .equ PROC_ID, 0x60000000 + .equ CPU_CTRL, 0x60007000 + .equ CPU_STATUS, 0x60007000 + .equ COP_CTRL, 0x60007004 + .equ COP_STATUS, 0x60007004 + .equ IIS_CONFIG, 0x70002800 + .equ SLEEP, 0x80000000 + .equ WAKE, 0x0 + .equ CPUSLEEPING, 0x80000000 + .equ COPSLEEPING, 0x80000000 + .equ CACHE_CTRL, 0x6000c000 + .equ CACHE_ENAB, 0x1 #endif msr cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ */ @@ -94,23 +102,12 @@ cop: ldr r0, =COP_CTRL mov r1, #SLEEP str r1, [r0] - -#ifdef CPU_PP502x - /* COP: Invalidate cache if enabled */ - ldr r2, =CACHE_CTRL - ldr r1, [r2] - tst r1, #CACHE_ENAB - beq 2f - ldr r0, =0xf000f044 - ldr r1, [r0] - orr r1, r1, #0x6 - str r1, [r0] -1: - ldr r1, [r2] - tst r1, #0x8000 - bne 1b -2: -#endif /* CPU_PP502x */ + nop + nop + + /* Invalidate cache */ + mov r0, #1 + bl cache_op ldr r0, =startup_loc ldr pc, [r0] @@ -120,7 +117,7 @@ cpu: ldr r0, =COP_STATUS 1: ldr r1, [r0] - tst r1, #SLEEPING + tst r1, #COPSLEEPING beq 1b /* Initialise bss section to zero */ @@ -148,22 +145,9 @@ cpu: ldr r1, =startup_loc str r0, [r1] -#ifdef CPU_PP502x - /* Flush cache if enabled */ - ldr r2, =CACHE_CTRL - ldr r1, [r2] - tst r1, #CACHE_ENAB - beq 2f - ldr r0, =0xf000f044 - ldr r1, [r0] - orr r1, r1, #0x2 - str r1, [r0] -1: - ldr r1, [r2] - tst r1, #0x8000 - bne 1b -2: -#endif /* CPU_PP502x */ + /* flush cache */ + mov r0, #0 + bl cache_op /* Wake up the coprocessor before executing the firmware */ ldr r0, =COP_CTRL @@ -192,3 +176,32 @@ boot_table: code+data must stay <= 256 bytes */ .space 400 #endif + +cache_op: + ldr r2, =CACHE_CTRL + ldr r1, [r2] + tst r1, #CACHE_ENAB + bxeq lr + cmp r0, #0 +#ifdef CPU_PP502x + ldr r0, =0xf000f044 + ldr r1, [r0] + orrne r1, r1, #0x6 + orreq r1, r1, #0x2 + str r1, [r0] +1: + ldr r1, [r2] + tst r1, #0x8000 + bne 1b +#elif CONFIG_CPU == PP5002 + ldrne r0, =0xf0004000 + ldreq r0, =0xf000c000 + add r1, r0, #0x2000 + mov r2, #0 +1: + cmp r1, r0 + strhi r2, [r0], #16 + bhi 1b +#endif /* CPU type */ + bx lr + diff --git a/firmware/target/arm/crt0-pp.S b/firmware/target/arm/crt0-pp.S index 8fd1e31f09..dac666ca1c 100644 --- a/firmware/target/arm/crt0-pp.S +++ b/firmware/target/arm/crt0-pp.S @@ -32,45 +32,49 @@ start: * */ #if CONFIG_CPU == PP5002 - .equ PROC_ID, 0xc4000000 - .equ CPU_ICLR, 0xcf001028 - .equ CPU_CTRL, 0xcf004054 - .equ COP_ICLR, 0xcf001038 - .equ COP_CTRL, 0xcf004058 - .equ COP_STATUS, 0xcf004050 - .equ IIS_CONFIG, 0xc0002500 - .equ SLEEP, 0x000000ca - .equ WAKE, 0x000000ce - .equ SLEEPING, 0x00004000 - .equ MMAP_LOG, 0xf000f010 /* MMAP2 */ - .equ MMAP_PHYS, 0xf000f014 + .equ PROC_ID, 0xc4000000 + .equ CPU_ICLR, 0xcf001028 + .equ CPU_CTRL, 0xcf004054 + .equ COP_ICLR, 0xcf001038 + .equ COP_CTRL, 0xcf004058 + .equ CPU_STATUS, 0xcf004050 + .equ COP_STATUS, 0xcf004050 + .equ IIS_CONFIG, 0xc0002500 + .equ SLEEP, 0x000000ca + .equ WAKE, 0x000000ce + .equ CPUSLEEPING, 0x00008000 + .equ COPSLEEPING, 0x00004000 + .equ CACHE_CTRL, 0xcf004024 + .equ MMAP_LOG, 0xf000f000 /* MMAP0 */ + .equ MMAP_PHYS, 0xf000f004 #if MEM > 32 - .equ MMAP_MASK, 0x00007400 + .equ MMAP_MASK, 0x00003c00 #else - .equ MMAP_MASK, 0x00003a00 + .equ MMAP_MASK, 0x00003e00 #endif - .equ MMAP_FLAGS, 0x00003f84 + .equ MMAP_FLAGS, 0x00003f84 #else - .equ PROC_ID, 0x60000000 - .equ CPU_ICLR, 0x60004028 - .equ CPU_CTRL, 0x60007000 - .equ CPU_STATUS, 0x60007000 - .equ COP_ICLR, 0x60004038 - .equ COP_CTRL, 0x60007004 - .equ COP_STATUS, 0x60007004 - .equ IIS_CONFIG, 0x70002800 - .equ SLEEP, 0x80000000 - .equ WAKE, 0x00000000 - .equ SLEEPING, 0x80000000 - .equ CACHE_CTRL, 0x6000c000 - .equ MMAP_LOG, 0xf000f000 /* MMAP0 */ - .equ MMAP_PHYS, 0xf000f004 + .equ PROC_ID, 0x60000000 + .equ CPU_ICLR, 0x60004028 + .equ CPU_CTRL, 0x60007000 + .equ CPU_STATUS, 0x60007000 + .equ COP_ICLR, 0x60004038 + .equ COP_CTRL, 0x60007004 + .equ COP_STATUS, 0x60007004 + .equ IIS_CONFIG, 0x70002800 + .equ SLEEP, 0x80000000 + .equ WAKE, 0x00000000 + .equ CPUSLEEPING, 0x80000000 + .equ COPSLEEPING, 0x80000000 + .equ CACHE_CTRL, 0x6000c000 + .equ MMAP_LOG, 0xf000f000 /* MMAP0 */ + .equ MMAP_PHYS, 0xf000f004 #if MEM > 32 - .equ MMAP_MASK, 0x00003c00 + .equ MMAP_MASK, 0x00003c00 #else - .equ MMAP_MASK, 0x00003e00 + .equ MMAP_MASK, 0x00003e00 #endif - .equ MMAP_FLAGS, 0x00000f84 + .equ MMAP_FLAGS, 0x00000f84 #endif msr cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */ @@ -111,21 +115,22 @@ pad_skip: ldrne r2, =COP_CTRL movne r1, #SLEEP strne r1, [r2] + nop + nop + nop /* wait for co-processor to sleep then CPU can begin its remapping */ ldreq r2, =COP_STATUS 1: ldreq r1, [r2] - tsteq r1, #SLEEPING + tsteq r1, #COPSLEEPING beq 1b -#ifdef CPU_PP502x /* disable cache and local interrupt vectors - it is really not desireable to have them enabled here */ ldr r2, =CACHE_CTRL mov r1, #0 str r1, [r2] -#endif mov r2, #0x40000000 ldr r3, =remap_start @@ -165,6 +170,9 @@ remap_end: /* Sleep us (co-processor) and wait for CPU to do kernel initialization */ movne r3, #SLEEP str r3, [r4] + nop + nop + nop /* Jump to co-processor init */ ldrne pc, =cop_init @@ -174,7 +182,7 @@ cpu_init: ldr r4, =COP_STATUS 1: ldr r3, [r4] - tst r3, #SLEEPING + tst r3, #COPSLEEPING beq 1b /* Copy exception handler code to address 0 */ @@ -275,7 +283,7 @@ cop_init: ldr r4, =CPU_STATUS 1: ldr r3, [r4] - tst r3, #SLEEPING + tst r3, #CPUSLEEPING beq 1b #endif @@ -377,7 +385,7 @@ UIE: #endif /* Align stacks to cache line boundary */ - .balign 16 + .balign 32 /* 256 words of IRQ stack */ .space 256*4 diff --git a/firmware/target/arm/system-pp5002.c b/firmware/target/arm/system-pp5002.c index 08783280be..164913f0f6 100644 --- a/firmware/target/arm/system-pp5002.c +++ b/firmware/target/arm/system-pp5002.c @@ -44,9 +44,7 @@ void irq(void) } else { - if (COP_INT_STAT & TIMER1_MASK) - TIMER1(); - else if (COP_INT_STAT & TIMER2_MASK) + if (COP_INT_STAT & TIMER2_MASK) TIMER2(); } } @@ -61,24 +59,60 @@ void irq(void) some other CPU frequency scaling. */ #ifndef BOOTLOADER +void flush_icache(void) ICODE_ATTR; +void flush_icache(void) +{ + intptr_t b, e; + + for (b = (intptr_t)&CACHE_FLUSH_BASE, e = b + CACHE_SIZE; + b < e; b += 16) { + outl(0x0, b); + } +} + +void invalidate_icache(void) ICODE_ATTR; +void invalidate_icache(void) +{ + intptr_t b, e; + + /* Flush */ + for (b = (intptr_t)&CACHE_FLUSH_BASE, e = b + CACHE_SIZE; + b < e; b += 16) { + outl(0x0, b); + } + + /* Invalidate */ + for (b = (intptr_t)&CACHE_INVALIDATE_BASE, e = b + CACHE_SIZE; + b < e; b += 16) { + outl(0x0, b); + } +} + static void ipod_init_cache(void) { - int i =0; + intptr_t b, e; + /* Initialising the cache in the iPod bootloader prevents Rockbox from starting */ - outl(inl(0xcf004050) & ~0x700, 0xcf004050); + PROC_STAT &= ~0x700; outl(0x4000, 0xcf004020); - outl(0x2, 0xcf004024); + CACHE_CTL = CACHE_INIT; - /* PP5002 has 8KB cache */ - for (i = 0xf0004000; i < (int)(0xf0006000); i += 16) { - outl(0x0, i); + for (b = (intptr_t)&CACHE_INVALIDATE_BASE, e = b + CACHE_SIZE; + b < e; b += 16) { + outl(0x0, b); } - outl(0x0, 0xf000f020); - outl(0x3fc0, 0xf000f024); + /* Cache if (addr & mask) >> 16 == (mask & match) >> 16: + * yes: 0x00000000 - 0x03ffffff + * no: 0x04000000 - 0x1fffffff + * yes: 0x20000000 - 0x23ffffff + * no: 0x24000000 - 0x3fffffff <= range containing uncached alias + */ + CACHE_MASK = 0x00001c00; + CACHE_OPERATION = 0x3fc0; - outl(0x3, 0xcf004024); + CACHE_CTL = CACHE_INIT | CACHE_RUN; } #ifdef HAVE_ADJUSTABLE_CPU_FREQ @@ -132,9 +166,10 @@ void system_init(void) #ifndef BOOTLOADER if (CURRENT_CORE == CPU) { - /* Remap the flash ROM from 0x00000000 to 0x20000000. */ - MMAP3_LOGICAL = 0x20000000 | 0x3a00; - MMAP3_PHYSICAL = 0x00000000 | 0x3f84; + /* Remap the flash ROM on CPU, keep hidden from COP: + * 0x00000000-0x03ffffff = 0x20000000-0x23ffffff */ + MMAP1_LOGICAL = 0x20003c00; + MMAP1_PHYSICAL = 0x00003f84; #if defined(IPOD_1G2G) || defined(IPOD_3G) DEV_EN = 0x0b9f; /* don't clock unused PP5002 hardware components */ @@ -150,7 +185,11 @@ void system_init(void) GPIOC_INT_EN = 0; GPIOD_INT_EN = 0; -#ifndef HAVE_ADJUSTABLE_CPU_FREQ +#ifdef HAVE_ADJUSTABLE_CPU_FREQ +#if NUM_CORES > 1 + cpu_boost_init(); +#endif +#else pp_set_cpu_frequency(CPUFREQ_MAX); #endif } diff --git a/firmware/target/arm/system-target.h b/firmware/target/arm/system-target.h index e9419b3f86..8dcbf0f9da 100644 --- a/firmware/target/arm/system-target.h +++ b/firmware/target/arm/system-target.h @@ -77,7 +77,7 @@ static inline unsigned int current_core(void) /* Return the actual ID instead of core index */ static inline unsigned int processor_id(void) { - unsigned char id; + unsigned int id; asm volatile ( "ldrb %0, [%1] \n" @@ -92,12 +92,18 @@ static inline unsigned int processor_id(void) /* All addresses within rockbox are in IRAM in the bootloader so are therefore uncached */ #define UNCACHED_ADDR(a) (a) -#else -#define UNCACHED_ADDR(a) \ - ((typeof (a))((uintptr_t)(a) | 0x10000000)) + +#else /* !BOOTLOADER */ + +#if CONFIG_CPU == PP5002 +#define UNCACHED_BASE_ADDR 0x28000000 +#else /* PP502x */ +#define UNCACHED_BASE_ADDR 0x10000000 #endif -#ifdef CPU_PP502x +#define UNCACHED_ADDR(a) \ + ((typeof (a))((uintptr_t)(a) | UNCACHED_BASE_ADDR)) +#endif /* BOOTLOADER */ /* Certain data needs to be out of the way of cache line interference * such as data for COP use or for use with UNCACHED_ADDR */ @@ -115,8 +121,6 @@ void invalidate_icache(void); void flush_icache(void); #endif -#endif /* CPU_PP502x */ - #endif /* CPU_PP */ #endif /* SYSTEM_TARGET_H */ diff --git a/firmware/thread.c b/firmware/thread.c index 126cc41c0f..37157be245 100644 --- a/firmware/thread.c +++ b/firmware/thread.c @@ -245,6 +245,20 @@ static int * const idle_stacks[NUM_CORES] NOCACHEDATA_ATTR = [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] NOCACHEBSS_ATTR; +#endif + #endif /* NUM_CORES */ #if CONFIG_CORELOCK == SW_CORELOCK @@ -391,10 +405,22 @@ void corelock_unlock(struct corelock *cl) * no other core requested a wakeup for it to perform a task. *--------------------------------------------------------------------------- */ -static inline void core_sleep(IF_COP(unsigned int core,) struct thread_entry **waking) +#if NUM_CORES == 1 +/* Shared single-core build debugging version */ +static inline void core_sleep(struct thread_entry **waking) +{ + set_interrupt_status(IRQ_FIQ_DISABLED, IRQ_FIQ_STATUS); + if (*waking == NULL) + { + PROC_CTL(CURRENT_CORE) = PROC_SLEEP; + nop; nop; nop; + } + set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS); +} +#elif defined (CPU_PP502x) +static inline void core_sleep(unsigned int core, + struct thread_entry **waking) { -#if NUM_CORES > 1 -#ifdef CPU_PP502x #if 1 /* Disabling IRQ and FIQ is important to making the fixed-time sequence * non-interruptable */ @@ -448,29 +474,83 @@ static inline void core_sleep(IF_COP(unsigned int core,) struct thread_entry **w /* Enable IRQ, FIQ */ set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS); #endif /* ASM/C selection */ -#else - /* TODO: PP5002 */ -#endif /* CONFIG_CPU == */ -#else +} +#elif CONFIG_CPU == PP5002 +/* PP5002 has no mailboxes - emulate using bytes */ +static inline void core_sleep(unsigned int core, + struct thread_entry **waking) +{ +#if 1 + asm volatile ( + "mrs r1, cpsr \n" /* Disable IRQ, FIQ */ + "orr r1, r1, #0xc0 \n" + "msr cpsr_c, r1 \n" + "mov r0, #1 \n" /* Signal intent to sleep */ + "strb r0, [%[sem], #2] \n" + "ldr r0, [%[waking]] \n" /* *waking == NULL? */ + "cmp r0, #0 \n" + "ldreqb r0, [%[sem], #1] \n" /* && stay_awake == 0? */ + "cmpeq r0, #0 \n" + "moveq r0, #0xca \n" /* Then sleep */ + "streqb r0, [%[ctl], %[c], lsl #2] \n" + "nop \n" /* nop's needed because of pipeline */ + "nop \n" + "nop \n" + "mov r0, #0 \n" /* Clear stay_awake and sleep intent */ + "strb r0, [%[sem], #1] \n" + "strb r0, [%[sem], #2] \n" + "1: \n" /* Wait for wake procedure to finish */ + "ldrb r0, [%[sem], #0] \n" + "cmp r0, #0 \n" + "bne 1b \n" + "bic r1, r1, #0xc0 \n" /* Enable interrupts */ + "msr cpsr_c, r1 \n" + : + : [sem]"r"(&core_semaphores[core]), [c]"r"(core), + [waking]"r"(waking), [ctl]"r"(&PROC_CTL(CPU)) + : "r0", "r1" + ); +#else /* C version for reference */ + /* Disable IRQ, FIQ */ set_interrupt_status(IRQ_FIQ_DISABLED, IRQ_FIQ_STATUS); - if (*waking == NULL) + + /* Signal intent to sleep */ + core_semaphores[core].intend_sleep = 1; + + /* Something waking or other processor intends to wake us? */ + if (*waking == NULL && core_semaphores[core].stay_awake == 0) { - PROC_CTL(IF_COP_CORE(core)) = PROC_SLEEP; + PROC_CTL(core) = PROC_SLEEP; /* Snooze */ + nop; nop; nop; } + + /* Signal wake - clear wake flag */ + core_semaphores[core].stay_awake = 0; + core_semaphores[core].intend_sleep = 0; + + /* Wait for other processor to finish wake procedure */ + while (core_semaphores[core].intend_wake != 0); + + /* Enable IRQ, FIQ */ set_interrupt_status(IRQ_FIQ_ENABLED, IRQ_FIQ_STATUS); -#endif /* NUM_CORES */ +#endif /* ASM/C selection */ } +#endif /* CPU type */ /*--------------------------------------------------------------------------- * Wake another processor core that is sleeping or prevent it from doing so * if it was already destined. FIQ, IRQ should be disabled before calling. *--------------------------------------------------------------------------- */ -void core_wake(IF_COP_VOID(unsigned int othercore)) -{ #if NUM_CORES == 1 +/* Shared single-core build debugging version */ +void core_wake(void) +{ /* No wakey - core already wakey */ +} #elif defined (CPU_PP502x) +void core_wake(unsigned int othercore) +{ #if 1 /* avoid r0 since that contains othercore */ asm volatile ( @@ -494,7 +574,8 @@ void core_wake(IF_COP_VOID(unsigned int othercore)) "str r1, [%[mbx], #8] \n" /* Done with wake procedure */ "msr cpsr_c, r3 \n" /* Restore int status */ : - : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), [oc]"r" (othercore) + : [ctl]"r"(&PROC_CTL(CPU)), [mbx]"r"(MBX_BASE), + [oc]"r"(othercore) : "r1", "r2", "r3"); #else /* C version for reference */ /* Disable interrupts - avoid reentrancy from the tick */ @@ -509,18 +590,68 @@ void core_wake(IF_COP_VOID(unsigned int othercore)) /* If sleeping, wake it up */ if (PROC_CTL(othercore) & PROC_SLEEP) - { PROC_CTL(othercore) = 0; - } /* Done with wake procedure */ MBX_MSG_CLR = 0x1 << othercore; set_irq_level(oldlevel); #endif /* ASM/C selection */ -#else - PROC_CTL(othercore) = PROC_WAKE; -#endif } +#elif CONFIG_CPU == PP5002 +/* PP5002 has no mailboxes - emulate using bytes */ +void core_wake(unsigned int othercore) +{ +#if 1 + /* avoid r0 since that contains othercore */ + asm volatile ( + "mrs r3, cpsr \n" /* Disable IRQ */ + "orr r1, r3, #0x80 \n" + "msr cpsr_c, r1 \n" + "mov r1, #1 \n" /* Signal intent to wake other core */ + "orr r1, r1, r1, lsl #8 \n" /* and set stay_awake */ + "strh r1, [%[sem], #0] \n" + "mov r2, #0x8000 \n" + "1: \n" /* If it intends to sleep, let it first */ + "ldrb r1, [%[sem], #2] \n" /* intend_sleep != 0 ? */ + "cmp r1, #1 \n" + "ldr r1, [%[st]] \n" /* && not sleeping ? */ + "tsteq r1, r2, lsr %[oc] \n" + "beq 1b \n" /* Wait for sleep or wake */ + "tst r1, r2, lsr %[oc] \n" + "ldrne r2, =0xcf004054 \n" /* If sleeping, wake it */ + "movne r1, #0xce \n" + "strneb r1, [r2, %[oc], lsl #2] \n" + "mov r1, #0 \n" /* Done with wake procedure */ + "strb r1, [%[sem], #0] \n" + "msr cpsr_c, r3 \n" /* Restore int status */ + : + : [sem]"r"(&core_semaphores[othercore]), + [st]"r"(&PROC_STAT), + [oc]"r"(othercore) + : "r1", "r2", "r3" + ); +#else /* C version for reference */ + /* Disable interrupts - avoid reentrancy from the tick */ + int oldlevel = set_irq_level(HIGHEST_IRQ_LEVEL); + + /* Signal intent to wake other processor - set stay awake */ + core_semaphores[othercore].intend_wake = 1; + core_semaphores[othercore].stay_awake = 1; + + /* If it intends to sleep, wait until it does or aborts */ + while (core_semaphores[othercore].intend_sleep != 0 && + (PROC_STAT & PROC_SLEEPING(othercore)) == 0); + + /* If sleeping, wake it up */ + if (PROC_STAT & PROC_SLEEPING(othercore)) + PROC_CTL(othercore) = PROC_WAKE; + + /* Done with wake procedure */ + core_semaphores[othercore].intend_wake = 0; + set_irq_level(oldlevel); +#endif /* ASM/C selection */ +} +#endif /* CPU type */ #if NUM_CORES > 1 /*--------------------------------------------------------------------------- @@ -2539,10 +2670,13 @@ void init_threads(void) #if NUM_CORES > 1 /* This code path will not be run on single core targets */ /* TODO: HAL interface for this */ /* Wake up coprocessor and let it initialize kernel and threads */ +#ifdef CPU_PP502x MBX_MSG_CLR = 0x3f; +#endif COP_CTL = PROC_WAKE; /* Sleep until finished */ CPU_CTL = PROC_SLEEP; + nop; nop; nop; nop; } else { -- cgit v1.2.3