14 files changed, 449 insertions, 230 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 3f558566f8..7169a669e8 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -408,6 +408,7 @@ common/memmove.c
 common/memset.c
 common/memset16.c
 common/strlen.c
+target/mips/mmu-mips.c
 #if CONFIG_CPU==JZ4732
 target/mips/ingenic_jz47xx/crt0.S
 #endif /* CONFIG_CPU == JZ4732 */
diff --git a/firmware/export/config-ondavx747.h b/firmware/export/config-ondavx747.h
index 2fe0564e71..d4badf89cd 100644
--- a/firmware/export/config-ondavx747.h
+++ b/firmware/export/config-ondavx747.h
@@ -37,7 +37,7 @@
 //#define HAVE_HOTSWAP
 
 //#define CONFIG_STORAGE (STORAGE_NAND | STORAGE_SD)
-#define CONFIG_STORAGE STORAGE_RAMDISK /* Multivolume currently handled at firmware/target/ level */
+#define CONFIG_STORAGE STORAGE_NAND /* Multivolume currently handled at firmware/target/ level */
 
 #define CONFIG_NAND NAND_CC
 
@@ -144,7 +144,7 @@
 #define __BACKLIGHT_INIT
 
 /* Offset ( in the firmware file's header ) to the file CRC */
-#define FIRMWARE_OFFSET_FILE_CRC 0
+#define FIRMWARE_OFFSET_FILE_CRC  0
 
 /* Offset ( in the firmware file's header ) to the real data */
 #define FIRMWARE_OFFSET_FILE_DATA 8
@@ -153,12 +153,18 @@
 /* #define HAVE_ADJUSTABLE_CPU_FREQ */
 
 #define BOOTFILE_EXT "vx747"
-#define BOOTFILE "rockbox." BOOTFILE_EXT
-#define BOOTDIR "/.rockbox"
+#define BOOTFILE     "rockbox." BOOTFILE_EXT
+#define BOOTDIR      "/.rockbox"
 
-#define CONFIG_USBOTG USBOTG_JZ4740
+#define CONFIG_USBOTG     USBOTG_JZ4740
 #define HAVE_USBSTACK
-#define USB_VENDOR_ID 0x07C4
-#define USB_PRODUCT_ID 0xA4A5
+#define USE_ROCKBOX_USB
+#define USB_VENDOR_ID     0x07C4
+#define USB_PRODUCT_ID    0xA4A5
+#define USB_NUM_ENDPOINTS 3
+/* This needs to be 2048 byte aligned, but USB_QHARRAY_ATTR should take care
+ * of that */
+#define USB_QHARRAY_ATTR   __attribute__((section(".qharray"),nocommon,aligned(4)))
+#define USB_DEVBSS_ATTR    IBSS_ATTR
 
 #endif
diff --git a/firmware/export/config-ondavx747p.h b/firmware/export/config-ondavx747p.h
index 5c4d3e7aea..2cd06d3323 100644
--- a/firmware/export/config-ondavx747p.h
+++ b/firmware/export/config-ondavx747p.h
@@ -158,7 +158,12 @@
 
 #define CONFIG_USBOTG USBOTG_JZ4740
 #define HAVE_USBSTACK
-#define USB_VENDOR_ID 0x041e 
-#define USB_PRODUCT_ID 0x4133
+#define USB_VENDOR_ID 0x07C4
+#define USB_PRODUCT_ID 0xA4A5
+#define USB_NUM_ENDPOINTS 3
+/* This needs to be 2048 byte aligned, but USB_QHARRAY_ATTR should take care
+ * of that */
+#define USB_QHARRAY_ATTR   __attribute__((section(".qharray"),nocommon,aligned(4)))
+#define USB_DEVBSS_ATTR    IBSS_ATTR
 
 #endif
diff --git a/firmware/export/config-ondavx767.h b/firmware/export/config-ondavx767.h
index 7216ef0d35..cb44de9e6e 100644
--- a/firmware/export/config-ondavx767.h
+++ b/firmware/export/config-ondavx767.h
@@ -156,7 +156,12 @@
 
 #define CONFIG_USBOTG   USBOTG_JZ4740
 #define HAVE_USBSTACK
-#define USB_VENDOR_ID 0x041e 
-#define USB_PRODUCT_ID 0x4133
+#define USB_VENDOR_ID 0x07C4
+#define USB_PRODUCT_ID 0xA4A5
+#define USB_NUM_ENDPOINTS 3
+/* This needs to be 2048 byte aligned, but USB_QHARRAY_ATTR should take care
+ * of that */
+#define USB_QHARRAY_ATTR   __attribute__((section(".qharray"),nocommon,aligned(4)))
+#define USB_DEVBSS_ATTR    IBSS_ATTR
 
 #endif
diff --git a/firmware/export/jz4740.h b/firmware/export/jz4740.h
index 409d1d5a6a..aa31a229f9 100644
--- a/firmware/export/jz4740.h
+++ b/firmware/export/jz4740.h
@@ -2105,57 +2105,57 @@
  *************************************************************************/
 
 #define SLCD_CFG    (SLCD_BASE + 0xA0)  /* SLCD Configure Register */
-#define SLCD_CTRL    (SLCD_BASE + 0xA4)  /* SLCD Control Register */
-#define SLCD_STATE    (SLCD_BASE + 0xA8)  /* SLCD Status Register */
-#define SLCD_DATA    (SLCD_BASE + 0xAC)  /* SLCD Data Register */
-#define SLCD_FIFO    (SLCD_BASE + 0xB0)  /* SLCD FIFO Register */
+#define SLCD_CTRL   (SLCD_BASE + 0xA4)  /* SLCD Control Register */
+#define SLCD_STATE  (SLCD_BASE + 0xA8)  /* SLCD Status Register */
+#define SLCD_DATA   (SLCD_BASE + 0xAC)  /* SLCD Data Register */
+#define SLCD_FIFO   (SLCD_BASE + 0xB0)  /* SLCD FIFO Register */
 
 #define REG_SLCD_CFG    REG32(SLCD_CFG)
