bcm64, imx8: provide more flexible MPIDR_EL1 CPUID to machno mapping

Handle the mapping from MPIDR_EL1 to machno with a
mpidindex() function, that uses a new MPIDMASK
constant from mem.h that signifies the cpuid bits.

This way, other affinity arrangements can be supported
by just changing the MPIDMASK constant.

6 files changed, 78 insertions, 14 deletions

diff --git a/sys/src/9/bcm64/l.s b/sys/src/9/bcm64/l.s
index 2290e051b..7e221047d 100644
--- a/sys/src/9/bcm64/l.s
+++ b/sys/src/9/bcm64/l.s
@@ -20,17 +20,21 @@ TEXT _start(SB), 1, $-4
 	BL	cachedinv(SB)
 	BL	cacheiinv(SB)
 
+	/* get machno in R0 */
+	MRS	MPIDR_EL1, R0
+	BL	mpidindex(SB)
+
+	/* get MACHP(R0) in R27 */
 	MOV	$(MACHADDR(0)-KZERO), R27
-	MRS	MPIDR_EL1, R1
-	ANDW	$(MAXMACH-1), R1
 	MOVWU	$MACHSIZE, R2
-	MULW	R1, R2, R2
+	MULW	R0, R2, R2
 	SUB	R2, R27
 
-	ADD	$(MACHSIZE-16), R27, R2
-	MOV	R2, SP
+	/* set the stack pointer */
+	ADD	$(MACHSIZE-16), R27, R1
+	MOV	R1, SP
 
-	CBNZ	R1, _startup
+	CBNZ	R0, _startup
 
 	/* clear page table and machs */
 	MOV	$(L1BOT-KZERO), R1
@@ -73,6 +77,24 @@ _stop:
 	WFE
 	B	_stop
 
+TEXT	mpidindex(SB), 1, $-4
+	MOV	R0,R1
+	MOV	$0,R0
+	MOV	$(MPIDMASK),R2
+_mpidindex0:
+	CBZ	R2,_mpidindex1
+	LSR	$63,R2,R3
+	CBZ	R3,_mpidindex2
+	LSR	$63,R1,R3
+	LSLW	$1,R0
+	ORRW	R3,R0,R0
+_mpidindex2:
+	LSL	$1,R1
+	LSL	$1,R2
+	B	_mpidindex0
+_mpidindex1:
+	RETURN
+
 TEXT sev(SB), 1, $-4
 	SEV
 	WFE
diff --git a/sys/src/9/bcm64/mem.h b/sys/src/9/bcm64/mem.h
index 1ed184d05..dacd0ebf6 100644
--- a/sys/src/9/bcm64/mem.h
+++ b/sys/src/9/bcm64/mem.h
@@ -32,6 +32,7 @@
 #define L1TABLE(v, l)	(L1TABLES - ((PTLX(v, 2) % L1TABLES) >> (((l)-1)*PTSHIFT)) + (l)-1)
 #define L1TOPSIZE	(1ULL << (EVASHIFT - PTLEVELS*PTSHIFT))
 
+#define MPIDMASK	3ULL			/* MPIDR_EL1 affinity bits signifying the CPUID */
 #define	MAXMACH		4			/* max # cpus system can run */
 #define	MACHSIZE	(8*KiB)
 
diff --git a/sys/src/9/bcm64/rebootcode.s b/sys/src/9/bcm64/rebootcode.s
index 009f10b8c..da5030df9 100644
--- a/sys/src/9/bcm64/rebootcode.s
+++ b/sys/src/9/bcm64/rebootcode.s
@@ -8,7 +8,7 @@ TEXT _start(SB), 1, $-4
 	MOV	$setSB(SB), R28
 
 	MRS	MPIDR_EL1, R27
-	ANDW	$(MAXMACH-1), R27
+	AND	$(MPIDMASK), R27
 	LSL	$3, R27
 	ADD	$(SPINTABLE-KZERO), R27
 
diff --git a/sys/src/9/imx8/l.s b/sys/src/9/imx8/l.s
index 795616738..495f337b5 100644
--- a/sys/src/9/imx8/l.s
+++ b/sys/src/9/imx8/l.s
@@ -21,17 +21,21 @@ TEXT _start(SB), 1, $-4
 	BL	l2cacheuwbinv(SB)
 	BL	cacheiinv(SB)
 
