[PATCH 4/5] MIPS: Handle mips_cps_core_entry within lower 4G

[Jiaxun Yang]

Set CM_GCR_Cx_RESET_BASE_MODE and use XKPHYS base address for
core_entry for 64bit CM when mips_cps_core_entry is beyond
KSEG1.

Also disable SMP and warn user if mips_cps_core_entry is
unsuitable as reset vector.

Signed-off-by: Jiaxun Yang <jiaxun.yang@xxxxxxxxxxx>
---
Note: IMO it does not solve the problem of MobileEye,
which have mips_cps_core_entry beyond lower 4G,
it just enables me to test kernel in XKPHYS on boston.
---
 arch/mips/include/asm/mips-cm.h |  1 +
 arch/mips/kernel/smp-cps.c      | 27 +++++++++++++++++++++------
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/arch/mips/include/asm/mips-cm.h b/arch/mips/include/asm/mips-cm.h
index 23c67c0871b1..15d8d69de455 100644
--- a/arch/mips/include/asm/mips-cm.h
+++ b/arch/mips/include/asm/mips-cm.h
@@ -311,6 +311,7 @@ GCR_CX_ACCESSOR_RW(32, 0x018, other)
 /* GCR_Cx_RESET_BASE - Configure where powered up cores will fetch from */
 GCR_CX_ACCESSOR_RW(32, 0x020, reset_base)
 #define CM_GCR_Cx_RESET_BASE_BEVEXCBASE		GENMASK(31, 12)
+#define CM_GCR_Cx_RESET_BASE_MODE		BIT(1)
 
 /* GCR_Cx_ID - Identify the current core */
 GCR_CX_ACCESSOR_RO(32, 0x028, id)
diff --git a/arch/mips/kernel/smp-cps.c b/arch/mips/kernel/smp-cps.c
index dd55d59b88db..623dfd05585b 100644
--- a/arch/mips/kernel/smp-cps.c
+++ b/arch/mips/kernel/smp-cps.c
@@ -26,6 +26,7 @@
 #include <asm/uasm.h>
 
 static DECLARE_BITMAP(core_power, NR_CPUS);
+static uint32_t core_entry_reg;
 
 struct core_boot_config *mips_cps_core_bootcfg;
 
@@ -37,7 +38,6 @@ static unsigned __init core_vpe_count(unsigned int cluster, unsigned core)
 static void __init cps_smp_setup(void)
 {
 	unsigned int nclusters, ncores, nvpes, core_vpes;
-	unsigned long core_entry;
 	int cl, c, v;
 
 	/* Detect & record VPE topology */
@@ -94,10 +94,20 @@ static void __init cps_smp_setup(void)
 	/* Make core 0 coherent with everything */
 	write_gcr_cl_coherence(0xff);
 
-	if (mips_cm_revision() >= CM_REV_CM3) {
-		core_entry = CKSEG1ADDR((unsigned long)mips_cps_core_entry);
-		write_gcr_bev_base(core_entry);
-	}
+	/*
+	 * Set up the core entry address
+	 * If accessible in KSEG1 just use KSEG1
+	 */
+	if (__pa_symbol(mips_cps_core_entry) < SZ_512M)
+		core_entry_reg =  CKSEG1ADDR(__pa_symbol(mips_cps_core_entry));
+
+	/* If CM is 64bit and with-in low 4G just use XKPHYS */
+	if (mips_cm_is64 && __pa_symbol(mips_cps_core_entry) < SZ_4G)
+		core_entry_reg =  __pa_symbol(mips_cps_core_entry) |
+					CM_GCR_Cx_RESET_BASE_MODE;
+
+	if (core_entry_reg && mips_cm_revision() >= CM_REV_CM3)
+		write_gcr_bev_base(core_entry_reg);
 
 #ifdef CONFIG_MIPS_MT_FPAFF
 	/* If we have an FPU, enroll ourselves in the FPU-full mask */
@@ -114,6 +124,11 @@ static void __init cps_prepare_cpus(unsigned int max_cpus)
 
 	mips_mt_set_cpuoptions();
 
+	if (!core_entry_reg) {
+		pr_err("core_entry address unsuitable, disabling smp-cps\n");
+		goto err_out;
+	}
+
 	/* Detect whether the CCA is unsuited to multi-core SMP */
 	cca = read_c0_config() & CONF_CM_CMASK;
 	switch (cca) {
@@ -213,7 +228,7 @@ static void boot_core(unsigned int core, unsigned int vpe_id)
 	mips_cm_lock_other(0, core, 0, CM_GCR_Cx_OTHER_BLOCK_LOCAL);
 
 	/* Set its reset vector */
-	write_gcr_co_reset_base(CKSEG1ADDR((unsigned long)mips_cps_core_entry));
+	write_gcr_co_reset_base(core_entry_reg);
 
 	/* Ensure its coherency is disabled */
 	write_gcr_co_coherence(0);
-- 
2.34.1