3 files changed, 170 insertions, 76 deletions

diff --git a/firmware/export/tcc77x.h b/firmware/export/tcc77x.h
index ad1a8a17aa..db128b68f6 100644
--- a/firmware/export/tcc77x.h
+++ b/firmware/export/tcc77x.h
@@ -182,51 +182,43 @@
 
 /* ECC controller */
 
-#define ECC_CTRL   (*(volatile unsigned long *)0x80000900)
-    #define ECC_DMA_REQ (1<<28)
-    #define ECC_ENC     (1<<27) /* MLC ECC3/4 */
-    #define ECC_FLG     (1<<26)
-    #define ECC_IEN     (1<<25)
-    #define ECC_MANUAL  (1<<22)
-    #define ECC_WCNT    (1<<12) /* [21:12] */
-    #define ECC_HOLD    (1<<7)
-    #define ECC_M4EN    (1<<6)
-    #define ECC_ZERO    (1<<5)
-    #define ECC_M3EN    (1<<4)
-    #define ECC_CNT_MASK (7<<1)
-    #define ECC_CNT     (1<<1)
-    #define ECC_SLC     (1<<0)
+#define ECC_CTRL       (*(volatile unsigned long *)0x80000900)
+    #define ECC_DMA_REQ   (1<<28)
+    #define ECC_ENC       (1<<27) /* MLC ECC3/4 */
+    #define ECC_READY     (1<<26)
+    #define ECC_IEN       (1<<25)
+    #define ECC_MANUAL    (1<<22)
+    #define ECC_WCNT      (1<<12) /* [21:12] */
+    #define ECC_HOLD      (1<<7)
+    #define ECC_M4EN      (1<<6)
+    #define ECC_ZERO      (1<<5)
+    #define ECC_M3EN      (1<<4)
+    #define ECC_CNT_MASK  (7<<1)
+    #define ECC_CNT       (1<<1)
+    #define ECC_SLC       (1<<0)
     
-#define ECC_BASE   (*(volatile unsigned long *)0x80000904)
-#define ECC_MASK   (*(volatile unsigned long *)0x80000908)
-#define ECC_CLR    (*(volatile unsigned long *)0x8000090c)
-#define SLC_ECC0   (*(volatile unsigned long *)0x80000910)
-#define SLC_ECC1   (*(volatile unsigned long *)0x80000914)
-#define SLC_ECC2   (*(volatile unsigned long *)0x80000918)
-#define SLC_ECC3   (*(volatile unsigned long *)0x8000091c)
-#define SLC_ECC4   (*(volatile unsigned long *)0x80000920)
-#define SLC_ECC5   (*(volatile unsigned long *)0x80000924)
-#define SLC_ECC6   (*(volatile unsigned long *)0x80000928)
-#define SLC_ECC7   (*(volatile unsigned long *)0x8000092c)
-#define MLC_ECC0W  (*(volatile unsigned long *)0x80000930)
-#define MLC_ECC1W  (*(volatile unsigned long *)0x80000934)
-#define MLC_ECC2W  (*(volatile unsigned long *)0x80000938)
-#define MLC_ECC0R  (*(volatile unsigned long *)0x80000940)
-#define MLC_ECC1R  (*(volatile unsigned long *)0x80000944)
-#define MLC_ECC2R  (*(volatile unsigned long *)0x80000948)
+#define ECC_BASE       (*(volatile unsigned long *)0x80000904)
+#define ECC_MASK       (*(volatile unsigned long *)0x80000908)
+#define ECC_CLR        (*(volatile unsigned long *)0x8000090c)
+#define SLC_ECC0       (*(volatile unsigned long *)0x80000910)
+#define SLC_ECC1       (*(volatile unsigned long *)0x80000914)
+#define SLC_ECC2       (*(volatile unsigned long *)0x80000918)
+#define SLC_ECC3       (*(volatile unsigned long *)0x8000091c)
+#define SLC_ECC4       (*(volatile unsigned long *)0x80000920)
+#define SLC_ECC5       (*(volatile unsigned long *)0x80000924)
+#define SLC_ECC6       (*(volatile unsigned long *)0x80000928)
+#define SLC_ECC7       (*(volatile unsigned long *)0x8000092c)
+#define MLC_ECC0W      (*(volatile unsigned long *)0x80000930)
+#define MLC_ECC1W      (*(volatile unsigned long *)0x80000934)
+#define MLC_ECC2W      (*(volatile unsigned long *)0x80000938)
+#define MLC_ECC0R      (*(volatile unsigned long *)0x80000940)
+#define MLC_ECC1R      (*(volatile unsigned long *)0x80000944)
+#define MLC_ECC2R      (*(volatile unsigned long *)0x80000948)
 #define ECC_CORR_START (*(volatile unsigned long *)0x8000094c)
