Add an ar7 tnetd7200 fix by DerAgo, thanks !

git-svn-id: svn://svn.openwrt.org/openwrt/trunk@8036 3c298f89-4303-0410-b956-a3cf2f4a3e73

2 files changed, 144 insertions, 36 deletions

diff --git a/target/linux/ar7-2.6/files/arch/mips/ar7/clock.c b/target/linux/ar7-2.6/files/arch/mips/ar7/clock.c
index 43e36ec90..5010b549e 100644
--- a/target/linux/ar7-2.6/files/arch/mips/ar7/clock.c
+++ b/target/linux/ar7-2.6/files/arch/mips/ar7/clock.c
@@ -40,6 +40,14 @@
 #define BOOT_PLL_ASYNC_MODE 0x02000000
 #define BOOT_PLL_2TO1_MODE 0x00008000
 
+#define TNETD7200_CLOCK_ID_CPU 0
+#define TNETD7200_CLOCK_ID_DSP 1
+#define TNETD7200_CLOCK_ID_USB 2
+
+#define TNETD7200_DEF_CPU_CLK 211000000
+#define TNETD7200_DEF_DSP_CLK 125000000
+#define TNETD7200_DEF_USB_CLK 48000000
+
 struct tnetd7300_clock {
 	volatile u32 ctrl;
 #define PREDIV_MASK 0x001f0000
@@ -70,18 +78,19 @@ struct tnetd7200_clock {
 	volatile u32 mul;
 	volatile u32 prediv;
 	volatile u32 postdiv;
-	u32 unused2[7];
+	volatile u32 postdiv2;
+	u32 unused2[6];
 	volatile u32 cmd;
 	volatile u32 status;
 	volatile u32 cmden;
 	u32 padding[15];
-};
+} __attribute__ ((packed));
 
 struct tnetd7200_clocks {
 	struct tnetd7200_clock cpu;
 	struct tnetd7200_clock dsp;
 	struct tnetd7200_clock usb;
-};
+} __attribute__ ((packed));
 
 int ar7_afe_clock = 35328000;
 int ar7_ref_clock = 25000000;
@@ -289,55 +298,155 @@ static int tnetd7200_get_clock(int base, struct tnetd7200_clock *clock,
 	return base * ((clock->mul & 0xf) + 1) / divisor;
 }
 
+
 static void tnetd7200_set_clock(int base, struct tnetd7200_clock *clock,
-				u32 *bootcr, u32 frequency) 
+				int prediv, int postdiv, int postdiv2, int mul, u32 frequency) 
 {
-	u32 status;
-	int prediv, postdiv, mul;
+    printk("Clocks: base = %d, frequency = %u, prediv = %d, postdiv = %d, postdiv2 = %d, mul = %d\n",
+        base, frequency, prediv, postdiv, postdiv2, mul);
 
-	calculate(base, frequency, &prediv, &postdiv, &mul);
+    clock->ctrl = 0;
+	clock->prediv = DIVISOR_ENABLE_MASK | ((prediv - 1) & 0x1F);
+    clock->mul = ((mul - 1) & 0xF);
 
-	clock->ctrl = 0;
-	clock->prediv = DIVISOR_ENABLE_MASK | prediv;
-	clock->mul = mul;
-	mdelay(1);
-	do {
-		status = clock->status;
-	} while (status & PLL_STATUS);
-	clock->postdiv = DIVISOR_ENABLE_MASK | postdiv;
-	clock->cmden = 1;
-	clock->cmd = 1;
-	do {
-		status = clock->status;
-	} while (status & PLL_STATUS);
-	clock->ctrl = 1;
+    for(mul = 0; mul < 2000; mul++) /* nop */;
+
+    while(clock->status & 0x1) /* nop */;
+
+    clock->postdiv = DIVISOR_ENABLE_MASK | ((postdiv - 1) & 0x1F);
+
+    clock->cmden |= 1;
+    clock->cmd |= 1;
+
+    while(clock->status & 0x1) /* nop */;
+
+    clock->postdiv2 = DIVISOR_ENABLE_MASK | ((postdiv2 - 1) & 0x1F);
+
+    clock->cmden |= 1;
+    clock->cmd |= 1;
+
+    while(clock->status & 0x1) /* nop */;
+
+    clock->ctrl |= 1;
+}
+
+static int tnetd7200_get_clock_base(int clock_id, u32 *bootcr)
+{
+    if (*bootcr & BOOT_PLL_ASYNC_MODE) {
+        // Async
+        switch (clock_id) {
+        case TNETD7200_CLOCK_ID_DSP:
+            return ar7_ref_clock;
+        default:
+            return ar7_afe_clock;
+        }
+    } else {
+        // Sync
+		if (*bootcr & BOOT_PLL_2TO1_MODE) {
+            // 2:1
+            switch (clock_id) {
+            case TNETD7200_CLOCK_ID_DSP:
+                return ar7_ref_clock;
+            default:
+                return ar7_afe_clock;
+            }
+        } else {
+            // 1:1
+            return ar7_ref_clock;
+        }
+    }
 }
 