+	/* get machno in R0 */
+	MRS	MPIDR_EL1, R0
+	BL	mpidindex(SB)
+
+	/* get MACHP(R0) in R27 */
 	MOV	$(MACHADDR(0)-KZERO), R27
-	MRS	MPIDR_EL1, R1
-	ANDW	$(MAXMACH-1), R1
 	MOVWU	$MACHSIZE, R2
-	MULW	R1, R2, R2
+	MULW	R0, R2, R2
 	SUB	R2, R27
 
-	ADD	$(MACHSIZE-16), R27, R2
-	MOV	R2, SP
+	/* set the stack pointer */
+	ADD	$(MACHSIZE-16), R27, R1
+	MOV	R1, SP
 
-	CBNZ	R1, _startup
+	CBNZ	R0, _startup
 
 	/* clear page table and machs */
 	MOV	$(L1BOT-KZERO), R1
@@ -74,6 +78,24 @@ _stop:
 	WFE
 	B	_stop
 
+TEXT	mpidindex(SB), 1, $-4
+	MOV	R0,R1
+	MOV	$0,R0
+	MOV	$(MPIDMASK),R2
+_mpidindex0:
+	CBZ	R2,_mpidindex1
+	LSR	$63,R2,R3
+	CBZ	R3,_mpidindex2
+	LSR	$63,R1,R3
+	LSLW	$1,R0
+	ORRW	R3,R0,R0
+_mpidindex2:
+	LSL	$1,R1
+	LSL	$1,R2
+	B	_mpidindex0
+_mpidindex1:
+	RETURN
+
 TEXT	aaa<>(SB), 1, $-4
 xxx:
 	MOV $(0x860040+VIRTIO), R1
diff --git a/sys/src/9/imx8/main.c b/sys/src/9/imx8/main.c
index 61187ae5d..d61a9938b 100644
--- a/sys/src/9/imx8/main.c
+++ b/sys/src/9/imx8/main.c
@@ -237,20 +237,38 @@ machinit(void)
 	active.machs[m->machno] = 1;
 }
 
+static uvlong
+machmpid(int machno)
+{
+	uvlong mpid = 0;
+	int i;
+
+	for(i = 0; i < 64; i++){
+		if(MPIDMASK & (1ULL<<i)){
+			mpid |= (machno & 1ULL) << i;
+			machno >>= 1;
+		}
+	}
+	return mpid;
+}
+
 void
 mpinit(void)
 {
+	extern int mpidindex(uvlong);
 	extern void _start(void);
 	int i;
 
 	for(i = 1; i < conf.nmach; i++){
 		Ureg u = {0};
 
+		assert(mpidindex(machmpid(i)) == i);
+
 		MACHP(i)->machno = i;
 		cachedwbinvse(MACHP(i), MACHSIZE);
 
 		u.r0 = 0x84000003;	/* CPU_ON */
-		u.r1 = (sysrd(MPIDR_EL1) & ~0xFF) | i;
+		u.r1 = (sysrd(MPIDR_EL1) & ~MPIDMASK) | machmpid(i);
 		u.r2 = PADDR(_start);
 		u.r3 = i;
 		smccall(&u);
diff --git a/sys/src/9/imx8/mem.h b/sys/src/9/imx8/mem.h
index cd0cfddae..cf8af6ad6 100644
--- a/sys/src/9/imx8/mem.h
+++ b/sys/src/9/imx8/mem.h
@@ -32,6 +32,7 @@
 #define L1TABLE(v, l)	(L1TABLES - ((PTLX(v, 2) % L1TABLES) >> (((l)-1)*PTSHIFT)) + (l)-1)
 #define L1TOPSIZE	(1ULL << (EVASHIFT - PTLEVELS*PTSHIFT))
 
+#define MPIDMASK	3ULL			/* MPIDR_EL1 affinity bits signifying the CPUID */
 #define	MAXMACH		4			/* max # cpus system can run */
 #define	MACHSIZE	(8*KiB)