-#define ECC_ERRADDR1 (*(volatile unsigned long *)0x80000950)
-#define ECC_ERRADDR2 (*(volatile unsigned long *)0x80000954)
-#define ECC_ERRADDR3 (*(volatile unsigned long *)0x80000958)
-#define ECC_ERRADDR4 (*(volatile unsigned long *)0x8000095c)
-#define ECC_ERRDATA1 (*(volatile unsigned long *)0x80000960)
-#define ECC_ERRDATA2 (*(volatile unsigned long *)0x80000964)
-#define ECC_ERRDATA3 (*(volatile unsigned long *)0x80000968)
-#define ECC_ERRDATA4 (*(volatile unsigned long *)0x8000096c)
-#define ECC_ERR_NUM  (*(volatile unsigned long *)0x80000970)
-
-#define ECC_ERRDATA(x) (*(volatile unsigned long *)(0x80000960 + (x) * 4))
-#define ECC_ERRADDR(x) (*(volatile unsigned long *)(0x80000950 + (x) * 4))
+#define ECC_ERRADDR(x) (*(volatile unsigned long *)(0x80000950+4*(x)))
+#define ECC_ERRDATA(x) (*(volatile unsigned long *)(0x80000960+4*(x)))
+#define ECC_ERR_NUM    (*(volatile unsigned long *)0x80000970)
+
 
 /* Digital Audio Interface */
 #define DADI_L0    (*(volatile unsigned long *)0x80000000)
diff --git a/firmware/export/tcc780x.h b/firmware/export/tcc780x.h
index 24a4415ef3..497f1514e0 100644
--- a/firmware/export/tcc780x.h
+++ b/firmware/export/tcc780x.h
@@ -232,18 +232,18 @@
 
 /* ECC Controller */
 
-#define ECC_CTRL    (*(volatile unsigned long *)0xF005B000)
-    #define ECC_M4EN  (1<<6)
-    #define ECC_ENC   (1<<27)
-    #define ECC_READY (1<<26)
-#define ECC_BASE    (*(volatile unsigned long *)0xF005B004)
-#define ECC_CLR     (*(volatile unsigned long *)0xF005B00C)
-#define ECC_MLC0W   (*(volatile unsigned long *)0xF005B030)
-#define ECC_MLC1W   (*(volatile unsigned long *)0xF005B034)
-#define ECC_MLC2W   (*(volatile unsigned long *)0xF005B038)
-#define ECC_ERRADDR (*(volatile unsigned long *)0xF005B050)
-#define ECC_ERRDATA (*(volatile unsigned long *)0xF005B060)
-#define ECC_ERR     (*(volatile unsigned long *)0xF005B070)
+#define ECC_CTRL       (*(volatile unsigned long *)0xF005B000)
+    #define ECC_ENC       (1<<27)
+    #define ECC_READY     (1<<26)
+    #define ECC_M4EN      (1<<6)
+#define ECC_BASE       (*(volatile unsigned long *)0xF005B004)
+#define ECC_CLR        (*(volatile unsigned long *)0xF005B00C)
+#define MLC_ECC0W      (*(volatile unsigned long *)0xF005B030)
+#define MLC_ECC1W      (*(volatile unsigned long *)0xF005B034)
+#define MLC_ECC2W      (*(volatile unsigned long *)0xF005B038)
+#define ECC_ERRADDR(x) (*(volatile unsigned long *)(0xF005B050+4*(x)))
+#define ECC_ERRDATA(x) (*(volatile unsigned long *)(0xF005B060+4*(x)))
+#define ECC_ERR_NUM    (*(volatile unsigned long *)0xF005B070)
 
 /* SD/MMC Controller */
 
diff --git a/firmware/target/arm/ata-nand-telechips.c b/firmware/target/arm/ata-nand-telechips.c
index ffe6de897e..7250211eb9 100644
--- a/firmware/target/arm/ata-nand-telechips.c
+++ b/firmware/target/arm/ata-nand-telechips.c
@@ -31,7 +31,10 @@
 
 #define SECTOR_SIZE 512
 
-/* #define USE_ECC_CORRECTION */
+/* ECC on read is implemented on the assumption that MLC-style 4-bit correction
+   is always used regardless of NAND chip type. This assumption is true for at
+   least D2 (MLC) and M200 (SLC). */
+#define USE_ECC_CORRECTION
 
 /* for compatibility */
 int ata_spinup_time = 0;
@@ -140,12 +143,6 @@ static struct write_cache write_caches[MAX_WRITE_CACHES];
 
 static int write_caches_in_use = 0;
 
-#ifdef USE_ECC_CORRECTION
-static unsigned int ecc_sectors_corrected = 0;
-static unsigned int ecc_bits_corrected = 0;
-static unsigned int ecc_fail_count = 0;
-#endif
-
 
 /* Conversion functions */
 
@@ -315,7 +312,7 @@ static void nand_read_uid(int bank, unsigned int* uid_buf)
 }
 
 
