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

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
     {