2 files changed, 20 insertions, 10 deletions

diff --git a/firmware/system.c b/firmware/system.c
index 7e7effe67d..921a7d2314 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -276,16 +276,6 @@ void irq(void)
 #endif
 #endif /* BOOTLOADER */
 
-unsigned int current_core(void)
-{
-    if((PROCESSOR_ID & 0xff) == PROC_ID_CPU)
-    {
-        return CPU;
-    }
-    return COP;
-}
-
-
 /* TODO: The following two function have been lifted straight from IPL, and
    hence have a lot of numeric addresses used straight. I'd like to use
    #defines for these, but don't know what most of them are for or even what
diff --git a/firmware/target/arm/system-target.h b/firmware/target/arm/system-target.h
index 2b72e9293e..0dd02a8704 100644
--- a/firmware/target/arm/system-target.h
+++ b/firmware/target/arm/system-target.h
@@ -36,7 +36,27 @@ static inline void udelay(unsigned usecs)
     while (TIME_BEFORE(USEC_TIMER, stop));
 }
 
+#if CONFIG_CPU == PP5020 || CONFIG_CPU == PP5024
+static inline unsigned int current_core(void)
+{
+    /*
+     * PROCESSOR_ID seems to be 32-bits:
+     * CPU = 0x55555555 = |01010101|01010101|01010101|01010101|
+     * COP = 0xaaaaaaaa = |10101010|10101010|10101010|10101010|
+     *                                                ^
+     */
+    unsigned int core;
+    asm volatile (
+        "ldr    %0, =0x60000000 \r\n" /* PROCESSOR_ID      */
+        "ldrb   %0, [%0]        \r\n" /* Just load the LSB */
+        "mov    %0, %0, lsr #7  \r\n" /* Bit 7 => index    */
+        : "=&r"(core)                 /* CPU=0, COP=1      */
+    );
+    return core;
+}
+#else
 unsigned int current_core(void);
+#endif
 
 #if CONFIG_CPU != PP5002