-static void nand_read_raw(int bank, int row, int column, int size, void* buf)
+static void nand_setup_read(int bank, int row, int column)
 {
     int i;
 
@@ -355,6 +352,23 @@ static void nand_read_raw(int bank, int row, int column, int size, void* buf)
 
     /* Wait until complete */
     while (!(NFC_CTRL & NFC_READY)) {};
+}
+
+
+static void nand_end_read(void)
+{
+    nand_chip_select(-1);
+        
+    /* Disable NFC bus clock */
+    BCLKCTR &= ~DEV_NAND;
+}
+
+
+static void nand_read_raw(int bank, int row, int column, int size, void* buf)
+{
+    int i;
+    
+    nand_setup_read(bank, row, column);
 
     /* Read data into page buffer */
     if (((unsigned int)buf & 3) || (size & 3))
@@ -374,11 +388,8 @@ static void nand_read_raw(int bank, int row, int column, int size, void* buf)
             ((unsigned int*)buf)[i] = NFC_WDATA;
         }
     }
-
-    nand_chip_select(-1);
-        
-    /* Disable NFC bus clock */
-    BCLKCTR &= ~DEV_NAND;
+    
+    nand_end_read();
 }
 
 
@@ -477,15 +488,106 @@ static void nand_get_chip_info(void)
 static bool nand_read_sector_of_phys_page(int bank, int page,
                                           int sector, void* buf)
 {
-#ifndef USE_ECC_CORRECTION
-    nand_read_raw(bank, page,
-                  sector * (SECTOR_SIZE+16),
-                  SECTOR_SIZE, buf);
-    return true;
-#else
-    /* Not yet implemented */
-    return false;
+    bool ret = true;
+    int i;
+    int page_offset = sector * (SECTOR_SIZE + 16);
+
+#ifdef USE_ECC_CORRECTION
+    unsigned long spare_buf[4];
+    
+    /* Set up the ECC controller to monitor reads from NFC_WDATA */
+    BCLKCTR |= DEV_ECC;
+    ECC_BASE = (unsigned long)&NFC_WDATA;
+    ECC_CTRL |= ECC_M4EN;
+    ECC_CTRL &= ~ECC_ENC;
+    ECC_CTRL |= ECC_READY;
+    ECC_CLR = 0;
 #endif
+
+    /* Read the sector data */
+    nand_setup_read(bank, page, page_offset);
+
+    /* Read data into page buffer */
+    if ((unsigned int)buf & 3)
+    {
+        /* If unaligned, read into a temporary buffer and copy to destination.
+           This way, reads are always done through NFC_WDATA - otherwise they
+           would not be 'seen' by the ECC controller. */
+        static char temp_buf[SECTOR_SIZE];
+        
+        unsigned int* ptr = (unsigned int*) temp_buf;
+        
+        for (i = 0; i < (SECTOR_SIZE/4); i++)
+        {
+            *ptr++ = NFC_WDATA;
+        }
+        
+        memcpy(buf, temp_buf, SECTOR_SIZE);
+    }
+    else
+    {
+        /* Use straight word copy as buffer and size are both word-aligned */
+        unsigned int* ptr = (unsigned int*) buf;
+        
+        for (i = 0; i < (SECTOR_SIZE/4); i++)
+        {
+            *ptr++ = NFC_WDATA;
+        }
+    }
+    
+#ifdef USE_ECC_CORRECTION
+    /* Stop monitoring before we read the OOB data */
+    ECC_CTRL &= ~ECC_M4EN;
+    BCLKCTR &= ~DEV_ECC;
+
+    /* Read a further 4 words (sector OOB data) */
+    spare_buf[0] = NFC_WDATA;
+    spare_buf[1] = NFC_WDATA;
+    spare_buf[2] = NFC_WDATA;
+    spare_buf[3] = NFC_WDATA;
+
+    /* Calculate MLC4 ECC using bytes 0,1,8-15 */
+    BCLKCTR |= DEV_ECC;
+    ECC_CTRL |= ECC_M4EN;
+    
+    MLC_ECC0W = *(unsigned short*)spare_buf;
+    MLC_ECC1W = spare_buf[2];
+    MLC_ECC2W = spare_buf[3];
+    
+    while (!(ECC_CTRL & ECC_READY)) {};
+
+    int errors = ECC_ERR_NUM & 7;
+    
+    switch (errors)
+    {
+        case 4:     /* nothing to correct */
+            break;
+            
+        case 7:     /* fail, can't correct */
+            ret = false;
+            break;
+            
+        default:    /* between 1 and 4 errors */
+        {
+            int i;
+            unsigned char* char_buf = (unsigned char*)buf;
+            
+            for (i = 0; i < errors + 1; i++)
+            {
+                int offset = 0x207 - ECC_ERRADDR(i);
+                char_buf[offset] ^= ECC_ERRDATA(i);
+            }
+        }
+    }
+
+    /* Disable ECC block */
+    ECC_CTRL &= ~ECC_M4EN;
+    BCLKCTR &= ~DEV_ECC;
+#endif
+    
+    nand_end_read();
+
+    return ret;
 }