-#define REG_SLCD_CTRL    REG8(SLCD_CTRL)
-#define REG_SLCD_STATE    REG8(SLCD_STATE)
-#define REG_SLCD_DATA    REG32(SLCD_DATA)
-#define REG_SLCD_FIFO    REG32(SLCD_FIFO)
+#define REG_SLCD_CTRL   REG8(SLCD_CTRL)
+#define REG_SLCD_STATE  REG8(SLCD_STATE)
+#define REG_SLCD_DATA   REG32(SLCD_DATA)
+#define REG_SLCD_FIFO   REG32(SLCD_FIFO)
 
 /* SLCD Configure Register */
-#define SLCD_CFG_BURST_BIT    14
-#define SLCD_CFG_BURST_MASK    (0x3 << SLCD_CFG_BURST_BIT)
-  #define SLCD_CFG_BURST_4_WORD    (0 << SLCD_CFG_BURST_BIT)
-  #define SLCD_CFG_BURST_8_WORD    (1 << SLCD_CFG_BURST_BIT)
-#define SLCD_CFG_DWIDTH_BIT    10
-#define SLCD_CFG_DWIDTH_MASK    (0x7 << SLCD_CFG_DWIDTH_BIT)
-  #define SLCD_CFG_DWIDTH_18    (0 << SLCD_CFG_DWIDTH_BIT)
-  #define SLCD_CFG_DWIDTH_16    (1 << SLCD_CFG_DWIDTH_BIT)
-  #define SLCD_CFG_DWIDTH_8_x3    (2 << SLCD_CFG_DWIDTH_BIT)
-  #define SLCD_CFG_DWIDTH_8_x2    (3 << SLCD_CFG_DWIDTH_BIT)
-  #define SLCD_CFG_DWIDTH_8_x1    (4 << SLCD_CFG_DWIDTH_BIT)
-  #define SLCD_CFG_DWIDTH_9_x2    (7 << SLCD_CFG_DWIDTH_BIT)
-#define SLCD_CFG_CWIDTH_BIT    8
-#define SLCD_CFG_CWIDTH_MASK    (0x3 << SLCD_CFG_CWIDTH_BIT)
-  #define SLCD_CFG_CWIDTH_16BIT    (0 << SLCD_CFG_CWIDTH_BIT)
-  #define SLCD_CFG_CWIDTH_8BIT    (1 << SLCD_CFG_CWIDTH_BIT)
-  #define SLCD_CFG_CWIDTH_18BIT    (2 << SLCD_CFG_CWIDTH_BIT)
-#define SLCD_CFG_CS_ACTIVE_LOW    (0 << 4)
-#define SLCD_CFG_CS_ACTIVE_HIGH    (1 << 4)
-#define SLCD_CFG_RS_CMD_LOW    (0 << 3)
-#define SLCD_CFG_RS_CMD_HIGH    (1 << 3)
-#define SLCD_CFG_CLK_ACTIVE_FALLING    (0 << 1)
+#define SLCD_CFG_BURST_BIT            14
+#define SLCD_CFG_BURST_MASK           (0x3 << SLCD_CFG_BURST_BIT)
+  #define SLCD_CFG_BURST_4_WORD       (0 << SLCD_CFG_BURST_BIT)
+  #define SLCD_CFG_BURST_8_WORD       (1 << SLCD_CFG_BURST_BIT)
+#define SLCD_CFG_DWIDTH_BIT           10
+#define SLCD_CFG_DWIDTH_MASK          (0x7 << SLCD_CFG_DWIDTH_BIT)
+  #define SLCD_CFG_DWIDTH_18          (0 << SLCD_CFG_DWIDTH_BIT)
+  #define SLCD_CFG_DWIDTH_16          (1 << SLCD_CFG_DWIDTH_BIT)
+  #define SLCD_CFG_DWIDTH_8_x3        (2 << SLCD_CFG_DWIDTH_BIT)
+  #define SLCD_CFG_DWIDTH_8_x2        (3 << SLCD_CFG_DWIDTH_BIT)
+  #define SLCD_CFG_DWIDTH_8_x1        (4 << SLCD_CFG_DWIDTH_BIT)
+  #define SLCD_CFG_DWIDTH_9_x2        (7 << SLCD_CFG_DWIDTH_BIT)
+#define SLCD_CFG_CWIDTH_BIT           8
+#define SLCD_CFG_CWIDTH_MASK          (0x3 << SLCD_CFG_CWIDTH_BIT)
+  #define SLCD_CFG_CWIDTH_16BIT       (0 << SLCD_CFG_CWIDTH_BIT)
+  #define SLCD_CFG_CWIDTH_8BIT        (1 << SLCD_CFG_CWIDTH_BIT)
+  #define SLCD_CFG_CWIDTH_18BIT       (2 << SLCD_CFG_CWIDTH_BIT)
+#define SLCD_CFG_CS_ACTIVE_LOW        (0 << 4)
+#define SLCD_CFG_CS_ACTIVE_HIGH       (1 << 4)
+#define SLCD_CFG_RS_CMD_LOW           (0 << 3)
+#define SLCD_CFG_RS_CMD_HIGH          (1 << 3)
+#define SLCD_CFG_CLK_ACTIVE_FALLING   (0 << 1)
 #define SLCD_CFG_CLK_ACTIVE_RISING    (1 << 1)
-#define SLCD_CFG_TYPE_PARALLEL    (0 << 0)
-#define SLCD_CFG_TYPE_SERIAL    (1 << 0)
+#define SLCD_CFG_TYPE_PARALLEL        (0 << 0)
+#define SLCD_CFG_TYPE_SERIAL          (1 << 0)
 
 /* SLCD Control Register */
-#define SLCD_CTRL_DMA_EN    (1 << 0)
+#define SLCD_CTRL_DMA_EN     (1 << 0)
 
 /* SLCD Status Register */
-#define SLCD_STATE_BUSY        (1 << 0)
+#define SLCD_STATE_BUSY      (1 << 0)
 
 /* SLCD Data Register */
 #define SLCD_DATA_RS_DATA    (0 << 31)
-#define SLCD_DATA_RS_COMMAND    (1 << 31)
+#define SLCD_DATA_RS_COMMAND (1 << 31)
 
 /* SLCD FIFO Register */
 #define SLCD_FIFO_RS_DATA    (0 << 31)
