debug: execute snoop filter in new cpumcpm-snoop-v1

Signed-off-by: Haojian Zhuang <haojian.zhuang@linaro.org>

1 files changed, 72 insertions, 3 deletions

diff --git a/arch/arm/mach-hisi/platmcpm.c b/arch/arm/mach-hisi/platmcpm.c
index 2bda12b936f9..6f295428c25b 100644
--- a/arch/arm/mach-hisi/platmcpm.c
+++ b/arch/arm/mach-hisi/platmcpm.c
@@ -66,6 +66,7 @@ static void __iomem *relocation, *sysctrl, *fabric;
 static int hip04_cpu_table[HIP04_MAX_CLUSTERS][HIP04_MAX_CPUS_PER_CLUSTER];
 static DEFINE_SPINLOCK(boot_lock);
 static struct hip04_secondary_cpu_data hip04_boot;
+static u32 fabric_phys_addr;
 
 static bool hip04_cluster_down(unsigned int cluster)
 {
@@ -77,6 +78,7 @@ static bool hip04_cluster_down(unsigned int cluster)
 	return true;
 }
 
+/*
 static void hip04_set_snoop_filter(unsigned int cluster, unsigned int on)
 {
 	unsigned long data;
@@ -94,6 +96,7 @@ static void hip04_set_snoop_filter(unsigned int cluster, unsigned int on)
 			break;
 	}
 }
+*/
 
 static int hip04_mcpm_power_up(unsigned int cpu, unsigned int cluster)
 {
@@ -125,7 +128,7 @@ static int hip04_mcpm_power_up(unsigned int cpu, unsigned int cluster)
 			data = readl_relaxed(sysctrl + \
 					     SC_CPU_RESET_STATUS(cluster));
 		} while (data & mask);
-		hip04_set_snoop_filter(cluster, 1);
+		//hip04_set_snoop_filter(cluster, 1);
 	}
 
 	hip04_cpu_table[cluster][cpu]++;
@@ -202,6 +205,62 @@ static void hip04_mcpm_powered_up(void)
 	spin_unlock(&boot_lock);
 }
 
+//#define OFFSET_DEBUG
+static void __naked hip04_mcpm_power_up_setup(unsigned int affinity_level)
+{
+	asm volatile ("			\n"
+	/* get cluster id from MFPR */
+"	mrc	p15, 0, r0, c0, c0, 5	\n"
+"	and	r0, r0, #0xf00		\n"
+"	mov	r1, r0, lsr #8		\n"
+	/* 1 << cluster id */
+"	mov	r0, #1			\n"
+"	mov	r3, r0, lsl r1		\n"
+	/* gpio, r1 is used as debug */
+"	mov	r1, #0xe4000000		\n"
+"	orr	r1, r1, #0x2000		\n"
+"	ldr	r0, [r1]		\n"
+/*"	orr	r0, r0, #0x70000000	\n"*/
+"	str	r0, [r1]		\n"
+"	ldr	r0, [r1]		\n"
+"	cmp	r0, #0			\n"
+"	bxeq	lr			\n"
+
+	/* get phys address of fabric */
+#ifdef OFFSET_DEBUG
+"	mov	r2, #0xe3000000		\n"
+"	orr	r2, r2, #0x020000	\n"
+"	orr	r2, r2, #0xa000		\n"
+#else
+"	adr	r2, .C0			\n"
+"	ldmia	r2, {r1, r3}		\n"
+	/* get fabric_phys_addr */
+"	add	r2, r2, r3		\n"
+/*"	ldr	r2, [r2, r3]		\n"*/
+#endif
+"	mov	r1, #0xe4000000		\n"
+"	orr	r1, r1, #0x2000		\n"
+"	ldr	r0, [r1]		\n"
+"	orr	r0, r0, #0x10000000	\n"
+"	str	r0, [r1]		\n"
+"	ldr	r0, [r1]		\n"
+"	ldr	r0, [r2, #"__stringify(FAB_SF_MODE)"]	\n"
+"	orr	r3, r0, r3		\n"
+"	str	r3, [r2, #"__stringify(FAB_SF_MODE)"]	\n"
+"1:	ldr	r0, [r2, #"__stringify(FAB_SF_MODE)"]	\n"
+"	cmp	r0, r3			\n"
+"	bne	1b			\n"
+"	ldr	r0, [r1]		\n"
+"	orr	r0, r0, #0x20000000	\n"
+"	str	r0, [r1]		\n"
+"	ldr	r0, [r1]		\n"
+"	bx	lr			\n"
+"	.align	2			\n"
+".C0:	.word	.			\n"
+"	.word	fabric_phys_addr - .C0	\n"
+	);
+}
+
 static const struct mcpm_platform_ops hip04_mcpm_ops = {
 	.power_up		= hip04_mcpm_power_up,
 	.power_down		= hip04_mcpm_power_down,
@@ -222,7 +281,7 @@ static bool __init hip04_cpu_table_init(void)
 		pr_err("%s: boot CPU is out of bound!\n", __func__);
 		return false;
 	}
-	hip04_set_snoop_filter(cluster, 1);
+	//hip04_set_snoop_filter(cluster, 1);
 	hip04_cpu_table[cluster][cpu] = 1;
 	return true;
 }
@@ -230,6 +289,7 @@ static bool __init hip04_cpu_table_init(void)
 static int __init hip04_mcpm_init(void)
 {
 	struct device_node *np, *np_fab;
+	struct resource fab_res;
 	int ret = -ENODEV;
 
 	np = of_find_compatible_node(NULL, NULL, "hisilicon,sysctrl");
@@ -283,6 +343,14 @@ static int __init hip04_mcpm_init(void)
 		ret = -ENOMEM;
 		goto err_sysctrl;
 	}
+	ret = of_address_to_resource(np_fab, 0, &fab_res);
+	if (ret) {
+		pr_err("failed to get fabric base phys\n");
+		goto err_fabric;
+	}
+	pr_err("#%s, %d, start:0x%llx, end:0x%llx\n", __func__, __LINE__, fab_res.start, fab_res.end);
+	fabric_phys_addr = fab_res.start;
+	pr_err("#%s, %d, fabric phys addr:0x%x at 0x%x\n", __func__, __LINE__, fabric_phys_addr, &fabric_phys_addr);
 	fabric = of_iomap(np_fab, 0);
 	if (!fabric) {
 		pr_err("failed to get fabric base\n");
@@ -294,7 +362,8 @@ static int __init hip04_mcpm_init(void)
 		return -EINVAL;
 	ret = mcpm_platform_register(&hip04_mcpm_ops);
 	if (!ret) {
-		mcpm_sync_init(NULL);
+		//mcpm_sync_init(NULL);
+		mcpm_sync_init(hip04_mcpm_power_up_setup);
 		pr_info("HiP04 MCPM initialized\n");
 	}
 	mcpm_smp_set_ops();