8 files changed, 74 insertions, 12 deletions

diff --git a/arch/blackfin/Kconfig b/arch/blackfin/Kconfig
index 0f34ec58bac3..c4cda6f52b61 100644
--- a/arch/blackfin/Kconfig
+++ b/arch/blackfin/Kconfig
@@ -850,7 +850,6 @@ config CPLB_SWITCH_TAB_L1
 config ICACHE_FLUSH_L1
         bool "Locate icache flush funcs in L1 Inst Memory"
         default y
-        depends on !SMP
         help
           If enabled, the Blackfin icache flushing functions are linked
           into L1 instruction memory.
diff --git a/arch/blackfin/include/asm/smp.h b/arch/blackfin/include/asm/smp.h
index 9dd487375247..af6c0aa79bae 100644
--- a/arch/blackfin/include/asm/smp.h
+++ b/arch/blackfin/include/asm/smp.h
@@ -17,7 +17,12 @@
 
 #define raw_smp_processor_id()  blackfin_core_id()
 
-extern char coreb_trampoline_start, coreb_trampoline_end;
+extern void bfin_relocate_coreb_l1_mem(void);
+
+#if defined(CONFIG_SMP) && defined(CONFIG_ICACHE_FLUSH_L1)
+asmlinkage void blackfin_icache_flush_range_l1(unsigned long *ptr);
+extern unsigned long blackfin_iflush_l1_entry[NR_CPUS];
+#endif
 
 struct corelock_slot {
         int lock;
diff --git a/arch/blackfin/kernel/setup.c b/arch/blackfin/kernel/setup.c
index ac71dc15cbdb..805c6132c779 100644
--- a/arch/blackfin/kernel/setup.c
+++ b/arch/blackfin/kernel/setup.c
@@ -215,11 +215,48 @@ void __init bfin_relocate_l1_mem(void)
 
         early_dma_memcpy_done();
 
+#if defined(CONFIG_SMP) && defined(CONFIG_ICACHE_FLUSH_L1)
+        blackfin_iflush_l1_entry[0] = (unsigned long)blackfin_icache_flush_range_l1;
+#endif
+
         /* if necessary, copy L2 text/data to L2 SRAM */
         if (L2_LENGTH && l2_len)
                 memcpy(_stext_l2, _l2_lma, l2_len);
 }
 
