1 files changed, 98 insertions, 81 deletions

diff --git a/firmware/target/arm/imx31/gigabeat-s/ata-imx31.c b/firmware/target/arm/imx31/gigabeat-s/ata-imx31.c
index 2272b2c929..2c362edbb7 100644
--- a/firmware/target/arm/imx31/gigabeat-s/ata-imx31.c
+++ b/firmware/target/arm/imx31/gigabeat-s/ata-imx31.c
@@ -23,8 +23,82 @@
 #include "system.h"
 #include "power.h"
 #include "panic.h"
-#include "pcf50606.h"
-#include "ata-target.h"
+#include "ata.h"
+#include "clkctl-imx31.h"
+
+static const struct ata_pio_timings
+{
+    uint16_t time_2w;       /* t2 during write */
+    uint16_t time_2r;       /* t2 during read */
+    uint8_t  time_1;        /* t1 */
+    uint8_t  time_pio_rdx;  /* trd */
+    uint8_t  time_4;        /* t4 */
+    uint8_t  time_9;        /* t9 */
+} pio_timings[5] =
+{
+    [0] =  /* PIO mode 0 */
+    {
+        .time_1 = 70,
+        .time_2w = 290,
+        .time_2r = 290,
+        .time_4 = 30,
+        .time_9 = 20
+    },
+    [1] = /* PIO mode 1 */
+    {
+        .time_1 = 50,
+        .time_2w = 290,
+        .time_2r = 290,
+        .time_4 = 20,
+        .time_9 = 15
+    },
+    [2] = /* PIO mode 2 */
+    {
+        .time_1 = 30,
+        .time_2w = 290,
+        .time_2r = 290,
+        .time_4 = 15,
+        .time_9 = 10
+    },
+    [3] = /* PIO mode 3 */
+    {
+        .time_1 = 30,
+        .time_2w = 80,
+        .time_2r = 80,
+        .time_4 = 10,
+        .time_9 = 10
+    },
+    [4] = /* PIO mode 4 */
+    {
+        .time_1 = 25,
+        .time_2w = 70,
+        .time_2r = 70,
+        .time_4 = 10,
+        .time_9 = 10
+    },
+};
+
+/* Setup the timing for PIO mode */
+static void ata_set_pio_mode(int mode)
+{
+    const struct ata_pio_timings * const timings = &pio_timings[mode];
+
+    /* T = period in nanoseconds */
+    int T = 1000 * 1000 * 1000 / imx31_clkctl_get_ata_clk();
+
+    while (!(ATA_INTERRUPT_PENDING & ATA_CONTROLLER_IDLE));
+
+    ATA_TIME_OFF = 3;
+    ATA_TIME_ON = 3;
+
+    ATA_TIME_1 = (timings->time_1 + T) / T;
+    ATA_TIME_2W = (timings->time_2w + T) / T;
+    ATA_TIME_2R = (timings->time_2r + T) / T;
+    ATA_TIME_AX = (35 + T) / T; /* tA */
+    ATA_TIME_PIO_RDX = 1;
+    ATA_TIME_4 = (timings->time_4 + T) / T;
+    ATA_TIME_9 = (timings->time_9 + T) / T;
+}
 
 void ata_reset(void)
 {
@@ -47,92 +121,35 @@ bool ata_is_coldstart(void)
     return true;
 }
 
-unsigned long get_pll(bool serial) {
-    unsigned long mfi, mfn, mfd, pdf, ref_clk;
-    unsigned long reg = 0, ccmr;
-    unsigned long long temp;
-    unsigned int prcs;
-
-    ccmr = CLKCTL_CCMR;
-    prcs = (ccmr & 0x6) >> 1;
-    if(prcs == 0x1) {
-        ref_clk = 32768 * 1024;
-    } else {
-        ref_clk = 27000000;
-    }
-
-    if(serial) {
-        reg = CLKCTL_SPCTL;
-    } else {
-        if((ccmr & 0x8) == 0)
-            return ref_clk;
-        if((ccmr & 0x80) != 0)
-            return ref_clk;
-        reg = CLKCTL_MPCTL;
-    }
-    pdf = (reg & (0x7 << 26)) >> 26;
-    mfd = (reg & (0x3FF << 16)) >> 16;
-    mfi = (reg & (0xF << 10)) >> 10;
-    mfi = (mfi <= 5) ? 5 : mfi;
-    mfn = (reg & 0x3FF);
-
-    if(mfn < 0x200) {
-        temp = (unsigned long long)2 *ref_clk * mfn;
-        temp /= (mfd + 1);
-        temp = (unsigned long long)2 *ref_clk * mfi + temp;
-        temp /= (pdf + 1);
-    } else {
-        temp = (unsigned long long)2 *ref_clk * (0x400 - mfn);
-        temp /= (mfd + 1);
-        temp = (unsigned long long)2 *ref_clk * mfi - temp;
-        temp /= (pdf + 1);
-
-    }
-    return (unsigned long)temp;
-}
-
-unsigned long get_ata_clock(void) {
-    unsigned long pll, ret_val, hclk, max_pdf, ipg_pdf, mcu_pdf;
-
-    max_pdf = (CLKCTL_PDR0 & (0x7 << 3)) >> 3;
-    ipg_pdf = (CLKCTL_PDR0 & (0x3 << 6)) >> 6;
-    mcu_pdf = (CLKCTL_PDR0 & 0x7);
-    if((CLKCTL_PMCR0 & 0xC0000000 ) == 0) {
-        pll = get_pll(true);
-    } else {
-        pll = get_pll(false);
-    }
-    hclk = pll/(max_pdf + 1);
-    ret_val = hclk / (ipg_pdf + 1);
-
-    return ret_val;
-}
-
 void ata_device_init(void)
 {
     ATA_INTF_CONTROL |= ATA_ATA_RST; /* Make sure we're not in reset mode */
 
     while (!(ATA_INTERRUPT_PENDING & ATA_CONTROLLER_IDLE));
 
-    /* Setup the timing for PIO mode */
-    int T = 1000 * 1000 * 1000 / get_ata_clock();
-    ATA_TIME_OFF = 3;
-    ATA_TIME_ON = 3;
-
-    ATA_TIME_1 = (T + 70)/T;
-    ATA_TIME_2W = (T + 290)/T;
-    ATA_TIME_2R = (T + 290)/T;
-    ATA_TIME_AX = (T + 50)/T;
-    ATA_TIME_PIO_RDX = 1;
-    ATA_TIME_4 = (T + 30)/T;
-    ATA_TIME_9 = (T + 20)/T;
+    /* Setup mode 0 by default */
+    ata_set_pio_mode(0);
 }
 
-#if 0
-#if !defined(BOOTLOADER)
-void copy_write_sectors(const unsigned char* buf, int wordcount)
+void ata_identify_ready(void)
 {
-    (void)buf; (void)wordcount;
+    const unsigned short* identify_info = ata_get_identify();
+    int mode = 0;
+
+    if (identify_info[53] & (1 << 1))
+    {
+        /* Set up advanced timings */
+        if (identify_info[64] & (1 << 1))
+            mode = 4; /* Mode 0, 1, 2, 3, 4 */
+        else if (identify_info[64] & (1 << 0))
+            mode = 3; /* Mode 0, 1, 2, 3 */
+        else
+            mode = 2; /* Mode 0, 1, 2 */
+    }
+
+    /* If mode changed, actually set the timings */
+    if (mode != 0)
+    {
+        ata_set_pio_mode(mode);
+    }
 }
-#endif
-#endif