drivers/bus: arm-cci: function to enable CCI ports from early boot code

This provides cci_enable_port_for_self().  This is the counterpart to
cci_disable_port_by_cpu(self).

This is meant to be called from the MCPM machine specific power_up_setup
callback code when the appropriate affinity level needs to be initialized.
The code therefore has to be position independent as the MMU is still off
and it cannot rely on any stack space.
Signed-off-by: Nicolas Pitre <nico@linaro.org>
Reviewed-by: Dave Martin <dave.martin@linaro.org>

drivers/bus/arm-cci.c

@@ -43,6 +43,7 @@ enum cci_ace_port_type {
 
 struct cci_ace_port {
 	void __iomem *base;
+	unsigned long phys;
 	enum cci_ace_port_type type;
 	struct device_node *dn;
 };
@@ -51,11 +52,13 @@ static struct cci_ace_port *ports;
 static unsigned int nb_cci_ports;
 
 static void __iomem *cci_ctrl_base;
+static unsigned long cci_ctrl_phys;
 
 struct cpu_port {
 	u64 mpidr;
 	u32 port;
 };
+
 /*
  * Use the port MSB as valid flag, shift can be made dynamic
  * by computing number of bits required for port indexes.
@@ -229,6 +232,102 @@ int notrace cci_disable_port_by_cpu(u64 mpidr)
 }
 EXPORT_SYMBOL_GPL(cci_disable_port_by_cpu);
 
+/**
+ * cci_enable_port_for_self() - enable a CCI port for calling CPU
+ *
+ * Enabling a CCI port for the calling CPU implies enabling the CCI
+ * port controlling that CPU's cluster. Caller must make sure that the
+ * CPU running the code is the first active CPU in the cluster and all
+ * other CPUs are quiescent in a low power state  or waiting for this CPU
+ * to complete the CCI initialization.
+ *
+ * Because this is called when the MMU is still off and with no stack,
+ * the code must be position independent and ideally rely on callee
+ * clobbered registers only.  To achieve this we must code this function
+ * entirely in assembler.
+ *
+ * On success this returns with the proper CCI port enabled.  In case of
+ * any failure this never returns as the inability to enable the CCI is
+ * fatal and there is no possible recovery at this stage.
+ */
+asmlinkage void __naked cci_enable_port_for_self(void)
+{
+	asm volatile ("\n"
+
+"	mrc	p15, 0, r0, c0, c0, 5	@ get MPIDR value \n"
+"	and	r0, r0, #"__stringify(MPIDR_HWID_BITMASK)" \n"
+"	adr	r1, 5f \n"
+"	ldr	r2, [r1] \n"
+"	add	r1, r1, r2		@ &cpu_port \n"
+"	add	ip, r1, %[sizeof_cpu_port] \n"
+
+	/* Loop over the cpu_port array looking for a matching MPIDR */
+"1:	ldr	r2, [r1, %[offsetof_cpu_port_mpidr_lsb]] \n"
+"	cmp	r2, r0 			@ compare MPIDR \n"
+"	bne	2f \n"
+
+	/* Found a match, now test port validity */
+"	ldr	r3, [r1, %[offsetof_cpu_port_port]] \n"
+"	tst	r3, #"__stringify(PORT_VALID)" \n"
+"	bne	3f \n"
+
+	/* no match, loop with the next cpu_port entry */
+"2:	add	r1, r1, %[sizeof_struct_cpu_port] \n"
+"	cmp	r1, ip			@ done? \n"
+"	blo	1b \n"
+
+	/* CCI port not found -- cheaply try to stall this CPU */
+"cci_port_not_found: \n"
+"	wfi \n"
+"	wfe \n"
+"	b	cci_port_not_found \n"
+
+	/* Use matched port index to look up the corresponding ports entry */
+"3:	bic	r3, r3, #"__stringify(PORT_VALID)" \n"
+"	adr	r0, 6f \n"
+"	ldmia	r0, {r1, r2} \n"
+"	sub	r1, r1, r0 		@ virt - phys \n"
+"	ldr	r0, [r0, r2] 		@ *(&ports) \n"
+"	mov	r2, %[sizeof_struct_ace_port] \n"
+"	mla	r0, r2, r3, r0		@ &ports[index] \n"
+"	sub	r0, r0, r1		@ virt_to_phys() \n"
+
+	/* Enable the CCI port */
+"	ldr	r0, [r0, %[offsetof_port_phys]] \n"
+"	mov	r3, #"__stringify(CCI_ENABLE_REQ)" \n"
+"	str	r3, [r0, #"__stringify(CCI_PORT_CTRL)"] \n"
+
+	/* poll the status reg for completion */
+"	adr	r1, 7f \n"
+"	ldr	r0, [r1] \n"
+"	ldr	r0, [r0, r1]		@ cci_ctrl_base \n"
+"4:	ldr	r1, [r0, #"__stringify(CCI_CTRL_STATUS)"] \n"
+"	tst	r1, #1 \n"
+"	bne	4b \n"
+
+"	mov	r0, #0 \n"
+"	bx	lr \n"
+
+"	.align	2 \n"
+"5:	.word	cpu_port - . \n"
+"6:	.word	. \n"
+"	.word	ports - 6b \n"
+"7:	.word	cci_ctrl_phys - . \n"
+	: :
+	[sizeof_cpu_port] "i" (sizeof(cpu_port)),
+#ifndef __ARMEB__
+	[offsetof_cpu_port_mpidr_lsb] "i" (offsetof(struct cpu_port, mpidr)),
+#else
+	[offsetof_cpu_port_mpidr_lsb] "i" (offsetof(struct cpu_port, mpidr)+4),
+#endif
+	[offsetof_cpu_port_port] "i" (offsetof(struct cpu_port, port)),
+	[sizeof_struct_cpu_port] "i" (sizeof(struct cpu_port)),
+	[sizeof_struct_ace_port] "i" (sizeof(struct cci_ace_port)),
+	[offsetof_port_phys] "i" (offsetof(struct cci_ace_port, phys)) );
+
+	unreachable();
+}
+
 /**
  * __cci_control_port_by_device() - function to control a CCI port by device
  *				    reference
@@ -306,6 +405,7 @@ static int __init cci_probe(void)
 	struct cci_nb_ports const *cci_config;
 	int ret, i, nb_ace = 0, nb_ace_lite = 0;
 	struct device_node *np, *cp;
+	struct resource res;
 	const char *match_str;
 	bool is_ace;
 
@@ -323,9 +423,12 @@ static int __init cci_probe(void)
 	if (!ports)
 		return -ENOMEM;
 
-	cci_ctrl_base = of_iomap(np, 0);
-
-	if (!cci_ctrl_base) {
+	ret = of_address_to_resource(np, 0, &res);
+	if (!ret) {
+		cci_ctrl_base = ioremap(res.start, resource_size(&res));
+		cci_ctrl_phys =	res.start;
+	}
+	if (ret || !cci_ctrl_base) {
 		WARN(1, "unable to ioremap CCI ctrl\n");
 		ret = -ENXIO;
 		goto memalloc_err;
@@ -353,9 +456,12 @@ static int __init cci_probe(void)
 			continue;
 		}
 
-		ports[i].base = of_iomap(cp, 0);
-
-		if (!ports[i].base) {
+		ret = of_address_to_resource(cp, 0, &res);
+		if (!ret) {
+			ports[i].base = ioremap(res.start, resource_size(&res));
+			ports[i].phys = res.start;
+		}
+		if (ret || !ports[i].base) {
 			WARN(1, "unable to ioremap CCI port %d\n", i);
 			continue;
 		}
@@ -382,6 +488,7 @@ static int __init cci_probe(void)
 	 * cluster power-up/power-down. Make sure it reaches main memory.
 	 */
 	sync_cache_w(&cci_ctrl_base);
+	sync_cache_w(&cci_ctrl_phys);
 	sync_cache_w(&ports);
 	sync_cache_w(&cpu_port);
 	__sync_cache_range_w(ports, sizeof(*ports) * nb_cci_ports);