Blackfin arch: unify the duplicated portions of __start and split mach-specific pieces into _mach_early_start where they will be easier to trim over time

Signed-off-by: Mike Frysinger <vapier.adi@gmail.com>
Signed-off-by: Bryan Wu <cooloney@kernel.org>

6 files changed, 146 insertions, 649 deletions

diff --git a/arch/blackfin/mach-bf527/head.S b/arch/blackfin/mach-bf527/head.S
index 9173dcecd158..af20183d0d94 100644
--- a/arch/blackfin/mach-bf527/head.S
+++ b/arch/blackfin/mach-bf527/head.S
@@ -30,93 +30,16 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
-#include <asm/trace.h>
-
 #ifdef CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach-common/clocks.h>
 #include <asm/mach/mem_init.h>
 #endif
 
-.extern ___bss_stop
-.extern ___bss_start
 .extern _bf53x_relocate_l1_mem
 
-#define INITIAL_STACK	0xFFB01000
-
 __INIT
 
-ENTRY(__start)
-	/* R0: argument of command line string, passed from uboot, save it */
-	R7 = R0;
-	/* Enable Cycle Counter and Nesting Of Interrupts */
-#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
-	R0 = SYSCFG_SNEN;
-#else
-	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
-#endif
-	SYSCFG = R0;
-	R0 = 0;
-
-	/* Clear Out All the data and pointer Registers */
-	R1 = R0;
-	R2 = R0;
-	R3 = R0;
-	R4 = R0;
-	R5 = R0;
-	R6 = R0;
-
-	P0 = R0;
-	P1 = R0;
-	P2 = R0;
-	P3 = R0;
-	P4 = R0;
-	P5 = R0;
-
-	LC0 = r0;
-	LC1 = r0;
-	L0 = r0;
-	L1 = r0;
-	L2 = r0;
-	L3 = r0;
-
-	/* Clear Out All the DAG Registers */
-	B0 = r0;
-	B1 = r0;
-	B2 = r0;
-	B3 = r0;
-
-	I0 = r0;
-	I1 = r0;
-	I2 = r0;
-	I3 = r0;
-
-	M0 = r0;
-	M1 = r0;
-	M2 = r0;
-	M3 = r0;
-
-	trace_buffer_init(p0,r0);
-	P0 = R1;
-	R0 = R1;
-
-	/* Turn off the icache */
-	p0.l = LO(IMEM_CONTROL);
-	p0.h = HI(IMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENICPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
-	/* Turn off the dcache */
-	p0.l = LO(DMEM_CONTROL);
-	p0.h = HI(DMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENDCPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
+ENTRY(_mach_early_start)
 #if defined(CONFIG_BF527)
 	p0.h = hi(EMAC_SYSTAT);
 	p0.l = lo(EMAC_SYSTAT);
@@ -152,57 +75,8 @@ ENTRY(__start)
 	w[p0] = r0.L;	/* To enable UART clock */
 	ssync;
 
-	/* Initialize stack pointer */
-	sp.l = lo(INITIAL_STACK);
-	sp.h = hi(INITIAL_STACK);
-	fp = sp;
-	usp = sp;
-
-#ifdef CONFIG_EARLY_PRINTK
-	SP += -12;
-	call _init_early_exception_vectors;
-	SP += 12;
-#endif
-
-	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
-	call _bf53x_relocate_l1_mem;
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-	call _start_dma_code;
-#endif
-
-	/* This section keeps the processor in supervisor mode
-	 * during kernel boot.  Switches to user mode at end of boot.
-	 * See page 3-9 of Hardware Reference manual for documentation.
-	 */
-
-	/* EVT15 = _real_start */
-
-	p0.l = lo(EVT15);
-	p0.h = hi(EVT15);
-	p1.l = _real_start;
-	p1.h = _real_start;
-	[p0] = p1;
-	csync;
-
-	p0.l = lo(IMASK);
-	p0.h = hi(IMASK);
-	p1.l = IMASK_IVG15;
-	p1.h = 0x0;
-	[p0] = p1;
-	csync;
-
-	raise 15;
-	p0.l = .LWAIT_HERE;
-	p0.h = .LWAIT_HERE;
-	reti = p0;
-#if ANOMALY_05000281
-	nop; nop; nop;
-#endif
-	rti;
-
-.LWAIT_HERE:
-	jump .LWAIT_HERE;
-ENDPROC(__start)
+	rts;
+ENDPROC(_mach_early_start)
 
 __FINIT
 
diff --git a/arch/blackfin/mach-bf533/head.S b/arch/blackfin/mach-bf533/head.S
index 7f0a7a0c6fd6..6603967367ec 100644
--- a/arch/blackfin/mach-bf533/head.S
+++ b/arch/blackfin/mach-bf533/head.S
@@ -30,74 +30,16 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
-#include <asm/trace.h>
 #ifdef CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach-common/clocks.h>
 #include <asm/mach/mem_init.h>
 #endif
 
-.extern ___bss_stop
-.extern ___bss_start
 .extern _bf53x_relocate_l1_mem
 
-#define INITIAL_STACK	0xFFB01000
-
 __INIT
 
-ENTRY(__start)
-	/* R0: argument of command line string, passed from uboot, save it */
-	R7 = R0;
-	/* Enable Cycle Counter and Nesting Of Interrupts */
-#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
-	R0 = SYSCFG_SNEN;
-#else
-	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
-#endif
-	SYSCFG = R0;
-	R0 = 0;
-
-	/* Clear Out All the data and pointer Registers */
-	R1 = R0;
-	R2 = R0;
-	R3 = R0;
-	R4 = R0;
-	R5 = R0;
-	R6 = R0;
-
-	P0 = R0;
-	P1 = R0;
-	P2 = R0;
-	P3 = R0;
-	P4 = R0;
-	P5 = R0;
-
-	LC0 = r0;
-	LC1 = r0;
-	L0 = r0;
-	L1 = r0;
-	L2 = r0;
-	L3 = r0;
-
-	/* Clear Out All the DAG Registers */
-	B0 = r0;
-	B1 = r0;
-	B2 = r0;
-	B3 = r0;
-
-	I0 = r0;
-	I1 = r0;
-	I2 = r0;
-	I3 = r0;
-
-	M0 = r0;
-	M1 = r0;
-	M2 = r0;
-	M3 = r0;
-
-	trace_buffer_init(p0,r0);
-	P0 = R1;
-	R0 = R1;
-
+ENTRY(_mach_early_start)
 	p0.h = hi(FIO_MASKA_C);
 	p0.l = lo(FIO_MASKA_C);
 	r0 = 0xFFFF(Z);
@@ -110,24 +52,6 @@ ENTRY(__start)
 	w[p0] = r0.L;	/* Disable all interrupts */
 	ssync;
 
-	/* Turn off the icache */
-	p0.l = LO(IMEM_CONTROL);
-	p0.h = HI(IMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENICPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
-	/* Turn off the dcache */
-	p0.l = LO(DMEM_CONTROL);
-	p0.h = HI(DMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENDCPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
 	/* Initialise UART - when booting from u-boot, the UART is not disabled
 	 * so if we dont initalize here, our serial console gets hosed */
 	p0.h = hi(BFIN_UART_LCR);
@@ -154,57 +78,8 @@ ENTRY(__start)
 	w[p0] = r0.L;	/* To enable UART clock */
 	ssync;
 
-	/* Initialize stack pointer */
-	sp.l = lo(INITIAL_STACK);
-	sp.h = hi(INITIAL_STACK);
-	fp = sp;
-	usp = sp;
-
-#ifdef CONFIG_EARLY_PRINTK
-	SP += -12;
-	call _init_early_exception_vectors;
-	SP += 12;
-#endif
-
-	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
-	call _bf53x_relocate_l1_mem;
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-	call _start_dma_code;
-#endif
-
-	/* This section keeps the processor in supervisor mode
-	 * during kernel boot.  Switches to user mode at end of boot.
-	 * See page 3-9 of Hardware Reference manual for documentation.
-	 */
-
-	/* EVT15 = _real_start */
-
-	p0.l = lo(EVT15);
-	p0.h = hi(EVT15);
-	p1.l = _real_start;
-	p1.h = _real_start;
-	[p0] = p1;
-	csync;
-
-	p0.l = lo(IMASK);
-	p0.h = hi(IMASK);
-	p1.l = IMASK_IVG15;
-	p1.h = 0x0;
-	[p0] = p1;
-	csync;
-
-	raise 15;
-	p0.l = .LWAIT_HERE;
-	p0.h = .LWAIT_HERE;
-	reti = p0;
-#if ANOMALY_05000281
-	nop; nop; nop;
-#endif
-	rti;
-
-.LWAIT_HERE:
-	jump .LWAIT_HERE;
-ENDPROC(__start)
+	rts;
+ENDPROC(_mach_early_start)
 
 __FINIT
 
diff --git a/arch/blackfin/mach-bf537/head.S b/arch/blackfin/mach-bf537/head.S
index c062acb04836..6a02e472587a 100644
--- a/arch/blackfin/mach-bf537/head.S
+++ b/arch/blackfin/mach-bf537/head.S
@@ -30,93 +30,16 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
-#include <asm/trace.h>
-
 #ifdef CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach-common/clocks.h>
 #include <asm/mach/mem_init.h>
 #endif
 
-.extern ___bss_stop
-.extern ___bss_start
 .extern _bf53x_relocate_l1_mem
 
-#define INITIAL_STACK	0xFFB01000
-
 __INIT
 
-ENTRY(__start)
-	/* R0: argument of command line string, passed from uboot, save it */
-	R7 = R0;
-	/* Enable Cycle Counter and Nesting Of Interrupts */
-#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
-	R0 = SYSCFG_SNEN;
-#else
-	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
-#endif
-	SYSCFG = R0;
-	R0 = 0;
-
-	/* Clear Out All the data and pointer Registers */
-	R1 = R0;
-	R2 = R0;
-	R3 = R0;
-	R4 = R0;
-	R5 = R0;
-	R6 = R0;
-
-	P0 = R0;
-	P1 = R0;
-	P2 = R0;
-	P3 = R0;
-	P4 = R0;
-	P5 = R0;
-
-	LC0 = r0;
-	LC1 = r0;
-	L0 = r0;
-	L1 = r0;
-	L2 = r0;
-	L3 = r0;
-
-	/* Clear Out All the DAG Registers */
-	B0 = r0;
-	B1 = r0;
-	B2 = r0;
-	B3 = r0;
-
-	I0 = r0;
-	I1 = r0;
-	I2 = r0;
-	I3 = r0;
-
-	M0 = r0;
-	M1 = r0;
-	M2 = r0;
-	M3 = r0;
-
-	trace_buffer_init(p0,r0);
-	P0 = R1;
-	R0 = R1;
-
-	/* Turn off the icache */
-	p0.l = LO(IMEM_CONTROL);
-	p0.h = HI(IMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENICPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
-	/* Turn off the dcache */
-	p0.l = LO(DMEM_CONTROL);
-	p0.h = HI(DMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENDCPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
+ENTRY(_mach_early_start)
 	/* Initialise General-Purpose I/O Modules on BF537 */
 	p0.h = hi(BFIN_PORT_MUX);
 	p0.l = lo(BFIN_PORT_MUX);
@@ -166,57 +89,8 @@ ENTRY(__start)
 	w[p0] = r0.L;	/* To enable UART clock */
 	ssync;
 
-	/* Initialize stack pointer */
-	sp.l = lo(INITIAL_STACK);
-	sp.h = hi(INITIAL_STACK);
-	fp = sp;
-	usp = sp;
-
-#ifdef CONFIG_EARLY_PRINTK
-	SP += -12;
-	call _init_early_exception_vectors;
-	SP += 12;
-#endif
-
-	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
-	call _bf53x_relocate_l1_mem;
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-	call _start_dma_code;
-#endif
-
-	/* This section keeps the processor in supervisor mode
-	 * during kernel boot.  Switches to user mode at end of boot.
-	 * See page 3-9 of Hardware Reference manual for documentation.
-	 */
-
-	/* EVT15 = _real_start */
-
-	p0.l = lo(EVT15);
-	p0.h = hi(EVT15);
-	p1.l = _real_start;
-	p1.h = _real_start;
-	[p0] = p1;
-	csync;
-
-	p0.l = lo(IMASK);
-	p0.h = hi(IMASK);
-	p1.l = IMASK_IVG15;
-	p1.h = 0x0;
-	[p0] = p1;
-	csync;
-
-	raise 15;
-	p0.l = .LWAIT_HERE;
-	p0.h = .LWAIT_HERE;
-	reti = p0;
-#if ANOMALY_05000281
-	nop; nop; nop;
-#endif
-	rti;
-
-.LWAIT_HERE:
-	jump .LWAIT_HERE;
-ENDPROC(__start)
+	rts;
+ENDPROC(_mach_early_start)
 
 __FINIT
 
diff --git a/arch/blackfin/mach-bf548/head.S b/arch/blackfin/mach-bf548/head.S
index 832a8d7212ac..cf94e1e222b8 100644
--- a/arch/blackfin/mach-bf548/head.S
+++ b/arch/blackfin/mach-bf548/head.S
@@ -30,145 +30,18 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
-#include <asm/trace.h>
 #ifdef CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach-common/clocks.h>
 #include <asm/mach/mem_init.h>
 #endif
 
-.extern ___bss_stop
-.extern ___bss_start
 .extern _bf53x_relocate_l1_mem
 
-#define INITIAL_STACK   0xFFB01000
-
 __INIT
 
-ENTRY(__start)
-	/* R0: argument of command line string, passed from uboot, save it */
-	R7 = R0;
-	/* Enable Cycle Counter and Nesting Of Interrupts */
-#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
-	R0 = SYSCFG_SNEN;
-#else
-	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
-#endif
-	SYSCFG = R0;
-	R0 = 0;
-
-	/* Clear Out All the data and pointer  Registers*/
-	R1 = R0;
-	R2 = R0;
-	R3 = R0;
-	R4 = R0;
-	R5 = R0;
-	R6 = R0;
-
-	P0 = R0;
-	P1 = R0;
-	P2 = R0;
-	P3 = R0;
-	P4 = R0;
-	P5 = R0;
-
-	LC0 = r0;
-	LC1 = r0;
-	L0 = r0;
-	L1 = r0;
-	L2 = r0;
-	L3 = r0;
-
-	/* Clear Out All the DAG Registers*/
-	B0 = r0;
-	B1 = r0;
-	B2 = r0;
-	B3 = r0;
-
-	I0 = r0;
-	I1 = r0;
-	I2 = r0;
-	I3 = r0;
-
-	M0 = r0;
-	M1 = r0;
-	M2 = r0;
-	M3 = r0;
-
-	trace_buffer_init(p0,r0);
-	P0 = R1;
-	R0 = R1;
-
-	/* Turn off the icache */
-	p0.l = LO(IMEM_CONTROL);
-	p0.h = HI(IMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENICPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
-	/* Turn off the dcache */
-	p0.l = LO(DMEM_CONTROL);
-	p0.h = HI(DMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENDCPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
-	/* Initialize stack pointer */
-	SP.L = LO(INITIAL_STACK);
-	SP.H = HI(INITIAL_STACK);
-	FP = SP;
-	USP = SP;
-
-#ifdef CONFIG_EARLY_PRINTK
-	SP += -12;
-	call _init_early_exception_vectors;
-	SP += 12;
-#endif
-
-	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
-	call _bf53x_relocate_l1_mem;
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-	call _start_dma_code;
-#endif
-
-	/* This section keeps the processor in supervisor mode
-	 * during kernel boot.  Switches to user mode at end of boot.
-	 * See page 3-9 of Hardware Reference manual for documentation.
-	 */
-
-	/* EVT15 = _real_start */
-
-	p0.l = lo(EVT15);
-	p0.h = hi(EVT15);
-	p1.l = _real_start;
-	p1.h = _real_start;
-	[p0] = p1;
-	csync;
-
-	p0.l = lo(IMASK);
-	p0.h = hi(IMASK);
-	p1.l = IMASK_IVG15;
-	p1.h = 0x0;
-	[p0] = p1;
-	csync;
-
-	raise 15;
-	p0.l = .LWAIT_HERE;
-	p0.h = .LWAIT_HERE;
-	reti = p0;
-#if ANOMALY_05000281
-	nop;
-	nop;
-	nop;
-#endif
-	rti;
-
-.LWAIT_HERE:
-	jump .LWAIT_HERE;
-ENDPROC(__start)
+ENTRY(_mach_early_start)
+	rts;
+ENDPROC(_mach_early_start)
 
 __FINIT
 
diff --git a/arch/blackfin/mach-bf561/head.S b/arch/blackfin/mach-bf561/head.S
index c541b312c25d..fe6f979947c6 100644
--- a/arch/blackfin/mach-bf561/head.S
+++ b/arch/blackfin/mach-bf561/head.S
@@ -30,93 +30,16 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
-#include <asm/trace.h>
-
-#if CONFIG_BFIN_KERNEL_CLOCK
+#ifdef CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach-common/clocks.h>
 #include <asm/mach/mem_init.h>
 #endif
 
-.extern ___bss_stop
-.extern ___bss_start
 .extern _bf53x_relocate_l1_mem
 
-#define INITIAL_STACK	0xFFB01000
-
 __INIT
 
-ENTRY(__start)
-	/* R0: argument of command line string, passed from uboot, save it */
-	R7 = R0;
-	/* Enable Cycle Counter and Nesting Of Interrupts */
-#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
-	R0 = SYSCFG_SNEN;
-#else
-	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
-#endif
-	SYSCFG = R0;
-	R0 = 0;
-
-	/* Clear Out All the data and pointer Registers */
-	R1 = R0;
-	R2 = R0;
-	R3 = R0;
-	R4 = R0;
-	R5 = R0;
-	R6 = R0;
-
-	P0 = R0;
-	P1 = R0;
-	P2 = R0;
-	P3 = R0;
-	P4 = R0;
-	P5 = R0;
-
-	LC0 = r0;
-	LC1 = r0;
-	L0 = r0;
-	L1 = r0;
-	L2 = r0;
-	L3 = r0;
-
-	/* Clear Out All the DAG Registers */
-	B0 = r0;
-	B1 = r0;
-	B2 = r0;
-	B3 = r0;
-
-	I0 = r0;
-	I1 = r0;
-	I2 = r0;
-	I3 = r0;
-
-	M0 = r0;
-	M1 = r0;
-	M2 = r0;
-	M3 = r0;
-
-	trace_buffer_init(p0,r0);
-	P0 = R1;
-	R0 = R1;
-
-	/* Turn off the icache */
-	p0.l = LO(IMEM_CONTROL);
-	p0.h = HI(IMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENICPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
-	/* Turn off the dcache */
-	p0.l = LO(DMEM_CONTROL);
-	p0.h = HI(DMEM_CONTROL);
-	R1 = [p0];
-	R0 = ~ENDCPLB;
-	R0 = R0 & R1;
-	[p0] = R0;
-	SSYNC;
-
+ENTRY(_mach_early_start)
 	/* Initialise UART - when booting from u-boot, the UART is not disabled
 	 * so if we dont initalize here, our serial console gets hosed */
 	p0.h = hi(BFIN_UART_LCR);
@@ -143,62 +66,13 @@ ENTRY(__start)
 	w[p0] = r0.L;	/* To enable UART clock */
 	ssync;
 
-	/* Initialize stack pointer */
-	sp.l = lo(INITIAL_STACK);
-	sp.h = hi(INITIAL_STACK);
-	fp = sp;
-	usp = sp;
-
-#ifdef CONFIG_EARLY_PRINTK
-	SP += -12;
-	call _init_early_exception_vectors;
-	SP += 12;
-#endif
-
-	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
-	call _bf53x_relocate_l1_mem;
-#if CONFIG_BFIN_KERNEL_CLOCK
-	call _start_dma_code;
-#endif
-
-	/* This section keeps the processor in supervisor mode
-	 * during kernel boot.  Switches to user mode at end of boot.
-	 * See page 3-9 of Hardware Reference manual for documentation.
-	 */
-
-	/* EVT15 = _real_start */
-
-	p0.l = lo(EVT15);
-	p0.h = hi(EVT15);
-	p1.l = _real_start;
-	p1.h = _real_start;
-	[p0] = p1;
-	csync;
-
-	p0.l = lo(IMASK);
-	p0.h = hi(IMASK);
-	p1.l = IMASK_IVG15;
-	p1.h = 0x0;
-	[p0] = p1;
-	csync;
-
-	raise 15;
-	p0.l = .LWAIT_HERE;
-	p0.h = .LWAIT_HERE;
-	reti = p0;
-#if ANOMALY_05000281
-	nop; nop; nop;
-#endif
-	rti;
-
-.LWAIT_HERE:
-	jump .LWAIT_HERE;
-ENDPROC(__start)
+	rts;
+ENDPROC(_mach_early_start)
 
 __FINIT
 
 .section .l1.text
-#if CONFIG_BFIN_KERNEL_CLOCK
+#ifdef CONFIG_BFIN_KERNEL_CLOCK
 ENTRY(_start_dma_code)
 	p0.h = hi(SICA_IWR0);
 	p0.l = lo(SICA_IWR0);
diff --git a/arch/blackfin/mach-common/head.S b/arch/blackfin/mach-common/head.S
index 6a989a031ed6..2c69ad49894e 100644
--- a/arch/blackfin/mach-common/head.S
+++ b/arch/blackfin/mach-common/head.S
@@ -14,13 +14,140 @@
 #include <asm/thread_info.h>
 #include <asm/trace.h>
 
+__INIT
+
+#define INITIAL_STACK	(L1_SCRATCH_START + L1_SCRATCH_LENGTH - 12)
+
+ENTRY(__start)
+	/* R0: argument of command line string, passed from uboot, save it */
+	R7 = R0;
+	/* Enable Cycle Counter and Nesting Of Interrupts */
+#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
+	R0 = SYSCFG_SNEN;
+#else
+	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
+#endif
+	SYSCFG = R0;
+	R0 = 0;
+
+	/* Clear Out All the data and pointer Registers */
+	R1 = R0;
+	R2 = R0;
+	R3 = R0;
+	R4 = R0;
+	R5 = R0;
+	R6 = R0;
+
+	P0 = R0;
+	P1 = R0;
+	P2 = R0;
+	P3 = R0;
+	P4 = R0;
+	P5 = R0;
+
+	LC0 = r0;
+	LC1 = r0;
+	L0 = r0;
+	L1 = r0;
+	L2 = r0;
+	L3 = r0;
+
+	/* Clear Out All the DAG Registers */
+	B0 = r0;
+	B1 = r0;
+	B2 = r0;
+	B3 = r0;
+
+	I0 = r0;
+	I1 = r0;
+	I2 = r0;
+	I3 = r0;
+
+	M0 = r0;
+	M1 = r0;
+	M2 = r0;
+	M3 = r0;
+
+	trace_buffer_init(p0,r0);
+	P0 = R1;
+	R0 = R1;
+
+	/* Turn off the icache */
+	p0.l = LO(IMEM_CONTROL);
+	p0.h = HI(IMEM_CONTROL);
+	R1 = [p0];
+	R0 = ~ENICPLB;
+	R0 = R0 & R1;
+	[p0] = R0;
+	SSYNC;
+
+	/* Turn off the dcache */
+	p0.l = LO(DMEM_CONTROL);
+	p0.h = HI(DMEM_CONTROL);
+	R1 = [p0];
+	R0 = ~ENDCPLB;
+	R0 = R0 & R1;
+	[p0] = R0;
+	SSYNC;
+
+	/* Let each Blackfin family do its own thing */
+	call _mach_early_start;
+
+	/* Initialize stack pointer */
+	sp.l = lo(INITIAL_STACK);
+	sp.h = hi(INITIAL_STACK);
+	fp = sp;
+	usp = sp;
+
+#ifdef CONFIG_EARLY_PRINTK
+	call _init_early_exception_vectors;
+#endif
+
+	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
+	call _bf53x_relocate_l1_mem;
+#ifdef CONFIG_BFIN_KERNEL_CLOCK
+	call _start_dma_code;
+#endif
+
+	/* This section keeps the processor in supervisor mode
+	 * during kernel boot.  Switches to user mode at end of boot.
+	 * See page 3-9 of Hardware Reference manual for documentation.
+	 */
+
+	/* EVT15 = _real_start */
+
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _real_start;
+	p1.h = _real_start;
+	[p0] = p1;
+	csync;
+
+	p0.l = lo(IMASK);
+	p0.h = hi(IMASK);
+	p1.l = IMASK_IVG15;
+	p1.h = 0x0;
+	[p0] = p1;
+	csync;
+
+	raise 15;
+	p0.l = .LWAIT_HERE;
+	p0.h = .LWAIT_HERE;
+	reti = p0;
+#if ANOMALY_05000281
+	nop; nop; nop;
+#endif
+	rti;
+
+.LWAIT_HERE:
+	jump .LWAIT_HERE;
+ENDPROC(__start)
+
 /* A little BF561 glue ... */
 #ifndef WDOG_CTL
 # define WDOG_CTL WDOGA_CTL
 #endif
 
-__INIT
-
 ENTRY(_real_start)
 	/* Enable nested interrupts */
 	[--sp] = reti;