-#define SLCD_FIFO_RS_COMMAND    (1 << 31)
+#define SLCD_FIFO_RS_COMMAND (1 << 31)
 
 
 /*************************************************************************
diff --git a/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c b/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c
index a4b4a86a55..4a36b069f9 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-nand-jz4740.c
@@ -22,11 +22,14 @@
 #include "config.h"
 #include "jz4740.h"
 #include "ata.h"
-#include "ata-nand-target.h"
+//#include "ata-nand-target.h" /* TODO */
 #include "nand_id.h"
 #include "system.h"
 #include "panic.h"
 #include "kernel.h"
+#include "storage.h"
+#include "buffer.h"
+#include "string.h"
 
 /*
  * Standard NAND flash commands
@@ -65,8 +68,8 @@ struct nand_param
     unsigned int bus_width;        /* data bus width: 8-bit/16-bit */
     unsigned int row_cycle;        /* row address cycles: 2/3 */
     unsigned int page_size;        /* page size in bytes: 512/2048 */
-    unsigned int oob_size;        /* oob size in bytes: 16/64 */
-    unsigned int page_per_block;    /* pages per block: 32/64/128 */
+    unsigned int oob_size;         /* oob size in bytes: 16/64 */
+    unsigned int page_per_block;   /* pages per block: 32/64/128 */
 };
 
 /*
@@ -106,6 +109,8 @@ struct nand_param
 static struct nand_info* chip_info = NULL;
 static struct nand_param internal_param;
 static struct mutex nand_mtx;
+static struct wakeup nand_wkup;
+static unsigned char temp_page[4096]; /* Max page size */
 
 static inline void jz_nand_wait_ready(void)
 {
@@ -141,7 +146,7 @@ static void jz_nand_write_dma(void *source, unsigned int len, int bw)
     
     dma_enable();
 
-    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) = 0;
+    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) = DMAC_DCCSR_NDES;
     REG_DMAC_DSAR(DMA_NAND_CHANNEL) = PHYSADDR((unsigned long)source);       
     REG_DMAC_DTAR(DMA_NAND_CHANNEL) = PHYSADDR((unsigned long)NAND_DATAPORT);       
     REG_DMAC_DTCR(DMA_NAND_CHANNEL) = len / 16;                        
@@ -149,9 +154,14 @@ static void jz_nand_write_dma(void *source, unsigned int len, int bw)
     REG_DMAC_DCMD(DMA_NAND_CHANNEL) = (DMAC_DCMD_SAI| DMAC_DCMD_DAI | DMAC_DCMD_SWDH_32 | DMAC_DCMD_DS_16BYTE |
                                        (bw == 8 ? DMAC_DCMD_DWDH_8 : DMAC_DCMD_DWDH_16));
     
-    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) = (DMAC_DCCSR_EN | DMAC_DCCSR_NDES);
+    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) |= DMAC_DCCSR_EN; /* Enable DMA channel */
+#if 1
     while( REG_DMAC_DTCR(DMA_NAND_CHANNEL) )
         yield();
+#else
+    REG_DMAC_DCMD(DMA_NAND_CHANNEL) |= DMAC_DCMD_TIE;  /* Enable DMA interrupt */
+    wakeup_wait(&nand_wkup, TIMEOUT_BLOCK);
+#endif
     
     dma_disable();
     
@@ -167,22 +177,44 @@ static void jz_nand_read_dma(void *target, unsigned int len, int bw)
 
     dma_enable();
     
-    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) = 0;
+    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) = DMAC_DCCSR_NDES;
     REG_DMAC_DSAR(DMA_NAND_CHANNEL) = PHYSADDR((unsigned long)NAND_DATAPORT);       
     REG_DMAC_DTAR(DMA_NAND_CHANNEL) = PHYSADDR((unsigned long)target);       
     REG_DMAC_DTCR(DMA_NAND_CHANNEL) = len / 4;                        
     REG_DMAC_DRSR(DMA_NAND_CHANNEL) = DMAC_DRSR_RS_AUTO;                
     REG_DMAC_DCMD(DMA_NAND_CHANNEL) = (DMAC_DCMD_SAI| DMAC_DCMD_DAI | DMAC_DCMD_DWDH_32 | DMAC_DCMD_DS_32BIT |
                                        (bw == 8 ? DMAC_DCMD_SWDH_8 : DMAC_DCMD_SWDH_16));
-    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) = (DMAC_DCCSR_EN | DMAC_DCCSR_NDES);
+    REG_DMAC_DCCSR(DMA_NAND_CHANNEL) |= DMAC_DCCSR_EN; /* Enable DMA channel */
+#if 1
     while( REG_DMAC_DTCR(DMA_NAND_CHANNEL) )
         yield();
+#else
+    REG_DMAC_DCMD(DMA_NAND_CHANNEL) |= DMAC_DCMD_TIE;  /* Enable DMA interrupt */
+    wakeup_wait(&nand_wkup, TIMEOUT_BLOCK);
+#endif
     
     dma_disable();
     
     mutex_unlock(&nand_mtx);
 }
 
