Blackfin arch: SMP supporting patchset: BF561 related code

Blackfin dual core BF561 processor can support SMP like features.
https://docs.blackfin.uclinux.org/doku.php?id=linux-kernel:smp-like

In this patch, we provide SMP extend to BF561 kernel code

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

4 files changed, 149 insertions, 0 deletions

diff --git a/arch/blackfin/mach-bf561/include/mach/blackfin.h b/arch/blackfin/mach-bf561/include/mach/blackfin.h
index 0ea8666e6764..f79f6626b7ec 100644
--- a/arch/blackfin/mach-bf561/include/mach/blackfin.h
+++ b/arch/blackfin/mach-bf561/include/mach/blackfin.h
@@ -66,8 +66,12 @@
 
 #define bfin_read_SIC_IMASK(x)          bfin_read32(SICA_IMASK0 + (x << 2))
 #define bfin_write_SIC_IMASK(x, val)    bfin_write32((SICA_IMASK0 + (x << 2)), val)
+#define bfin_read_SICB_IMASK(x)         bfin_read32(SICB_IMASK0 + (x << 2))
+#define bfin_write_SICB_IMASK(x, val)   bfin_write32((SICB_IMASK0 + (x << 2)), val)
 #define bfin_read_SIC_ISR(x)            bfin_read32(SICA_ISR0 + (x << 2))
 #define bfin_write_SIC_ISR(x, val)      bfin_write32((SICA_ISR0 + (x << 2)), val)
+#define bfin_read_SICB_ISR(x)           bfin_read32(SICB_ISR0 + (x << 2))
+#define bfin_write_SICB_ISR(x, val)     bfin_write32((SICB_ISR0 + (x << 2)), val)
 
 #define BFIN_UART_NR_PORTS      1
 
diff --git a/arch/blackfin/mach-bf561/include/mach/defBF561.h b/arch/blackfin/mach-bf561/include/mach/defBF561.h
index 4eca2026bb92..d7c509759659 100644
--- a/arch/blackfin/mach-bf561/include/mach/defBF561.h
+++ b/arch/blackfin/mach-bf561/include/mach/defBF561.h
@@ -912,6 +912,9 @@
 #define ACTIVE_PLLDISABLED      0x0004  /* Processor In Active Mode With PLL Disabled   */
 #define PLL_LOCKED                      0x0020  /* PLL_LOCKCNT Has Been Reached                                 */
 
+/* SICA_SYSCR Masks */
+#define COREB_SRAM_INIT         0x0020
+
 /* SWRST Mask */
 #define SYSTEM_RESET           0x0007   /* Initiates a system software reset */
 #define DOUBLE_FAULT_A         0x0008   /* Core A Double Fault Causes Reset */
diff --git a/arch/blackfin/mach-bf561/include/mach/mem_map.h b/arch/blackfin/mach-bf561/include/mach/mem_map.h
index f1d4c0637bd2..488c3bda65bf 100644
--- a/arch/blackfin/mach-bf561/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf561/include/mach/mem_map.h
@@ -85,4 +85,124 @@
 #define L1_SCRATCH_START        COREA_L1_SCRATCH_START
 #define L1_SCRATCH_LENGTH       0x1000
 
