karo: merge with Ka-Ro specific tree for secure boot support

[karo-tx-uboot.git] / arch / arm / cpu / arm926ejs / mxs / spl_mem_init.c

diff --git a/arch/arm/cpu/arm926ejs/mxs/spl_mem_init.c b/arch/arm/cpu/arm926ejs/mxs/spl_mem_init.c
index 97ef67d8c5843b06fcd6c7d903cd3d4cb21d976b..d5842e68133e880ba142c65b45512f9912827a54 100644 (file)
--- a/arch/arm/cpu/arm926ejs/mxs/spl_mem_init.c
+++ b/arch/arm/cpu/arm926ejs/mxs/spl_mem_init.c
@@ -158,18 +158,17 @@ static void mxs_mem_init_clock(void)
        writeb(CLKCTRL_FRAC_CLKGATE,
                &clkctrl_regs->hw_clkctrl_frac0_clr[CLKCTRL_FRAC0_EMI]);
 
-       early_delay(11000);
 
        /* Set EMI clock divider for EMI clock to 411 / 2 = 205MHz */
        writel((2 << CLKCTRL_EMI_DIV_EMI_OFFSET) |
                (1 << CLKCTRL_EMI_DIV_XTAL_OFFSET),
                &clkctrl_regs->hw_clkctrl_emi);
+       while (readl(&clkctrl_regs->hw_clkctrl_emi) & CLKCTRL_EMI_BUSY_REF_EMI)
+               ;
 
        /* Unbypass EMI */
        writel(CLKCTRL_CLKSEQ_BYPASS_EMI,
                &clkctrl_regs->hw_clkctrl_clkseq_clr);
-
-       early_delay(10000);
 }
 
 static void mxs_mem_setup_cpu_and_hbus(void)
@@ -187,49 +186,51 @@ static void mxs_mem_setup_cpu_and_hbus(void)
                &clkctrl_regs->hw_clkctrl_clkseq_set);
 
        /* HBUS = 151MHz */
-       writel(CLKCTRL_HBUS_DIV_MASK, &clkctrl_regs->hw_clkctrl_hbus_set);
-       writel(((~3) << CLKCTRL_HBUS_DIV_OFFSET) & CLKCTRL_HBUS_DIV_MASK,
-               &clkctrl_regs->hw_clkctrl_hbus_clr);
-
-       early_delay(10000);
+       clrsetbits_le32(&clkctrl_regs->hw_clkctrl_hbus,
+                       CLKCTRL_HBUS_DIV_MASK,
+                       3 << CLKCTRL_HBUS_DIV_OFFSET);
+       while (readl(&clkctrl_regs->hw_clkctrl_hbus) & CLKCTRL_HBUS_ASM_BUSY)
+               ;
 
        /* CPU clock divider = 1 */
        clrsetbits_le32(&clkctrl_regs->hw_clkctrl_cpu,
-                       CLKCTRL_CPU_DIV_CPU_MASK, 1);
+                       CLKCTRL_CPU_DIV_CPU_MASK,
+                       1 << CLKCTRL_CPU_DIV_CPU_OFFSET);
+       while (readl(&clkctrl_regs->hw_clkctrl_cpu) & CLKCTRL_CPU_BUSY_REF_CPU)
+               ;
 
        /* Disable CPU bypass */
        writel(CLKCTRL_CLKSEQ_BYPASS_CPU,
                &clkctrl_regs->hw_clkctrl_clkseq_clr);
-
-       early_delay(15000);
 }
 
-static void mxs_mem_setup_vdda(void)
+void data_abort_memdetect_handler(void)
 {
-       struct mxs_power_regs *power_regs =
-               (struct mxs_power_regs *)MXS_POWER_BASE;
-
-       writel((0xc << POWER_VDDACTRL_TRG_OFFSET) |
-               (0x7 << POWER_VDDACTRL_BO_OFFSET_OFFSET) |
-               POWER_VDDACTRL_LINREG_OFFSET_1STEPS_BELOW,
-               &power_regs->hw_power_vddactrl);
+       asm volatile("subs pc, lr, #4");
 }
 
 uint32_t mxs_mem_get_size(void)
 {
        uint32_t sz, da;
        uint32_t *vt = (uint32_t *)0x20;
-       /* The following is "subs pc, r14, #4", used as return from DABT. */
-       const uint32_t data_abort_memdetect_handler = 0xe25ef004;
+       unsigned long cr;
+
+       /* move vector table to low memory */
+       asm volatile(
+               "mrc p15, 0, %0, c1, c0, 0\n"
+               "bic r7, %0, #(1 << 13)\n"
+               "mcr p15, 0, r7, c1, c0, 0\n"
+               : "=r"(cr) : : "r7");
 
        /* Replace the DABT handler. */
        da = vt[4];
-       vt[4] = data_abort_memdetect_handler;
+       vt[4] = (uint32_t)data_abort_memdetect_handler;
 
        sz = get_ram_size((long *)PHYS_SDRAM_1, PHYS_SDRAM_1_SIZE);
 
        /* Restore the old DABT handler. */
        vt[4] = da;
+       asm volatile("mcr p15, 0, %0, c1, c0, 0\n" : : "r"(cr));
 
        return sz;
 }
@@ -328,15 +329,11 @@ void mxs_mem_init(void)
 
        mxs_mem_init_clock();
 
-       mxs_mem_setup_vdda();
-
 #if defined(CONFIG_MX23)
        mx23_mem_init();
 #elif defined(CONFIG_MX28)
        mx28_mem_init();
 #endif
 
-       early_delay(10000);
-
        mxs_mem_setup_cpu_and_hbus();
 }