+void DMA_CALLBACK(DMA_NAND_CHANNEL)(void)
+{
+    if (REG_DMAC_DCCSR(DMA_NAND_CHANNEL) & DMAC_DCCSR_HLT)
+        REG_DMAC_DCCSR(DMA_NAND_CHANNEL) &= ~DMAC_DCCSR_HLT;
+
+    if (REG_DMAC_DCCSR(DMA_NAND_CHANNEL) & DMAC_DCCSR_AR)
+        REG_DMAC_DCCSR(DMA_NAND_CHANNEL) &= ~DMAC_DCCSR_AR;
+
+    if (REG_DMAC_DCCSR(DMA_NAND_CHANNEL) & DMAC_DCCSR_CT)
+        REG_DMAC_DCCSR(DMA_NAND_CHANNEL) &= ~DMAC_DCCSR_CT;
+
+    if (REG_DMAC_DCCSR(DMA_NAND_CHANNEL) & DMAC_DCCSR_TT)
+        REG_DMAC_DCCSR(DMA_NAND_CHANNEL) &= ~DMAC_DCCSR_TT;
+    
+    wakeup_signal(&nand_wkup);
+}
+
 static inline void jz_nand_read_buf(void *buf, int count, int bw)
 {
     if (bw == 8)
@@ -368,6 +400,7 @@ static int jz_nand_read_page(int block, int page, unsigned char *dst)
             {
                 /* Uncorrectable error occurred */
                 panicf("Uncorrectable ECC error at NAND page 0x%x block 0x%x", page, block);
+                return -1;
             }
             else
             {
@@ -451,16 +484,9 @@ static int jz_nand_init(void)
     internal_param.oob_size = chip_info->page_size/32;
     internal_param.page_per_block = chip_info->pages_per_block;
     
-    mutex_init(&nand_mtx);
-    
     return 0;
 }
 
-void jz_nand_read(int block, int page, unsigned char *buf)
-{
-    jz_nand_read_page(block, page, buf);
-}
-
 static bool inited = false;
 int nand_init(void)
 {
@@ -469,23 +495,41 @@ int nand_init(void)
     if(!inited)
     {
         res = jz_nand_init();
+        mutex_init(&nand_mtx);
+        wakeup_init(&nand_wkup);
+        
         inited = true;
     }
-    
+
     return res;
 }
 
-/* TODO */
-int nand_read_sectors(unsigned long start, int count, void* buf)
+int nand_read_sectors(IF_MV2(int drive,) unsigned long start, int count, void* buf)
 {
-    (void)start;
-    (void)count;
-    (void)buf;
-    return -1;
+    int i, ret = 0;
+    
+    start *= 512;
+    count *= 512;
+    
+    if(count <= chip_info->page_size)
+    {
+        ret = jz_nand_read_page(start % (chip_info->page_size * chip_info->pages_per_block), start % chip_info->page_size, temp_page);
+        memcpy(buf, temp_page, count);
+        return ret;
+    }
+    else
+    {
+        for(i=0; i<count && ret == 0; i+=chip_info->page_size)
+        {
+            ret = jz_nand_read_page((start+i) % (chip_info->page_size * chip_info->pages_per_block), (start+i) % chip_info->page_size, temp_page);
+            memcpy(buf+i, temp_page, (count-i < chip_info->page_size ? count-i : chip_info->page_size));
+        }
+        return ret;
+    }
 }
 
 /* TODO */
-int nand_write_sectors(unsigned long start, int count, const void* buf)
+int nand_write_sectors(IF_MV2(int drive,) unsigned long start, int count, const void* buf)
 {
     (void)start;
     (void)count;
@@ -526,3 +570,20 @@ void nand_enable(bool on)
     /* null - flash controller is enabled/disabled as needed. */
     (void)on;
 }
+
+#ifdef STORAGE_GET_INFO
+void nand_get_info(IF_MV2(int drive,) struct storage_info *info)
+{
+    (void)drive;
+    /* firmware version */
+    info->revision="0.00";
+
+    info->vendor="Rockbox";
+    info->product="NAND Storage";
+
+    /* blocks count */
+    /* TODO: proper amount of sectors! */
+    info->num_sectors = (chip_info->page_size / 512) * chip_info->pages_per_block * chip_info->blocks_per_bank;
+    info->sector_size = 512;
+}
+#endif
diff --git a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c
index 22e274b4d1..3bb9a27007 100644
--- a/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/ata-sd-jz4740.c
@@ -23,31 +23,20 @@
 #include "jz4740.h"
 #include "ata.h"
 #include "ata-sd-target.h"
+#include "logf.h"
 #include "sd.h"
 #include "system.h"
 #include "kernel.h"
 #include "panic.h"
 #include "debug.h"
+#include "storage.h"
 
 static struct wakeup sd_wakeup;
 
 //#define MMC_DMA_ENABLE
 #define MMC_DMA_INTERRUPT 0
 
-//#define DEBUG(x...) DEBUGF(x);
-#define DEBUG(x...) printf(x);
-
-#ifdef MMC_POWER_PIN
-#define MMC_POWER_OFF()                   \
-do {                                      \
-          __gpio_set_pin(MMC_POWER_PIN);  \
-} while (0)
-
-#define MMC_POWER_ON()                     \
-do {                                       \
-          __gpio_clear_pin(MMC_POWER_PIN); \
-} while (0)
-#endif
+#define DEBUG(x...) logf(x);
 
 #ifdef MMC_CD_PIN
 #define MMC_INSERT_STATUS() __gpio_get_pin(MMC_CD_PIN)
@@ -1088,7 +1077,7 @@ static void mmc_send_cmd(struct mmc_request *request, int cmd, unsigned int arg,
     request->block_len = block_len;
     request->buffer = buffer;
     request->cnt = nob * block_len;
-    printf("mmc_send_cmd: command = %d",cmd);
+    logf("mmc_send_cmd: command = %d",cmd);
     jz_mmc_exec_cmd(request);
 }
 
@@ -1154,3 +1143,20 @@ int _sd_write_sectors(unsigned long start, int count, const void* buf)
     (void)buf;
     return -1;
 }
+
+#ifdef STORAGE_GET_INFO
+void sd_get_info(IF_MV2(int drive,) struct storage_info *info)
+{
+    (void)drive;
+    /* firmware version */
+    info->revision="0.00";
+
+    info->vendor="Rockbox";
+    info->product="SD Storage";
+
+    /* blocks count */
+    /* TODO: proper amount of sectors! */
+    info->num_sectors = 0;
+    info->sector_size = 512;
+}
+#endif
diff --git a/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c b/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
index 8112c21147..cc2c84c6b4 100644
--- a/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/lcd-jz4740.c
@@ -103,7 +103,7 @@ void lcd_update_rect(int x, int y, int width, int height)
     REG_DMAC_DCCSR(DMA_LCD_CHANNEL) |= DMAC_DCCSR_EN; /* Enable DMA channel */
     REG_DMAC_DCMD(DMA_LCD_CHANNEL) |= DMAC_DCMD_TIE; /* Enable DMA interrupt */
 