+#ifndef __ASSEMBLY__
+
+#ifdef CONFIG_SMP
+
+#define get_l1_scratch_start_cpu(cpu)                           \
+        ({ unsigned long __addr;                                \
+           __addr = (cpu) ? COREB_L1_SCRATCH_START : COREA_L1_SCRATCH_START;\
+           __addr; })
+
+#define get_l1_code_start_cpu(cpu)                              \
+        ({ unsigned long __addr;                                \
+           __addr = (cpu) ? COREB_L1_CODE_START : COREA_L1_CODE_START;  \
+           __addr; })
+
+#define get_l1_data_a_start_cpu(cpu)                            \
+        ({ unsigned long __addr;                                \
+           __addr = (cpu) ? COREB_L1_DATA_A_START : COREA_L1_DATA_A_START;\
+           __addr; })
+
+#define get_l1_data_b_start_cpu(cpu)                            \
+        ({ unsigned long __addr;                                \
+           __addr = (cpu) ? COREB_L1_DATA_B_START : COREA_L1_DATA_B_START;\
+           __addr; })
+
+#define get_l1_scratch_start()  get_l1_scratch_start_cpu(blackfin_core_id())
+#define get_l1_code_start()     get_l1_code_start_cpu(blackfin_core_id())
+#define get_l1_data_a_start()   get_l1_data_a_start_cpu(blackfin_core_id())
+#define get_l1_data_b_start()   get_l1_data_b_start_cpu(blackfin_core_id())
+
+#else /* !CONFIG_SMP */
+#define get_l1_scratch_start_cpu(cpu)   L1_SCRATCH_START
+#define get_l1_code_start_cpu(cpu)      L1_CODE_START
+#define get_l1_data_a_start_cpu(cpu)    L1_DATA_A_START
+#define get_l1_data_b_start_cpu(cpu)    L1_DATA_B_START
+#define get_l1_scratch_start()          L1_SCRATCH_START
+#define get_l1_code_start()             L1_CODE_START
+#define get_l1_data_a_start()           L1_DATA_A_START
+#define get_l1_data_b_start()           L1_DATA_B_START
+#endif /* !CONFIG_SMP */
+
+#else /* __ASSEMBLY__ */
+
+/*
+ * The following macros both return the address of the PDA for the
+ * current core.
+ *
+ * In its first safe (and hairy) form, the macro neither clobbers any
+ * register aside of the output Preg, nor uses the stack, since it
+ * could be called with an invalid stack pointer, or the current stack
+ * space being uncovered by any CPLB (e.g. early exception handling).
+ *
+ * The constraints on the second form are a bit relaxed, and the code
+ * is allowed to use the specified Dreg for determining the PDA
+ * address to be returned into Preg.
+ */
+#ifdef CONFIG_SMP
+#define GET_PDA_SAFE(preg)              \
+        preg.l = lo(DSPID);             \
+        preg.h = hi(DSPID);             \
+        preg = [preg];                  \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        preg = preg << 2;               \
+        if cc jump 2f;                  \
+        cc = preg == 0x0;               \
+        preg.l = _cpu_pda;              \
+        preg.h = _cpu_pda;              \
+        if !cc jump 3f;                 \
+1:                                      \
+        /* preg = 0x0; */               \
+        cc = !cc; /* restore cc to 0 */ \
+        jump 4f;                        \
+2:                                      \
+        cc = preg == 0x0;               \
+        preg.l = _cpu_pda;              \
+        preg.h = _cpu_pda;              \
+        if cc jump 4f;                  \
+        /* preg = 0x1000000; */         \
+        cc = !cc; /* restore cc to 1 */ \
+3:                                      \
+        preg = [preg];                  \
+4:
+
+#define GET_PDA(preg, dreg)             \
+        preg.l = lo(DSPID);             \
+        preg.h = hi(DSPID);             \
+        dreg = [preg];                  \
+        preg.l = _cpu_pda;              \
+        preg.h = _cpu_pda;              \
+        cc = bittst(dreg, 0);           \
+        if !cc jump 1f;                 \
+        preg = [preg];                  \
+1:                                      \
+
+#define GET_CPUID(preg, dreg)           \
+        preg.l = lo(DSPID);             \
+        preg.h = hi(DSPID);             \
+        dreg = [preg];                  \
+        dreg = ROT dreg BY -1;          \
+        dreg = CC;
+
+#else
+#define GET_PDA_SAFE(preg)              \
+        preg.l = _cpu_pda;              \
+        preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)     GET_PDA_SAFE(preg)
+#endif /* CONFIG_SMP */
+
+#endif /* __ASSEMBLY__ */
+
 #endif                          /* _MEM_MAP_533_H_ */
diff --git a/arch/blackfin/mach-bf561/include/mach/smp.h b/arch/blackfin/mach-bf561/include/mach/smp.h
new file mode 100644
index 000000000000..f9e65ebe81b2
--- /dev/null
+++ b/arch/blackfin/mach-bf561/include/mach/smp.h
@@ -0,0 +1,22 @@
+#ifndef _MACH_BF561_SMP
+#define _MACH_BF561_SMP
+
+struct task_struct;
+
+void platform_init_cpus(void);
+
+void platform_prepare_cpus(unsigned int max_cpus);
+
+int platform_boot_secondary(unsigned int cpu, struct task_struct *idle);
+
+void platform_secondary_init(unsigned int cpu);
+
+void platform_request_ipi(int (*handler)(int, void *));
+
+void platform_send_ipi(cpumask_t callmap);
+
+void platform_send_ipi_cpu(unsigned int cpu);
+
+void platform_clear_ipi(unsigned int cpu);
+
+#endif /* !_MACH_BF561_SMP */