16 files changed, 187 insertions, 128 deletions

diff --git a/firmware/rolo.c b/firmware/rolo.c
index de19c8e925..e60af46704 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -201,7 +201,7 @@ void rolo_restart(const unsigned char* source, unsigned char* dest,
         : : "r"(dest)
     );
 #elif defined(CPU_MIPS)
-    __dcache_writeback_all();
+    commit_discard_idcache();
     asm volatile(
         "jr     %0               \n"
         : : "r"(dest)
diff --git a/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c b/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c
index a582db82cc..5f320f8e9b 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c
@@ -151,7 +151,7 @@ static void jz_nand_write_dma(void *source, unsigned int len, int bw)
     mutex_lock(&nand_dma_mtx);
 
     if(((unsigned int)source < 0xa0000000) && len)
-         dma_cache_wback_inv((unsigned long)source, len);
+         commit_discard_dcache_range(source, len);
 
     dma_enable();
 
@@ -184,7 +184,7 @@ static void jz_nand_read_dma(void *target, unsigned int len, int bw)
     mutex_lock(&nand_dma_mtx);
 
     if(((unsigned int)target < 0xa0000000) && len)
-        dma_cache_wback_inv((unsigned long)target, len);
+        discard_dcache_range(target, len);
 
     dma_enable();
 
diff --git a/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4760.c b/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4760.c
index 1eacf9170a..efce5742d0 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4760.c
@@ -150,7 +150,7 @@ static void jz_nand_write_dma(void *source, unsigned int len, int bw)
     mutex_lock(&nand_dma_mtx);
 
     if(((unsigned int)source < 0xa0000000) && len)
-         dma_cache_wback_inv((unsigned long)source, len);
+         commit_discard_dcache_range(source, len);
 
     dma_enable();
 
@@ -183,7 +183,7 @@ static void jz_nand_read_dma(void *target, unsigned int len, int bw)
     mutex_lock(&nand_dma_mtx);
 
     if(((unsigned int)target < 0xa0000000) && len)
-        dma_cache_wback_inv((unsigned long)target, len);
+        discard_dcache_range(target, len);
 
     dma_enable();
 
diff --git a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c
index 0eb175c03f..56dd50814a 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c
@@ -417,7 +417,7 @@ static void jz_sd_receive_data_dma(struct sd_request *req)
 #endif
 
     /* flush dcache */
-    //dma_cache_wback_inv((unsigned long) req->buffer, size);
+    discard_dcache_range(req->buffer, size);
     /* setup dma channel */
     REG_DMAC_DSAR(DMA_SD_RX_CHANNEL) = PHYSADDR(MSC_RXFIFO);    /* DMA source addr */
     REG_DMAC_DTAR(DMA_SD_RX_CHANNEL) = PHYSADDR((unsigned long) req->buffer);    /* DMA dest addr */
@@ -452,7 +452,7 @@ static void jz_mmc_transmit_data_dma(struct mmc_request *req)
 #endif
 
     /* flush dcache */
-    //dma_cache_wback_inv((unsigned long) req->buffer, size);
+    commit_discard_dcache_range(req->buffer, size);
     /* setup dma channel */
     REG_DMAC_DSAR(DMA_SD_TX_CHANNEL) = PHYSADDR((unsigned long) req->buffer);    /* DMA source addr */
     REG_DMAC_DTAR(DMA_SD_TX_CHANNEL) = PHYSADDR(MSC_TXFIFO);    /* DMA dest addr */
diff --git a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c
index 55ffecce09..1960fcbd35 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4760.c
@@ -532,6 +532,9 @@ static int jz_sd_transmit_data(const int drive, struct sd_request *req)
 #if SD_DMA_ENABLE
 static int jz_sd_receive_data_dma(const int drive, struct sd_request *req)
 {
+    /* flush dcache */
+    discard_dcache_range(req->buffer, req->cnt);
+
     /* setup dma channel */
     REG_DMAC_DCCSR(DMA_SD_RX_CHANNEL(drive)) = 0;
     REG_DMAC_DSAR(DMA_SD_RX_CHANNEL(drive)) = PHYSADDR(MSC_RXFIFO(MSC_CHN(drive))); /* DMA source addr */
@@ -558,16 +561,13 @@ static int jz_sd_receive_data_dma(const int drive, struct sd_request *req)
     /* clear status and disable channel */
     REG_DMAC_DCCSR(DMA_SD_RX_CHANNEL(drive)) = 0;
 
-    /* flush dcache */
-    dma_cache_wback_inv((unsigned long) req->buffer, req->cnt);
-
     return SD_NO_ERROR;
 }
 
 static int jz_sd_transmit_data_dma(const int drive, struct sd_request *req)
 {
     /* flush dcache */
-    dma_cache_wback_inv((unsigned long) req->buffer, req->cnt);
+    commit_discard_dcache_range(req->buffer, req->cnt);
 
     /* setup dma channel */
     REG_DMAC_DCCSR(DMA_SD_TX_CHANNEL(drive)) = 0;
diff --git a/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4740.c b/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4740.c
index 6f317f7b3f..f4f363b25b 100644
--- a/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4740.c
@@ -32,7 +32,7 @@ void memset(void *target, unsigned char c, size_t len)
     else
     {
         if(((unsigned int)target < 0xa0000000) && len)
-             dma_cache_wback_inv((unsigned long)target, len);
+             discard_dcache_range(target, len);
 
         dp = (unsigned char *)((unsigned int)(&d) | 0xa0000000);
         *(dp + 0) = c;
@@ -52,7 +52,6 @@ void memset(void *target, unsigned char c, size_t len)
             dp = (unsigned char *)((unsigned int)target + (len & (32 - 1)));
             for(d = 0;d < (len % 32); d++)
                 *dp++ = c;
-
         }
     }
 }