-    wakeup_wait(&lcd_wkup, TIMEOUT_BLOCK);
+    wakeup_wait(&lcd_wkup, TIMEOUT_BLOCK); /* Sleeping in lcd_update() should be safe */
     
     REG_DMAC_DCCSR(DMA_LCD_CHANNEL) &= ~DMAC_DCCSR_EN; /* Disable DMA channel */
     
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx747/ata-sd-target.h b/firmware/target/mips/ingenic_jz47xx/onda_vx747/ata-sd-target.h
index cc6bd8a773..ba324c1dcb 100644
--- a/firmware/target/mips/ingenic_jz47xx/onda_vx747/ata-sd-target.h
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx747/ata-sd-target.h
@@ -39,13 +39,28 @@ int _sd_write_sectors(unsigned long start, int count, const void* buf);
 int _sd_init(void);
 
 #define MMC_CD_PIN    (29 + 1 * 32)  /* Pin to check card insertion */
-//#define MMC_POWER_PIN (30 + 1 * 32)  /* Pin to enable/disable card power */
-//#define MMC_PW_PIN    (14 + 3 * 32)  /* Pin to check protect card */
+//#define MMC_POWER_PIN (22 + 2 * 32)  /* Pin to enable/disable card power */
+
+#ifdef MMC_POWER_PIN
+#define MMC_POWER_OFF()                   \
+do {                                      \
+          __gpio_clear_pin(MMC_POWER_PIN);  \
+} while (0)
+
+#define MMC_POWER_ON()                     \
+do {                                       \
+          __gpio_set_pin(MMC_POWER_PIN); \
+} while (0)
+#endif
 
 static inline void mmc_init_gpio(void)
 {
     __gpio_as_msc();
     __gpio_as_input(MMC_CD_PIN);
+#ifdef MMC_POWER_PIN
+    __gpio_as_func0(MMC_POWER_PIN);
+    __gpio_as_output(MMC_POWER_PIN);
+#endif
     __gpio_enable_pull(32*3+29);
     __gpio_enable_pull(32*3+10);
     __gpio_enable_pull(32*3+11);
diff --git a/firmware/target/mips/ingenic_jz47xx/onda_vx747/usb-target.h b/firmware/target/mips/ingenic_jz47xx/onda_vx747/usb-target.h
index 09b563fd14..fbb14b7ac4 100644
--- a/firmware/target/mips/ingenic_jz47xx/onda_vx747/usb-target.h
+++ b/firmware/target/mips/ingenic_jz47xx/onda_vx747/usb-target.h
@@ -26,17 +26,19 @@
 
 #define GPIO_UDC_DETE      (32 * 3 + 28)
 #define IRQ_GPIO_UDC_DETE  (IRQ_GPIO_0 + GPIO_UDC_DETE)
+#define USB_GPIO_IRQ        GPIO124
 
-#define USB_INIT_GPIO()                   \
-{                                         \
-    REG_GPIO_PXFUNS(3) = 0x10000000;      \
-    REG_GPIO_PXSELS(3) = 0x10000000;      \
-    REG_GPIO_PXPES(3)  = 0x10000000;      \
-    __gpio_as_input(GPIO_UDC_DETE);       \
+#define USB_INIT_GPIO()                           \
+{                                                 \
+    REG_GPIO_PXPES(3)  = 0x10000000;              \
+    __gpio_as_irq_rise_edge(GPIO_UDC_DETE);       \
 }
 
 #define USB_DRV_CONNECTED()    (__gpio_get_pin(GPIO_UDC_DETE) == 1)
 
+/* Connect by events, not by tick polling */
+#define USB_STATUS_BY_EVENT
+
 int usb_detect(void);
 void usb_init_device(void);
 bool usb_drv_connected(void);
diff --git a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
index 033b42214b..9c7f83530f 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/system-jz4740.c
@@ -23,6 +23,7 @@
 #include "jz4740.h"
 #include "mips.h"
 #include "mipsregs.h"
+#include "mmu-mips.h"
 #include "panic.h"
 #include "system.h"
 #include "string.h"
@@ -524,106 +525,6 @@ void dma_cache_wback_inv(unsigned long addr, unsigned long size)
     }
 }
 
-#define BARRIER                            \
-    __asm__ __volatile__(                  \
-    "    .set    noreorder          \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    nop                        \n"    \
-    "    .set    reorder            \n");
-
-#define DEFAULT_PAGE_SHIFT       PL_4K
-#define DEFAULT_PAGE_MASK        PM_4K
-#define UNIQUE_ENTRYHI(idx, ps)  (A_K0BASE + ((idx) << (ps + 1)))
-#define ASID_MASK                M_EntryHiASID
-#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;
-    int entry;
-    unsigned int old_irq = disable_irq_save();
-    
-    /* Save old context and create impossible VPN2 value */
-    old_ctx = read_c0_entryhi();
-    write_c0_entrylo0(0);
-    write_c0_entrylo1(0);
-    BARRIER;
-
-    /* Blast 'em all away. */
-    for(entry = 0; entry < 32; entry++)
-    {
-        /* Make sure all entries differ. */
-        write_c0_entryhi(UNIQUE_ENTRYHI(entry, DEFAULT_PAGE_SHIFT));
-        write_c0_index(entry);
-        BARRIER;
-        tlb_write_indexed();
-    }
-    BARRIER;
-    write_c0_entryhi(old_ctx);
-    
-    restore_irq(old_irq);
-}
-
-static void add_wired_entry(unsigned long entrylo0, unsigned long entrylo1,
-                            unsigned long entryhi,  unsigned long pagemask)
-{
-    unsigned long wired;
-    unsigned long old_pagemask;
-    unsigned long old_ctx;
-    unsigned int  old_irq = disable_irq_save();
-    
-    old_ctx = read_c0_entryhi() & ASID_MASK;
-    old_pagemask = read_c0_pagemask();
-    wired = read_c0_wired();
-    write_c0_wired(wired + 1);
-    write_c0_index(wired);
-    BARRIER;
-    write_c0_pagemask(pagemask);
-    write_c0_entryhi(entryhi);
-    write_c0_entrylo0(entrylo0);
-    write_c0_entrylo1(entrylo1);
-    BARRIER;
-    tlb_write_indexed();
-    BARRIER;
-
-    write_c0_entryhi(old_ctx);
-    BARRIER;
-    write_c0_pagemask(old_pagemask);
-    local_flush_tlb_all();
-    restore_irq(old_irq);
-}
-
-static void map_address(unsigned long virtual, unsigned long physical, unsigned long length)
-{
-    unsigned long entry0  = (physical & PFN_MASK) << PFN_SHIFT;
-    unsigned long entry1  = ((physical+length) & PFN_MASK) << PFN_SHIFT;
-    unsigned long entryhi = virtual & ~VPN2_SHIFT;
-    
-    entry0 |= (M_EntryLoG | M_EntryLoV | (K_CacheAttrC << S_EntryLoC) );
-    entry1 |= (M_EntryLoG | M_EntryLoV | (K_CacheAttrC << S_EntryLoC) );
-    
-    add_wired_entry(entry0, entry1, entryhi, DEFAULT_PAGE_MASK);
-}
-
-
-static void tlb_init(void)
-{
-    write_c0_pagemask(DEFAULT_PAGE_MASK);
-    write_c0_wired(0);
-    write_c0_framemask(0);
-    
-    local_flush_tlb_all();
-/*
-    map_address(0x80000000, 0x80000000, 0x4000);
-    map_address(0x80004000, 0x80004000, MEM * 0x100000);
-*/
-}
-
 void tlb_refill_handler(void)
 {
     panicf("TLB refill handler at 0x%08lx! [0x%x]", read_c0_epc(), read_c0_badvaddr());
diff --git a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
index 0916a9414a..7bd0ba4ae5 100644
--- a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
@@ -60,9 +60,11 @@ struct usb_endpoint
         unsigned int sent;
         unsigned int received;
     };