+#ifdef CONFIG_SMP
+void __init bfin_relocate_coreb_l1_mem(void)
+{
+        unsigned long text_l1_len = (unsigned long)_text_l1_len;
+        unsigned long data_l1_len = (unsigned long)_data_l1_len;
+        unsigned long data_b_l1_len = (unsigned long)_data_b_l1_len;
+
+        blackfin_dma_early_init();
+
+        /* if necessary, copy L1 text to L1 instruction SRAM */
+        if (L1_CODE_LENGTH && text_l1_len)
+                early_dma_memcpy((void *)COREB_L1_CODE_START, _text_l1_lma,
+                                text_l1_len);
+
+        /* if necessary, copy L1 data to L1 data bank A SRAM */
+        if (L1_DATA_A_LENGTH && data_l1_len)
+                early_dma_memcpy((void *)COREB_L1_DATA_A_START, _data_l1_lma,
+                                data_l1_len);
+
+        /* if necessary, copy L1 data B to L1 data bank B SRAM */
+        if (L1_DATA_B_LENGTH && data_b_l1_len)
+                early_dma_memcpy((void *)COREB_L1_DATA_B_START, _data_b_l1_lma,
+                                data_b_l1_len);
+
+        early_dma_memcpy_done();
+
+#ifdef CONFIG_ICACHE_FLUSH_L1
+        blackfin_iflush_l1_entry[1] = (unsigned long)blackfin_icache_flush_range_l1 -
+                        (unsigned long)_stext_l1 + COREB_L1_CODE_START;
+#endif
+}
+#endif
+
 #ifdef CONFIG_ROMKERNEL
 void __init bfin_relocate_xip_data(void)
 {
diff --git a/arch/blackfin/kernel/vmlinux.lds.S b/arch/blackfin/kernel/vmlinux.lds.S
index f5fd234e5773..854fa49f1c3e 100644
--- a/arch/blackfin/kernel/vmlinux.lds.S
+++ b/arch/blackfin/kernel/vmlinux.lds.S
@@ -176,6 +176,7 @@ SECTIONS
         {
                 . = ALIGN(4);
                 __stext_l1 = .;
+                *(.l1.text.head)
                 *(.l1.text)
 #ifdef CONFIG_SCHEDULE_L1
                 SCHED_TEXT
diff --git a/arch/blackfin/mach-bf561/secondary.S b/arch/blackfin/mach-bf561/secondary.S
index 148e50764555..4c462838f4e1 100644
--- a/arch/blackfin/mach-bf561/secondary.S
+++ b/arch/blackfin/mach-bf561/secondary.S
@@ -13,7 +13,11 @@
 #include <asm/asm-offsets.h>
 #include <asm/trace.h>
 
-__INIT
+/*
+ * This code must come first as CoreB is hardcoded (in hardware)
+ * to start at the beginning of its L1 instruction memory.
+ */
+.section .l1.text.head
 
 /* Lay the initial stack into the L1 scratch area of Core B */
 #define INITIAL_STACK   (COREB_L1_SCRATCH_START + L1_SCRATCH_LENGTH - 12)
@@ -160,7 +164,6 @@ ENTRY(_coreb_trampoline_start)
 .LWAIT_HERE:
         jump .LWAIT_HERE;
 ENDPROC(_coreb_trampoline_start)
-ENTRY(_coreb_trampoline_end)
 
 #ifdef CONFIG_HOTPLUG_CPU
 .section ".text"
diff --git a/arch/blackfin/mach-bf561/smp.c b/arch/blackfin/mach-bf561/smp.c
index 1074a7ef81c7..82b94e137886 100644
--- a/arch/blackfin/mach-bf561/smp.c
+++ b/arch/blackfin/mach-bf561/smp.c
@@ -30,18 +30,11 @@ void __init platform_init_cpus(void)
 
 void __init platform_prepare_cpus(unsigned int max_cpus)
 {
-        int len;
-
-        len = &coreb_trampoline_end - &coreb_trampoline_start + 1;
-        BUG_ON(len > L1_CODE_LENGTH);
-
-        dma_memcpy((void *)COREB_L1_CODE_START, &coreb_trampoline_start, len);
+        bfin_relocate_coreb_l1_mem();
 
         /* Both cores ought to be present on a bf561! */
         cpu_set(0, cpu_present_map); /* CoreA */
         cpu_set(1, cpu_present_map); /* CoreB */
-
-        printk(KERN_INFO "CoreB bootstrap code to SRAM %p via DMA.\n", (void *)COREB_L1_CODE_START);
 }
 
 int __init setup_profiling_timer(unsigned int multiplier) /* not supported */
diff --git a/arch/blackfin/mach-common/cache.S b/arch/blackfin/mach-common/cache.S
index 85aadeb76658..9f4dd35bfd74 100644
--- a/arch/blackfin/mach-common/cache.S
+++ b/arch/blackfin/mach-common/cache.S
@@ -69,10 +69,30 @@
 #endif
 
 /* Invalidate all instruction cache lines assocoiated with this memory area */
+#ifdef CONFIG_SMP
+# define _blackfin_icache_flush_range _blackfin_icache_flush_range_l1
+#endif
 ENTRY(_blackfin_icache_flush_range)
         do_flush IFLUSH
 ENDPROC(_blackfin_icache_flush_range)
 
+#ifdef CONFIG_SMP
+.text
+# undef _blackfin_icache_flush_range
+ENTRY(_blackfin_icache_flush_range)
+        p0.L = LO(DSPID);
+        p0.H = HI(DSPID);
+        r3 = [p0];
+        r3 = r3.b (z);
+        p2 = r3;
+        p0.L = _blackfin_iflush_l1_entry;
+        p0.H = _blackfin_iflush_l1_entry;
+        p0 = p0 + (p2 << 2);
+        p1 = [p0];
+        jump (p1);
+ENDPROC(_blackfin_icache_flush_range)
+#endif
+
 #ifdef CONFIG_DCACHE_FLUSH_L1
 .section .l1.text
 #else
diff --git a/arch/blackfin/mach-common/smp.c b/arch/blackfin/mach-common/smp.c
index 5f7617d66905..6e17a265c4d3 100644
--- a/arch/blackfin/mach-common/smp.c
+++ b/arch/blackfin/mach-common/smp.c
@@ -40,6 +40,10 @@
  */
 struct corelock_slot corelock __attribute__ ((__section__(".l2.bss")));
 
+#ifdef CONFIG_ICACHE_FLUSH_L1
+unsigned long blackfin_iflush_l1_entry[NR_CPUS];
+#endif
+
 void __cpuinitdata *init_retx_coreb, *init_saved_retx_coreb,
         *init_saved_seqstat_coreb, *init_saved_icplb_fault_addr_coreb,
         *init_saved_dcplb_fault_addr_coreb;