2 files changed, 60 insertions, 27 deletions

diff --git a/firmware/export/pp5020.h b/firmware/export/pp5020.h
index 073d33e494..0d2ffc4d94 100644
--- a/firmware/export/pp5020.h
+++ b/firmware/export/pp5020.h
@@ -584,8 +584,10 @@
 #define CACHE_DATA_BASE         (*(volatile unsigned long*)(0xf0000000))
 /* 0xf0002000-0xf0003fff */
 #define CACHE_DATA_MIRROR_BASE  (*(volatile unsigned long*)(0xf0002000))
-/* 0xf0004000-0xf0007fff */
-#define CACHE_STATUS_BASE       (*(volatile unsigned long*)(0xf0004000))
+/* 0xf0004000-0xf0005fff */
+#define CACHE_STATUS_BASE_CPU   (*(volatile unsigned long*)(0xf0004000))
+/* 0xf0006000-0xf0007fff */
+#define CACHE_STATUS_BASE_COP   (*(volatile unsigned long*)(0xf0006000))
 #define CACHE_FLUSH_BASE        (*(volatile unsigned long*)(0xf0008000))
 #define CACHE_INVALID_BASE      (*(volatile unsigned long*)(0xf000c000))
 #define MMAP_PHYS_READ_MASK     0x0100
diff --git a/firmware/target/arm/pp/system-pp502x.c b/firmware/target/arm/pp/system-pp502x.c
index ad0577c38f..686e2ac3bd 100644
--- a/firmware/target/arm/pp/system-pp502x.c
+++ b/firmware/target/arm/pp/system-pp502x.c
@@ -29,6 +29,21 @@
 #include "button-target.h"
 #include "usb_drv.h"
 
+/* Bit 0 - 20: Cached Address */
+#define CACHE_ADDRESS_MASK ((1<<21)-1)
+/* Bit 22: Cache line dirty */
+#define CACHE_LINE_DIRTY    (1<<22)
+/* Bit 23: Cache line valid */
+#define CACHE_LINE_VALID    (1<<23)
+/* Cache Size - 8K*/
+#define CACHE_SIZE       0x2000
+
+/*Initial memory address used to prime cache
+ * could be targeted to a more 'important' address
+ * Note:  Don't start at 0x0, as the compiler thinks it's a
+ * null pointer dereference and will helpfully blow up the code. */
+#define CACHED_INIT_ADDR CACHEALIGN_UP(0x2000)
+
 #if !defined(BOOTLOADER) || defined(HAVE_BOOTLOADER_USB_MODE)
 extern void TIMER1(void);
 extern void TIMER2(void);