+    bool busy;
 
     const enum ep_type type;
     const bool use_dma;
+    bool wait;
 
     const unsigned int fifo_addr;
     unsigned short fifo_size;
@@ -71,13 +73,14 @@ struct usb_endpoint
 static unsigned char ep0_rx_buf[64];
 static unsigned char ep0state = USB_EP0_IDLE;
 static struct usb_endpoint endpoints[] =
-{/*    buf        length sent     type      use_dma  fifo_addr  fifo_size */
-    {&ep0_rx_buf,    0,  {0},  ep_control,   false, USB_FIFO_EP0,  64 },
-    {NULL,           0,  {0},  ep_control,   false, USB_FIFO_EP0,  64 },
-    {NULL,           0,  {0},  ep_bulk,      false, USB_FIFO_EP1,  512},
-    {NULL,           0,  {0},  ep_bulk,      false, USB_FIFO_EP1,  512},
-    {NULL,           0,  {0},  ep_interrupt, false, USB_FIFO_EP2,  64 }
+{/*    buf        length sent  busy  type      use_dma  wait   fifo_addr  fifo_size */
+    {&ep0_rx_buf,    0,  {0}, false, ep_control,   false, false,  USB_FIFO_EP0,  64 },
+    {NULL,           0,  {0}, false, ep_control,   false, false,  USB_FIFO_EP0,  64 },
+    {NULL,           0,  {0}, false, ep_bulk,      false, false,  USB_FIFO_EP1,  512},
+    {NULL,           0,  {0}, false, ep_bulk,      false, false,  USB_FIFO_EP1,  512},
+    {NULL,           0,  {0}, false, ep_interrupt, false, false,  USB_FIFO_EP2,  64 }
 };
+static struct wakeup ep_wkup[TOTAL_EP()];
 
 static inline void select_endpoint(int ep)
 {
@@ -167,13 +170,14 @@ static void EP0_send(void)
     struct usb_endpoint* ep = &endpoints[1];
     unsigned int length;
     unsigned char csr0;
-    
+       
     select_endpoint(0);
     csr0 = REG_USB_REG_CSR0;
     
     if(ep->length == 0)
     {
-        REG_USB_REG_CSR0 = (csr0 | USB_CSR0_SVDOUTPKTRDY | USB_CSR0_DATAEND);
+        //REG_USB_REG_CSR0 = (csr0 | USB_CSR0_SVDOUTPKTRDY | USB_CSR0_DATAEND);
+        REG_USB_REG_CSR0 = (csr0 | USB_CSR0_SVDOUTPKTRDY);
         return;
     }
     
@@ -189,6 +193,8 @@ static void EP0_send(void)
     {
         REG_USB_REG_CSR0 = (csr0 | USB_CSR0_INPKTRDY | USB_CSR0_DATAEND); /* Set data end! */
         ep0state = USB_EP0_IDLE;
+        if(ep->wait)
+            wakeup_signal(&ep_wkup[1]);
     }
     else
         REG_USB_REG_CSR0 = (csr0 | USB_CSR0_INPKTRDY);
@@ -199,7 +205,7 @@ static void EP0_handler(void)
     logf("EP0_handler");
     
     unsigned char csr0;
-
+    
     /* Read CSR0 */
     select_endpoint(0);
     csr0 = REG_USB_REG_CSR0;
@@ -229,11 +235,13 @@ static void EP0_handler(void)
     /* Call relevant routines for endpoint 0 state */
     if(ep0state == USB_EP0_IDLE)
     {
-        if(csr0 & USB_CSR0_OUTPKTRDY)   /* There is data in the fifo */
+        if(csr0 & USB_CSR0_OUTPKTRDY)   /* There is a packet in the fifo */
         {
             readFIFO(&endpoints[0], REG_USB_REG_COUNT0);
             REG_USB_REG_CSR0 = csr0 | USB_CSR0_SVDOUTPKTRDY; /* clear OUTPKTRDY bit */
             usb_core_control_request((struct usb_ctrlrequest*)endpoints[0].buf);
+            if(endpoints[0].wait)
+                wakeup_signal(&ep_wkup[0]);
         }
     }
     else if(ep0state == USB_EP0_TX)
@@ -249,7 +257,7 @@ static void EPIN_handler(unsigned int endpoint)
     csr = REG_USB_REG_INCSR;
     logf("EPIN_handler(%d): 0x%x", endpoint, csr);
     
-    if(ep->buf == NULL || ep->length == 0)
+    if(!ep->busy)
         return;
     
     if(csr & USB_INCSR_SENTSTALL)
@@ -258,15 +266,19 @@ static void EPIN_handler(unsigned int endpoint)
         return;
     }
     
+#if 0
     if(ep->use_dma == true)
         return;
+#endif
     
     if(csr & USB_INCSR_FFNOTEMPT)
     {
-        logf("FIFO is not empty!");
+        logf("FIFO is not empty!: 0x%x", csr);
         return;
     }
     
+    logf("EP%d: %d -> %d", endpoint, ep->sent, ep->length);
+    
     if(ep->sent == 0)
         length = (ep->length <= ep->fifo_size ? ep->length : ep->fifo_size);
     else
@@ -279,9 +291,13 @@ static void EPIN_handler(unsigned int endpoint)
     if(ep->sent >= ep->length)
     {
         usb_core_transfer_complete(endpoint, USB_DIR_IN, 0, ep->sent);
+        if(ep->wait)
+            wakeup_signal(&ep_wkup[endpoint*2+1]);
+        logf("sent complete");
         ep->sent = 0;
         ep->length = 0;
         ep->buf = NULL;
+        ep->busy = false;
     }
 }
 
