1 files changed, 51 insertions, 4 deletions

diff --git a/arch/blackfin/mach-bf548/head.S b/arch/blackfin/mach-bf548/head.S
index 74b34c7f3629..74fe258421a5 100644
--- a/arch/blackfin/mach-bf548/head.S
+++ b/arch/blackfin/mach-bf548/head.S
@@ -298,8 +298,8 @@ ENTRY(_start_dma_code)
         w[p0] = r0.l;
         ssync;
 
-        p0.h = hi(SIC_IWR);
-        p0.l = lo(SIC_IWR);
+        p0.h = hi(SIC_IWR0);
+        p0.l = lo(SIC_IWR0);
         r0.l = 0x1;
         r0.h = 0x0;
         [p0] = r0;
@@ -324,12 +324,25 @@ ENTRY(_start_dma_code)
         w[p0] = r0.l;
         ssync;
 
+#if defined(CONFIG_BF54x)
+        P2.H = hi(EBIU_RSTCTL);
+        P2.L = lo(EBIU_RSTCTL);
+        R0 = [P2];
+        BITSET (R0, 3);
+#else
         P2.H = hi(EBIU_SDGCTL);
         P2.L = lo(EBIU_SDGCTL);
         R0 = [P2];
         BITSET (R0, 24);
+#endif
         [P2] = R0;
         SSYNC;
+#if defined(CONFIG_BF54x)
+.LSRR_MODE:
+        R0 = [P2];
+        CC = BITTST(R0, 4);
+        if !CC JUMP .LSRR_MODE;
+#endif
 
         r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
         r0 = r0 << 9;                    /* Shift it over,                  */
@@ -361,6 +374,39 @@ ENTRY(_start_dma_code)
         w[p0] = r0.l;
         ssync;
 
+#if defined(CONFIG_BF54x)
+        P2.H = hi(EBIU_RSTCTL);
+        P2.L = lo(EBIU_RSTCTL);
+        R0 = [P2];
+        CC = BITTST(R0, 0);
+        if CC jump .Lskipddrrst;
+        BITSET (R0, 0);
+.Lskipddrrst:
+        BITCLR (R0, 3);
+        [P2] = R0;
+        SSYNC;
+
+        p0.l = lo(EBIU_DDRCTL0);
+        p0.h = hi(EBIU_DDRCTL0);
+        r0.l = lo(mem_DDRCTL0);
+        r0.h = hi(mem_DDRCTL0);
+        [p0] = r0;
+        ssync;
+
+        p0.l = lo(EBIU_DDRCTL1);
+        p0.h = hi(EBIU_DDRCTL1);
+        r0.l = lo(mem_DDRCTL1);
+        r0.h = hi(mem_DDRCTL1);
+        [p0] = r0;
+        ssync;
+
+        p0.l = lo(EBIU_DDRCTL2);
+        p0.h = hi(EBIU_DDRCTL2);
+        r0.l = lo(mem_DDRCTL2);
+        r0.h = hi(mem_DDRCTL2);
+        [p0] = r0;
+        ssync;
+#else
         p0.l = lo(EBIU_SDRRC);
         p0.h = hi(EBIU_SDRRC);
         r0 = mem_SDRRC;
@@ -394,9 +440,10 @@ ENTRY(_start_dma_code)
         R1 = R1 | R0;
         [P2] = R1;
         SSYNC;
+#endif
 
-        p0.h = hi(SIC_IWR);
-        p0.l = lo(SIC_IWR);
+        p0.h = hi(SIC_IWR0);
+        p0.l = lo(SIC_IWR0);
         r0.l = lo(IWR_ENABLE_ALL);
         r0.h = hi(IWR_ENABLE_ALL);
         [p0] = r0;