@@ -44,7 +59,7 @@ unsigned char probed_ramsize;
 
 void __attribute__((interrupt("IRQ"))) irq_handler(void)
 {
-    if(CURRENT_CORE == CPU)
+    if(IF_COP_CORE(CPU) == CPU)
     {
         if (CPU_INT_STAT & TIMER1_MASK) {
             TIMER1();
@@ -218,6 +233,39 @@ void ICODE_ATTR commit_dcache(void)
     }
 }
 
+static void ICODE_ATTR cache_invalidate_special(void)
+{
+    /* Cache lines which are not marked as valid can cause memory
+     * corruption when there are many writes to and code fetches from
+     * cached memory. This workaround points all cache status to the
+     * maximum line address and marked valid but not dirty. Since that area
+     * is never accessed, the cache lines don't affect anything, and
+     * they're effectively discarded. Interrupts must be disabled here
+     * because any change they make to cached memory could be discarded.
+     *  A status word is 32 bits and is mirrored four times for each cache line
+        bit 0-20        line_address >> 11
+        bit 21          unused?
+        bit 22          line_dirty
+        bit 23          line_valid
+        bit 24-31       unused?
+     */
+    register volatile unsigned long *p;
+    if (IF_COP_CORE(CPU) == CPU)
+    {
+        for (p = &CACHE_STATUS_BASE_CPU;
+             p < (&CACHE_STATUS_BASE_CPU) + CACHE_SIZE;
+             p += CACHEALIGN_BITS) /* sizeof(p) * CACHEALIGN_BITS */
+            *p = CACHE_LINE_VALID | CACHE_ADDRESS_MASK;
+    }
+    else
+    {
+        for (p = &CACHE_STATUS_BASE_COP;
+             p < (&CACHE_STATUS_BASE_COP) + CACHE_SIZE;
+             p += CACHEALIGN_BITS)
+            *p = CACHE_LINE_VALID | CACHE_ADDRESS_MASK;
+    }
+}
+
 void ICODE_ATTR commit_discard_idcache(void)
 {
     if (CACHE_CTL & CACHE_CTL_ENABLE)
@@ -225,21 +273,7 @@ void ICODE_ATTR commit_discard_idcache(void)
         register int istat = disable_interrupt_save(IRQ_FIQ_STATUS);
 
         commit_dcache();
-
-        /* Cache lines which are not marked as valid can cause memory
-         * corruption when there are many writes to and code fetches from
-         * cached memory. This workaround points all cache status words past
-         * end of RAM and marks them as valid, but not dirty. Since that area
-         * is never accessed, the cache lines don't affect anything, and
-         * they're effectively discarded. Interrupts must be disabled here
-         * because any change they make to cached memory could be discarded.
-         */
-
-        register volatile unsigned long *p;
-        for (p = &CACHE_STATUS_BASE;
-             p < (&CACHE_STATUS_BASE) + 512*16/sizeof(*p);
-             p += 16/sizeof(*p))
-            *p = ((MEMORYSIZE*0x100000) >> 11) | 0x800000;
+        cache_invalidate_special();
 
         restore_interrupt(istat);
     }
@@ -258,7 +292,7 @@ static void init_cache(void)
 
 #ifndef BOOTLOADER
     /* what's this do? */
-    CACHE_PRIORITY |= CURRENT_CORE == CPU ? 0x10 : 0x20;
+    CACHE_PRIORITY |= IF_COP_CORE(CPU) == CPU ? 0x10 : 0x20;
 #endif
 
     /* Cache if (addr & mask) >> 16 == (mask & match) >> 16:
@@ -278,11 +312,8 @@ static void init_cache(void)
      * can run from cached RAM, rewriting of cache status words may not
      * be safe and the cache is filled instead by reading. */
 
-    /* Note:  Don't start at 0x0, as the compiler thinks it's a
-       null pointer dereference and will helpfully blow up the code. */
-
-    register volatile char *p;
-    for (p = (volatile char *)0x1000; p < (volatile char *)0x3000; p += 0x10)
+    register volatile char *p = (volatile char *)CACHED_INIT_ADDR;
+    for (;p < (volatile char *)CACHED_INIT_ADDR + CACHE_SIZE; p += CACHEALIGN_SIZE)
         (void)*p;
 }
 #endif /* BOOTLOADER || HAVE_BOOTLOADER_USB_MODE */
@@ -293,7 +324,7 @@ static void init_cache(void)
 void scale_suspend_core(bool suspend) ICODE_ATTR;
 void scale_suspend_core(bool suspend)
 {
-    unsigned int core = CURRENT_CORE;
+    unsigned int core = IF_COP_CORE(CPU);
     IF_COP( unsigned int othercore = 1 - core; )
     static int oldstatus IBSS_ATTR;
 
@@ -442,7 +473,7 @@ static void pp_set_cpu_frequency(long frequency)
 #ifndef BOOTLOADER
 void system_init(void)
 {
-    if (CURRENT_CORE == CPU)
+    if (IF_COP_CORE(CPU) == CPU)
     {
 #if defined (IRIVER_H10) || defined(IRIVER_H10_5GB) || defined(IPOD_COLOR)
         /* set minimum startup configuration */
@@ -628,7 +659,7 @@ void system_exception_wait(void)
     COP_INT_DIS = -1;
 
     /* Halt */
-    PROC_CTL(CURRENT_CORE) = 0x40000000;
+    PROC_CTL(IF_COP_CORE(CPU)) = 0x40000000;
     while (1);
 }