@@ -289,36 +305,44 @@ static void EPOUT_handler(unsigned int endpoint)
 {
     struct usb_endpoint* ep = &endpoints[endpoint*2];
     unsigned int size, csr;
-
+    
     select_endpoint(endpoint);
     csr = REG_USB_REG_OUTCSR;
     logf("EPOUT_handler(%d): 0x%x", endpoint, csr);
-    
-    if(ep->buf == NULL || ep->length == 0)
+   
+    if(!ep->busy)
         return;
-    
+   
     if(csr & USB_OUTCSR_SENTSTALL)
     {
+        logf("stall sent, flushing fifo..");
+        flushFIFO(ep);
         REG_USB_REG_OUTCSR = csr & ~USB_OUTCSR_SENTSTALL;
         return;
     }
     
-    size = REG_USB_REG_OUTCOUNT;
-    
-    readFIFO(ep, size);
-    ep->received += size;
-
-    REG_USB_REG_OUTCSR = csr & ~USB_OUTCSR_OUTPKTRDY;
-    
-    logf("received: %d length: %d", ep->received, ep->length);
-    
-    if(size < ep->fifo_size || ep->received >= ep->length)
+    if(csr & USB_OUTCSR_OUTPKTRDY) /* There is a packet in the fifo */
     {
-        usb_core_transfer_complete(endpoint, USB_DIR_OUT, 0, ep->received);
-        logf("receive transfer_complete");
-        ep->received = 0;
-        ep->length = 0;
-        ep->buf = NULL;
+        size = REG_USB_REG_OUTCOUNT;
+        
+        readFIFO(ep, size);
+        ep->received += size;
+
+        REG_USB_REG_OUTCSR = csr & ~USB_OUTCSR_OUTPKTRDY;
+        
+        logf("received: %d max length: %d", ep->received, ep->length);
+        
+        if(size < ep->fifo_size || ep->received >= ep->length)
+        {
+            usb_core_transfer_complete(endpoint, USB_DIR_OUT, 0, ep->received);
+            if(ep->wait)
+                wakeup_signal(&ep_wkup[endpoint*2]);
+            logf("receive transfer_complete");
+            ep->received = 0;
+            ep->length = 0;
+            ep->buf = NULL;
+            ep->busy = false;
+        }
     }
 }
 
@@ -330,6 +354,9 @@ static void setup_endpoint(struct usb_endpoint *ep)
     
     select_endpoint(EP_NUMBER2(ep));
     
+    ep->busy = false;
+    ep->wait = false;
+    
     if(ep->type == ep_bulk)
     {
         if(REG_USB_REG_POWER & USB_POWER_HSMODE)
@@ -474,11 +501,13 @@ void UDC(void)
 
 bool usb_drv_stalled(int endpoint, bool in)
 {
+    endpoint &= 0x7F;
+    
     logf("usb_drv_stalled(%d, %s)", endpoint, in?"IN":"OUT");
     
-    select_endpoint(endpoint & 0x7F);
+    select_endpoint(endpoint);
     
-    if(endpoint == 0)
+    if(endpoint == EP_CONTROL)
         return (REG_USB_REG_CSR0 & USB_CSR0_SENDSTALL) != 0;
     else
     {
@@ -491,6 +520,8 @@ bool usb_drv_stalled(int endpoint, bool in)
 
 void usb_drv_stall(int endpoint, bool stall, bool in)
 {
+    endpoint &= 0x7F;
+    
     logf("usb_drv_stall(%d,%s,%s)", endpoint, stall?"Y":"N", in?"IN":"OUT");
     
     select_endpoint(endpoint);
@@ -533,10 +564,25 @@ int usb_detect(void)
 
 void usb_init_device(void)
 {
+    unsigned int i;
+    
     USB_INIT_GPIO();
+#ifdef USB_GPIO_IRQ
+    system_enable_irq(IRQ_GPIO_UDC_DETE);
+#endif
     system_enable_irq(IRQ_UDC);
+    
+    for(i=0; i<TOTAL_EP(); i++)
+        wakeup_init(&ep_wkup[i]);
 }
 
+#ifdef USB_GPIO_IRQ
+void USB_GPIO_IRQ(void)
+{
+    usb_status_event(usb_detect());
+}
+#endif
+
 void usb_enable(bool on)
 {
     if(on)
@@ -545,6 +591,11 @@ void usb_enable(bool on)
         usb_core_exit();
 }
 
+void usb_attach(void)
+{
+    usb_enable(true);
+}
+
 void usb_drv_init(void)
 {
     logf("usb_drv_init()");
@@ -591,7 +642,7 @@ void usb_drv_set_address(int address)
     REG_USB_REG_FADDR = address;
 }
 
-int usb_drv_send(int endpoint, void* ptr, int length)
+int usb_drv_send_nonblocking(int endpoint, void* ptr, int length)
 {
     int flags;
     endpoint &= 0x7F;
@@ -619,6 +670,7 @@ int usb_drv_send(int endpoint, void* ptr, int length)
         endpoints[endpoint*2+1].buf = ptr;
         endpoints[endpoint*2+1].sent = 0;
         endpoints[endpoint*2+1].length = length;
+        endpoints[endpoint*2+1].busy = true;
 #if 0
         select_endpoint(endpoint);
         
@@ -626,7 +678,6 @@ int usb_drv_send(int endpoint, void* ptr, int length)
         REG_USB_REG_COUNT2 = length;
         REG_USB_REG_CNTL2 = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 | USB_CNTL_DIR_IN | USB_CNTL_ENA);
 #else
-
         EPIN_handler(endpoint);
 #endif
         restore_irq(flags);
@@ -634,9 +685,20 @@ int usb_drv_send(int endpoint, void* ptr, int length)
     }
 }
 
-int usb_drv_send_nonblocking(int endpoint, void* ptr, int length)
+int usb_drv_send(int endpoint, void* ptr, int length)
 {
-    return usb_drv_send(endpoint, ptr, length);
+    int ret;
+    endpoint &= 0x7F;
+    
+    if(endpoint == EP_CONTROL && ptr == NULL && length == 0)
+        return 0; /* ACK request, handled by the USB controller */
+    
+    endpoints[endpoint*2+1].wait = true;
+    ret = usb_drv_send_nonblocking(endpoint, ptr, length);
+    wakeup_wait(&ep_wkup[endpoint*2+1], TIMEOUT_BLOCK);
+    endpoints[endpoint*2+1].wait = false;
+
+    return ret;
 }
 
 int usb_drv_recv(int endpoint, void* ptr, int length)
@@ -654,7 +716,9 @@ int usb_drv_recv(int endpoint, void* ptr, int length)
         endpoints[endpoint*2].buf = ptr;
         endpoints[endpoint*2].received = 0;
         endpoints[endpoint*2].length = length;
+        endpoints[endpoint*2].busy = true;
         restore_irq(flags);
+        
         return 0;
     }
 }