+
 static void __init tnetd7200_init_clocks(void)
 {
 	u32 *bootcr = (u32 *)ioremap_nocache(AR7_REGS_DCL, 4);
 	struct tnetd7200_clocks *clocks = (struct tnetd7200_clocks *)ioremap_nocache(AR7_REGS_POWER + 0x80, sizeof(struct tnetd7200_clocks)); 
-
-	ar7_cpu_clock = tnetd7200_get_clock(ar7_afe_clock,
-					    &clocks->cpu,
-					    bootcr, ar7_afe_clock);
+    int cpu_base, cpu_mul, cpu_prediv, cpu_postdiv;
+    int dsp_base, dsp_mul, dsp_prediv, dsp_postdiv;
+    int usb_base, usb_mul, usb_prediv, usb_postdiv;
+
+    /*
+        Log from Fritz!Box 7170 Annex B:
+
+        CPU revision is: 00018448
+        Clocks: Async mode
+        Clocks: Setting DSP clock
+        Clocks: prediv: 1, postdiv: 1, mul: 5
+        Clocks: base = 25000000, frequency = 125000000, prediv = 1, postdiv = 2, postdiv2 = 1, mul = 10
+        Clocks: Setting CPU clock
+        Adjusted requested frequency 211000000 to 211968000
+        Clocks: prediv: 1, postdiv: 1, mul: 6
+        Clocks: base = 35328000, frequency = 211968000, prediv = 1, postdiv = 1, postdiv2 = -1, mul = 6
+        Clocks: Setting USB clock
+        Adjusted requested frequency 48000000 to 48076920
+        Clocks: prediv: 13, postdiv: 1, mul: 5
+        Clocks: base = 125000000, frequency = 48000000, prediv = 13, postdiv = 1, postdiv2 = -1, mul = 5
+
+        DSL didn't work if you didn't set the postdiv 2:1 postdiv2 combination, driver hung on startup.
+        Haven't tested this on a synchronous board, neither do i know what to do with ar7_dsp_clock
+    */
+
+    cpu_base = tnetd7200_get_clock_base(TNETD7200_CLOCK_ID_CPU, bootcr);
+    dsp_base = tnetd7200_get_clock_base(TNETD7200_CLOCK_ID_DSP, bootcr);
 
 	if (*bootcr & BOOT_PLL_ASYNC_MODE) {
-		ar7_bus_clock = 125000000;
+        printk("Clocks: Async mode\n");
+
+        printk("Clocks: Setting DSP clock\n");
+        calculate(dsp_base, TNETD7200_DEF_DSP_CLK, &dsp_prediv, &dsp_postdiv, &dsp_mul);
+        ar7_bus_clock = ((dsp_base / dsp_prediv) * dsp_mul) / dsp_postdiv;
+        tnetd7200_set_clock(dsp_base, &clocks->dsp, 
+            dsp_prediv, dsp_postdiv * 2, dsp_postdiv, dsp_mul * 2, 
+            ar7_bus_clock);
+
+        printk("Clocks: Setting CPU clock\n");
+        calculate(cpu_base, TNETD7200_DEF_CPU_CLK, &cpu_prediv, &cpu_postdiv, &cpu_mul);
+        ar7_cpu_clock = ((cpu_base / cpu_prediv) * cpu_mul) / cpu_postdiv;
+        tnetd7200_set_clock(cpu_base, &clocks->cpu, 
+            cpu_prediv, cpu_postdiv, -1, cpu_mul, 
+            ar7_cpu_clock);
+
 	} else {
 		if (*bootcr & BOOT_PLL_2TO1_MODE) {
-			ar7_bus_clock = ar7_cpu_clock / 2;
+            printk("Clocks: Sync 2:1 mode\n");
+
+            printk("Clocks: Setting CPU clock\n");
+            calculate(cpu_base, TNETD7200_DEF_CPU_CLK, &cpu_prediv, &cpu_postdiv, &cpu_mul);
+            ar7_cpu_clock = ((cpu_base / cpu_prediv) * cpu_mul) / cpu_postdiv;
+            tnetd7200_set_clock(cpu_base, &clocks->cpu, 
+                cpu_prediv, cpu_postdiv, -1, cpu_mul, 
+                ar7_cpu_clock);
+
+            printk("Clocks: Setting DSP clock\n");
+            calculate(dsp_base, TNETD7200_DEF_DSP_CLK, &dsp_prediv, &dsp_postdiv, &dsp_mul);
+            ar7_bus_clock = ar7_cpu_clock / 2;
+            tnetd7200_set_clock(dsp_base, &clocks->dsp, 
+                dsp_prediv, dsp_postdiv * 2, dsp_postdiv, dsp_mul * 2, 
+                ar7_bus_clock);
 		} else {
-			ar7_bus_clock = ar7_cpu_clock;
+            printk("Clocks: Sync 1:1 mode\n");
+
+            printk("Clocks: Setting DSP clock\n");
+            calculate(dsp_base, TNETD7200_DEF_CPU_CLK, &dsp_prediv, &dsp_postdiv, &dsp_mul);
+            ar7_bus_clock = ((dsp_base / dsp_prediv) * dsp_mul) / dsp_postdiv;
+            tnetd7200_set_clock(dsp_base, &clocks->dsp, 
+                dsp_prediv, dsp_postdiv * 2, dsp_postdiv, dsp_mul * 2, 
+                ar7_bus_clock);
+
+            ar7_cpu_clock = ar7_bus_clock;
 		}
 	}
 
-	tnetd7200_set_clock(ar7_ref_clock * 5, &clocks->usb,
-			    bootcr, 48000000);
+    printk("Clocks: Setting USB clock\n");
+    usb_base = ar7_bus_clock;
+    calculate(usb_base, TNETD7200_DEF_USB_CLK, &usb_prediv, &usb_postdiv, &usb_mul);
+    tnetd7200_set_clock(usb_base, &clocks->usb, 
+        usb_prediv, usb_postdiv, -1, usb_mul, 
+        TNETD7200_DEF_USB_CLK);
 
-	if (ar7_dsp_clock == 250000000)
-		tnetd7200_set_clock(ar7_ref_clock, &clocks->dsp,
-				    bootcr, ar7_dsp_clock);
+#warning FIXME: ????! Hrmm
+    ar7_dsp_clock = ar7_cpu_clock;
 
 	iounmap(clocks);
 	iounmap(bootcr);
@@ -347,11 +456,10 @@ void __init ar7_init_clocks(void)
 {
 	switch (ar7_chip_id()) {
 	case AR7_CHIP_7100:
+#warning FIXME: Check if the new 7200 clock init works for 7100
 		tnetd7200_init_clocks();
 		break;
 	case AR7_CHIP_7200:
-#warning FIXME: check revision
-		ar7_dsp_clock = 250000000;
 		tnetd7200_init_clocks();
 		break;
 	case AR7_CHIP_7300:
diff --git a/target/linux/ar7-2.6/files/arch/mips/ar7/setup.c b/target/linux/ar7-2.6/files/arch/mips/ar7/setup.c
index 3b5215d7e..6c9c1bf7b 100644
--- a/target/linux/ar7-2.6/files/arch/mips/ar7/setup.c
+++ b/target/linux/ar7-2.6/files/arch/mips/ar7/setup.c
@@ -110,7 +110,7 @@ void __init plat_mem_setup(void)
 
 	prom_meminit();
 #warning FIXME: clock initialisation
-	//ar7_init_clocks();
+	ar7_init_clocks();
 
 	ioport_resource.start = 0;
 	ioport_resource.end   = ~0;