@@ -68,7 +67,7 @@ void memset16(void *target, unsigned short c, size_t len)
     else
     {
         if(((unsigned int)target < 0xa0000000) && len)
-             dma_cache_wback_inv((unsigned long)target, len);
+             discard_dcache_range(target, len);
 
         d = c;
         REG_DMAC_DSAR(ch) = PHYSADDR((unsigned long)&d);
@@ -97,10 +96,10 @@ void memcpy(void *target, const void *source, size_t len)
         _memcpy(target, source, len);
 
     if(((unsigned int)source < 0xa0000000) && len)
-        dma_cache_wback_inv((unsigned long)source, len);
+        commit_dcache_range(source, len);
 
     if(((unsigned int)target < 0xa0000000) && len)
-        dma_cache_wback_inv((unsigned long)target, len);
+        discard_dcache_range(target, len);
 
     REG_DMAC_DSAR(ch) = PHYSADDR((unsigned long)source);
     REG_DMAC_DTAR(ch) = PHYSADDR((unsigned long)target);
diff --git a/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4760.c b/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4760.c
index 4cdea2ad08..87d2b4e210 100644
--- a/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/dma_acc-jz4760.c
@@ -29,7 +29,7 @@ void memset_dma(void *target, int c, size_t len, unsigned int bits)
     unsigned char *dp;
 
     if(((unsigned int)target < 0xa0000000) && len)
-         dma_cache_wback_inv((unsigned long)target, len);
+         discard_dcache_range(target, len);
 
     dp = (unsigned char *)((unsigned int)(&d) | 0xa0000000);
     *(dp + 0) = c;
@@ -68,14 +68,14 @@ void memset_dma(void *target, int c, size_t len, unsigned int bits)
 void memcpy_dma(void *target, const void *source, size_t len, unsigned int bits)
 {
     if(((unsigned int)source < 0xa0000000) && len)
-        dma_cache_wback_inv((unsigned long)source, len);
-    
+        commit_dcache_range(source, len);
+
     if(((unsigned int)target < 0xa0000000) && len)
-        dma_cache_wback_inv((unsigned long)target, len);
-    
+        discard_dcache_range(target, len);
+
     REG_MDMAC_DCCSR(MDMA_CHANNEL) = 0;
     REG_MDMAC_DSAR(MDMA_CHANNEL) = PHYSADDR((unsigned long)source);
-    REG_MDMAC_DTAR(MDMA_CHANNEL) = PHYSADDR((unsigned long)target); 
+    REG_MDMAC_DTAR(MDMA_CHANNEL) = PHYSADDR((unsigned long)target);
     REG_MDMAC_DRSR(MDMA_CHANNEL) = DMAC_DRSR_RS_AUTO;
     switch (bits)
     {
diff --git a/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c b/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
index cf676622f1..a2d5b73ea8 100644
--- a/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
@@ -109,8 +109,9 @@ void lcd_update_rect(int x, int y, int width, int height)
     REG_DMAC_DCMD(DMA_LCD_CHANNEL)  = ( DMAC_DCMD_SAI     | DMAC_DCMD_RDIL_IGN | DMAC_DCMD_SWDH_32
                                       | DMAC_DCMD_DWDH_16 | DMAC_DCMD_DS_16BYTE );
 
-    __dcache_writeback_all(); /* Size of framebuffer is way bigger than cache size.
-                                 We need to find a way to make the framebuffer uncached, so this statement can get removed. */
+    // XXX range
+    commit_discard_dcache(); /* Size of framebuffer is way bigger than cache size.
+                                We need to find a way to make the framebuffer uncached, so this statement can get removed. */
 
     while(REG_SLCD_STATE & SLCD_STATE_BUSY);
     REG_SLCD_CTRL |= SLCD_CTRL_DMA_EN; /* Enable SLCD DMA support */
@@ -174,7 +175,7 @@ void lcd_blit_yuv(unsigned char * const src[3],
     yuv_src[1] = src[1] + (z >> 2) + (src_x >> 1);
     yuv_src[2] = src[2] + (yuv_src[1] - src[1]);
 
-    __dcache_writeback_all();
+    commit_discard_dcache(); // XXX range
 
     __cpm_start_ipu();
 
diff --git a/firmware/target/mips/ingenic_jz47xx/pcm-jz4740.c b/firmware/target/mips/ingenic_jz47xx/pcm-jz4740.c
index 83d3646ed1..00a2b22591 100644
--- a/firmware/target/mips/ingenic_jz47xx/pcm-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/pcm-jz4740.c
@@ -69,6 +69,8 @@ static inline void set_dma(const void *addr, size_t size)
     int burst_size;
     logf("%x %d %x", (unsigned int)addr, size, REG_AIC_SR);
 
+    commit_discard_dcache_range(addr, size);
+
     if(size % 16)
     {
         if(size % 4)
@@ -88,7 +90,6 @@ static inline void set_dma(const void *addr, size_t size)
         burst_size = DMAC_DCMD_DS_16BYTE;
     }
 
-    __dcache_writeback_all();
     REG_DMAC_DCCSR(DMA_AIC_TX_CHANNEL) = DMAC_DCCSR_NDES;
     REG_DMAC_DSAR(DMA_AIC_TX_CHANNEL)  = PHYSADDR((unsigned long)addr);
     REG_DMAC_DTAR(DMA_AIC_TX_CHANNEL)  = PHYSADDR((unsigned long)AIC_DR);
diff --git a/firmware/target/mips/ingenic_jz47xx/pcm-jz4760.c b/firmware/target/mips/ingenic_jz47xx/pcm-jz4760.c
index 39df037f76..59b086e4f8 100644
--- a/firmware/target/mips/ingenic_jz47xx/pcm-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/pcm-jz4760.c
@@ -28,7 +28,6 @@
 #include "pcm-internal.h"
 #include "cpu.h"
 
-
 /****************************************************************************
  ** Playback DMA transfer
  **/
@@ -60,8 +59,8 @@ static inline void set_dma(const void *addr, size_t size)
     int burst_size;
     logf("%x %d %x", (unsigned int)addr, size, REG_AIC_SR);
 
-    dma_cache_wback_inv((unsigned long)addr, size);
-    
+    commit_discard_dcache_range(addr, size);
+
     if(size % 16)
     {
         if(size % 4)
diff --git a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
index 87094dd7ae..d3a753a58e 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
@@ -511,24 +511,23 @@ static void sdram_init(void)
 void ICODE_ATTR system_main(void)
 {
     int i;
-       
-    __dcache_writeback_all();
-    __icache_invalidate_all();
-    
+
+    commit_discard_idcache();
+
     write_c0_status(1 << 28 | 1 << 10 ); /* Enable CP | Mask interrupt 2 */
-    
+
     /* Disable all interrupts */
     for(i=0; i<IRQ_MAX; i++)
         dis_irq(i);
-    
+
     mmu_init();
     pll_init();
     sdram_init();
-    
+
     /* Disable unneeded clocks, clocks are enabled when needed */
     __cpm_stop_all();
     __cpm_suspend_usbhost();
-    
+
     /* Enable interrupts at core level */
     enable_interrupt();
 }
diff --git a/firmware/target/mips/ingenic_jz47xx/system-jz4760.c b/firmware/target/mips/ingenic_jz47xx/system-jz4760.c
index ff87e5ad9e..eee767c5ca 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/system-jz4760.c
@@ -673,8 +673,7 @@ void ICODE_ATTR system_main(void)
 {
     int i;
 
-    __dcache_writeback_all();
-    __icache_invalidate_all();
+    commit_discard_idcache();
 
     write_c0_status(1 << 28 | 1 << 10 ); /* Enable CP | Mask interrupt 2 */
 
diff --git a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
index e8cd2033ff..fd38b2b1a4 100644
--- a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
@@ -274,7 +274,7 @@ static void EPIN_handler(unsigned int endpoint)
     }
 
     logf("EP%d: %d -> %d", endpoint, ep->sent, ep->length);
-    
+
     if(ep->sent == 0)
         length = MIN(ep->length, ep->fifo_size);
     else
@@ -365,7 +365,7 @@ static void EPDMA_handler(int number)
         /* Disable DMA */
         REG_USB_REG_CNTL2 = 0;
 
-        __dcache_invalidate_all();
+        commit_discard_dcache(); // XXX range?
 
         select_endpoint(endpoint);
         /* Read out last packet manually */
@@ -707,8 +707,7 @@ static void usb_drv_send_internal(struct usb_endpoint* ep, void* ptr, int length
     {
         if(ep->use_dma)
         {
-            //dma_cache_wback_inv((unsigned long)ptr, length);
-            __dcache_writeback_all();
+            commit_discard_dcache_range(ptr, length);
             REG_USB_REG_ADDR1 = PHYSADDR((unsigned long)ptr);
             REG_USB_REG_COUNT1 = length;
             REG_USB_REG_CNTL1 = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 |
@@ -767,8 +766,7 @@ int usb_drv_recv(int endpoint, void* ptr, int length)
         ep->busy = true;
         if(ep->use_dma)
         {
-            //dma_cache_wback_inv((unsigned long)ptr, length);
-            __dcache_writeback_all();
+            discard_dcache_range(ptr, length);
             REG_USB_REG_ADDR2 = PHYSADDR((unsigned long)ptr);
             REG_USB_REG_COUNT2 = length;
             REG_USB_REG_CNTL2 = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 |
diff --git a/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c b/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c
index 3c7bb80f2c..275fd3fd2b 100644
--- a/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c
@@ -360,7 +360,7 @@ static void EPIN_handler(unsigned int endpoint)
     }
 
     logf("EP%d: %d -> %d", endpoint, ep->sent, ep->length);
-    
+
     if(ep->sent == 0)
         length = MIN(ep->length, ep->fifo_size);
     else
@@ -456,7 +456,7 @@ static void EPDMA_handler(int number)
         /* Disable DMA */
         REG_USB_CNTL(1) = 0;
 
-        __dcache_invalidate_all();
+        commit_discard_dcache(); // XXX range?
 
         select_endpoint(endpoint);
         /* Read out last packet manually */
@@ -846,8 +846,7 @@ static void usb_drv_send_internal(struct usb_endpoint* ep, void* ptr, int length
     {
         if(ep->use_dma)
         {
-            //dma_cache_wback_inv((unsigned long)ptr, length);
-            __dcache_writeback_all();
+            commit_discard_dcache_range(ptr, length);
             REG_USB_ADDR(0) = PHYSADDR((unsigned long)ptr);
             REG_USB_COUNT(0) = length;
             REG_USB_CNTL(0) = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 |
@@ -921,8 +920,7 @@ int usb_drv_recv(int endpoint, void* ptr, int length)
     ep->busy = true;
     if(ep->use_dma)
     {
-        //dma_cache_wback_inv((unsigned long)ptr, length);
-        __dcache_writeback_all();
+        discard_dcache_range(ptr, length);
         REG_USB_ADDR(1) = PHYSADDR((unsigned long)ptr);
         REG_USB_COUNT(1) = length;
         REG_USB_CNTL(1) = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 |
diff --git a/firmware/target/mips/mmu-mips.c b/firmware/target/mips/mmu-mips.c
index 552348014e..14a013211d 100644
--- a/firmware/target/mips/mmu-mips.c
+++ b/firmware/target/mips/mmu-mips.c
@@ -5,9 +5,9 @@
  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
  *                     \/            \/     \/    \/            \/
- * $Id$
  *
  * Copyright (C) 2009 by Maurus Cuelenaere
+ * Copyright (C) 2015 by Marcin Bukat
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -25,8 +25,16 @@
 #include "system.h"
 #include "mmu-mips.h"
 
+#if CONFIG_CPU == JZ4732 || CONFIG_CPU == JZ4760B
+/* XBurst core has 32 JTLB entries */
+#define NR_TLB_ENTRIES  32
+#else
+#error please define NR_TLB_ENTRIES
+#endif
+
 #define BARRIER                            \
     __asm__ __volatile__(                  \
+    "    .set    push               \n"    \
     "    .set    noreorder          \n"    \
     "    nop                        \n"    \
     "    nop                        \n"    \
@@ -34,7 +42,7 @@
     "    nop                        \n"    \
     "    nop                        \n"    \
     "    nop                        \n"    \
-    "    .set    reorder            \n");
+    "    .set    pop                \n");
 
 #define DEFAULT_PAGE_SHIFT       PL_4K
 #define DEFAULT_PAGE_MASK        PM_4K
@@ -43,6 +51,7 @@
 #define VPN2_SHIFT               S_EntryHiVPN2
 #define PFN_SHIFT                S_EntryLoPFN
 #define PFN_MASK                 0xffffff
+
 static void local_flush_tlb_all(void)
 {
     unsigned long old_ctx;
@@ -55,10 +64,11 @@ static void local_flush_tlb_all(void)
     write_c0_entrylo1(0);
     BARRIER;
 
-    /* Blast 'em all away. */
-    for(entry = 0; entry < 32; entry++)
+    /* blast all entries except the wired one */
+    for(entry = read_c0_wired(); entry < NR_TLB_ENTRIES; entry++)
     {
-        /* Make sure all entries differ. */
+        /* Make sure all entries differ and are in unmapped space, making them
+         * impossible to match */
         write_c0_entryhi(UNIQUE_ENTRYHI(entry, DEFAULT_PAGE_SHIFT));
         write_c0_index(entry);
         BARRIER;
@@ -119,84 +129,133 @@ void mmu_init(void)
     write_c0_framemask(0);
 
     local_flush_tlb_all();
-/*
-    map_address(0x80000000, 0x80000000, 0x4000, K_CacheAttrC);
-    map_address(0x80004000, 0x80004000, MEMORYSIZE * 0x100000, K_CacheAttrC);
-*/
 }
 
-#define SYNC_WB() __asm__ __volatile__ ("sync")
+/* Target specific operations:
+ * - invalidate BTB (Branch Table Buffer)
+ * - sync barrier after cache operations */
+#if CONFIG_CPU == JZ4732 || CONFIG_CPU == JZ4760B
+#define INVALIDATE_BTB()                     \
+do {                                         \
+        unsigned long tmp;                   \
+        __asm__ __volatile__(                \
+        "    .set push            \n"        \
+        "    .set noreorder       \n"        \
+        "    .set mips32          \n"        \
+        "    mfc0 %0, $16, 7      \n"        \
+        "    nop                  \n"        \
+        "    ori  %0, 2           \n"        \
+        "    mtc0 %0, $16, 7      \n"        \
+        "    nop                  \n"        \
+        "    .set pop             \n"        \
+        : "=&r"(tmp));                       \
+    } while (0)
+
+#define SYNC_WB() __asm__ __volatile__ ("sync":::"memory")
+#else /* !JZ4732 */
+#define INVALIDATE_BTB() do { } while(0)
+#define SYNC_WB() do { } while(0)
+#endif /* CONFIG_CPU */
+
+#define __CACHE_OP(op, addr)                 \
+    __asm__ __volatile__(                    \
+    "    .set    push\n\t         \n"        \
+    "    .set    noreorder        \n"        \
+    "    .set    mips32\n\t       \n"        \
+    "    cache   %0, %1           \n"        \
+    "    .set    pop              \n"        \
+    :                                        \
+    : "i" (op), "m"(*(unsigned char *)(addr)))
 
-#define cache_op(base,op)                       \
-        __asm__ __volatile__("                  \
-                .set noreorder;                 \
-                .set mips3;                     \
-                cache %1, (%0);                 \
-                .set mips0;                     \
-                .set reorder"                   \
-                :                               \
-                : "r" (base),                   \
-                  "i" (op));
+/* rockbox cache api */
 
-void __icache_invalidate_all(void)
+/* Writeback whole D-cache
+ * Alias to commit_discard_dcache() as there is no index type
+ * variant of writeback-only operation
+ */
+void commit_dcache(void) __attribute__((alias("commit_discard_dcache")));
+
+/* Writeback whole D-cache and invalidate D-cache lines */
+void commit_discard_dcache(void)
 {
-    unsigned long start;
-    unsigned long end;
+    unsigned int i;
+
+    /* Use index type operation and iterate whole cache */
+    for (i=A_K0BASE; i<A_K0BASE+CACHE_SIZE; i+=CACHEALIGN_SIZE)
+        __CACHE_OP(DCIndexWBInv, i);
 
-    start = A_K0BASE;
-    end = start + CACHE_SIZE;
-    while(start < end)
-    {
-        cache_op(start,ICIndexInv);
-        start += CACHE_LINE_SIZE;
-    }
     SYNC_WB();
 }
 
-void __dcache_invalidate_all(void)
+/* Writeback lines of D-cache corresponding to address range and
+ * invalidate those D-cache lines
+ */
+void commit_discard_dcache_range(const void *base, unsigned int size)
 {
-    unsigned long start;
-    unsigned long end;
+    char *s;
+
+    for (s=(char *)base; s<(char *)base+size; s+=CACHEALIGN_SIZE)
+        __CACHE_OP(DCHitWBInv, s);
 
-    start = A_K0BASE;
-    end = start + CACHE_SIZE;
-    while (start < end)
-    {
-        cache_op(start,DCIndexWBInv);
-        start += CACHE_LINE_SIZE;
-    }
     SYNC_WB();
 }
 
-void __idcache_invalidate_all(void)
+/* Writeback lines of D-cache corresponding to address range
+ */
+void commit_dcache_range(const void *base, unsigned int size)
 {
-    __dcache_invalidate_all();
-    __icache_invalidate_all();
+    char *s;
+
+    for (s=(char *)base; s<(char *)base+size; s+=CACHEALIGN_SIZE)
+        __CACHE_OP(DCHitWB, s);
+
+    SYNC_WB();
 }
 
-void __dcache_writeback_all(void)
+/* Invalidate D-cache lines corresponding to address range
+ * WITHOUT writeback
+ */
+void discard_dcache_range(const void *base, unsigned int size)
 {
-    __dcache_invalidate_all();
+    char *s;
+
+    if (((int)base & CACHEALIGN_SIZE - 1) ||
+        (((int)base + size) & CACHEALIGN_SIZE - 1)) {
+            /* Overlapping sections, so we need to write back instead */
+            commit_discard_dcache_range(base, size);
+            return;
+    };
+
+    for (s=(char *)base; s<(char *)base+size; s+=CACHEALIGN_SIZE)
+        __CACHE_OP(DCHitInv, s);
+
+    SYNC_WB();
 }
 
-void dma_cache_wback_inv(unsigned long addr, unsigned long size)
+/* Invalidate whole I-cache */
+static void discard_icache(void)
 {
-    unsigned long end, a;
+    unsigned int i;
 
-    if (size >= CACHE_SIZE*2) {
-        __dcache_writeback_all();
-    }
-    else {
-        unsigned long dc_lsize = CACHE_LINE_SIZE;
-
-        a = addr & ~(dc_lsize - 1);
-        end = (addr + size - 1) & ~(dc_lsize - 1);
-        while (1) {
-            cache_op(a,DCHitWBInv);
-            if (a == end)
-                break;
-            a += dc_lsize;
-        }
-    }
-    SYNC_WB();
+    asm volatile (".set   push       \n"
+                  ".set   noreorder  \n"
+                  ".set   mips32     \n"
+                  "mtc0   $0, $28    \n" /* TagLo */
+                  "mtc0   $0, $29    \n" /* TagHi */
+                  ".set   pop        \n"
+                  );
+    /* Use index type operation and iterate whole cache */
+    for (i=A_K0BASE; i<A_K0BASE+CACHE_SIZE; i+=CACHEALIGN_SIZE)
+        __CACHE_OP(ICIndexStTag, i);
+
+    INVALIDATE_BTB();
+}
+
+/* Invalidate the entire I-cache
+ * and writeback + invalidate the entire D-cache
+ */
+void commit_discard_idcache(void)
+{
+    commit_discard_dcache();
+    discard_icache();
 }
diff --git a/firmware/target/mips/mmu-mips.h b/firmware/target/mips/mmu-mips.h
index 7e1e36d3f4..f96ddcc28d 100644
--- a/firmware/target/mips/mmu-mips.h
+++ b/firmware/target/mips/mmu-mips.h
@@ -28,19 +28,25 @@ void map_address(unsigned long virtual, unsigned long physical,
                  unsigned long length, unsigned int cache_flags);
 void mmu_init(void);
 
-#define HAVE_CPUCACHE_INVALIDATE
-//#define HAVE_CPUCACHE_FLUSH
-
-void __idcache_invalidate_all(void);
-void __icache_invalidate_all(void);
-void __dcache_invalidate_all(void);
-void __dcache_writeback_all(void);
-
-void dma_cache_wback_inv(unsigned long addr, unsigned long size);
-
-#define commit_discard_idcache   __idcache_invalidate_all
-#define commit_discard_icache    __icache_invalidate_all
-#define commit_discard_dcache    __dcache_invalidate_all
-#define commit_dcache            __dcache_writeback_all
+/* Commits entire DCache */
+void commit_dcache(void);
+/* Commit and discard entire DCache, will do writeback */
+void commit_discard_dcache(void);
+
+/* Write DCache back to RAM for the given range and remove cache lines
+ * from DCache afterwards */
+void commit_discard_dcache_range(const void *base, unsigned int size);
+
+/* Write DCache back to RAM for the given range */
+void commit_dcache_range(const void *base, unsigned int size);
+
+/*
+ * Remove cache lines for the given range from DCache
+ * will *NOT* do write back except for buffer edges not on a line boundary
+ */
+void discard_dcache_range(const void *base, unsigned int size);
+
+/* Discards the entire ICache, and commit+discards the entire DCache */
+void commit_discard_idcache(void);
 
 #endif /* __MMU_MIPS_INCLUDE_H */