diff --git a/firmware/target/mips/mmu-mips.c b/firmware/target/mips/mmu-mips.c
new file mode 100644
index 0000000000..3c1b932325
--- /dev/null
+++ b/firmware/target/mips/mmu-mips.c
@@ -0,0 +1,125 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2009 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#include "config.h"
+#include "mips.h"
+#include "mipsregs.h"
+#include "system.h"
+#include "mmu-mips.h"
+
+#define BARRIER                            \
+    __asm__ __volatile__(                  \
+    "    .set    noreorder          \n"    \
+    "    nop                        \n"    \
+    "    nop                        \n"    \
+    "    nop                        \n"    \
+    "    nop                        \n"    \
+    "    nop                        \n"    \
+    "    nop                        \n"    \
+    "    .set    reorder            \n");
+
+#define DEFAULT_PAGE_SHIFT       PL_4K
+#define DEFAULT_PAGE_MASK        PM_4K
+#define UNIQUE_ENTRYHI(idx, ps)  (A_K0BASE + ((idx) << (ps + 1)))
+#define ASID_MASK                M_EntryHiASID
+#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;
+    int entry;
+    unsigned int old_irq = disable_irq_save();
+    
+    /* Save old context and create impossible VPN2 value */
+    old_ctx = read_c0_entryhi();
+    write_c0_entrylo0(0);
+    write_c0_entrylo1(0);
+    BARRIER;
+
+    /* Blast 'em all away. */
+    for(entry = 0; entry < 32; entry++)
+    {
+        /* Make sure all entries differ. */
+        write_c0_entryhi(UNIQUE_ENTRYHI(entry, DEFAULT_PAGE_SHIFT));
+        write_c0_index(entry);
+        BARRIER;
+        tlb_write_indexed();
+    }
+    BARRIER;
+    write_c0_entryhi(old_ctx);
+    
+    restore_irq(old_irq);
+}
+
+static void add_wired_entry(unsigned long entrylo0, unsigned long entrylo1,
+                            unsigned long entryhi,  unsigned long pagemask)
+{
+    unsigned long wired;
+    unsigned long old_pagemask;
+    unsigned long old_ctx;
+    unsigned int  old_irq = disable_irq_save();
+    
+    old_ctx = read_c0_entryhi() & ASID_MASK;
+    old_pagemask = read_c0_pagemask();
+    wired = read_c0_wired();
+    write_c0_wired(wired + 1);
+    write_c0_index(wired);
+    BARRIER;
+    write_c0_pagemask(pagemask);
+    write_c0_entryhi(entryhi);
+    write_c0_entrylo0(entrylo0);
+    write_c0_entrylo1(entrylo1);
+    BARRIER;
+    tlb_write_indexed();
+    BARRIER;
+
+    write_c0_entryhi(old_ctx);
+    BARRIER;
+    write_c0_pagemask(old_pagemask);
+    local_flush_tlb_all();
+    restore_irq(old_irq);
+}
+
+void map_address(unsigned long virtual, unsigned long physical, unsigned long length)
+{
+    unsigned long entry0  = (physical & PFN_MASK) << PFN_SHIFT;
+    unsigned long entry1  = ((physical+length) & PFN_MASK) << PFN_SHIFT;
+    unsigned long entryhi = virtual & ~VPN2_SHIFT;
+    
+    entry0 |= (M_EntryLoG | M_EntryLoV | (K_CacheAttrC << S_EntryLoC) );
+    entry1 |= (M_EntryLoG | M_EntryLoV | (K_CacheAttrC << S_EntryLoC) );
+    
+    add_wired_entry(entry0, entry1, entryhi, DEFAULT_PAGE_MASK);
+}
+
+void tlb_init(void)
+{
+    write_c0_pagemask(DEFAULT_PAGE_MASK);
+    write_c0_wired(0);
+    write_c0_framemask(0);
+    
+    local_flush_tlb_all();
+/*
+    map_address(0x80000000, 0x80000000, 0x4000);
+    map_address(0x80004000, 0x80004000, MEM * 0x100000);
+*/
+}
diff --git a/firmware/target/mips/mmu-mips.h b/firmware/target/mips/mmu-mips.h
new file mode 100644
index 0000000000..1670d57f1c
--- /dev/null
+++ b/firmware/target/mips/mmu-mips.h
@@ -0,0 +1,28 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2009 by Maurus Cuelenaere
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#ifndef __MMU_MIPS_INCLUDE_H
+#define __MMU_MIPS_INCLUDE_H
+
+void map_address(unsigned long virtual, unsigned long physical, unsigned long length);
+void tlb_init(void);
+
+#endif /* __MMU_MIPS_INCLUDE_H */