code: plan9front

Info  •  Files  •  Log

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.

--- a/sys/src/9/bcm64/l.s

+++ b/sys/src/9/bcm64/l.s

@@ -20,17 +20,21 @@

 	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

@@ -72,6 +76,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

--- 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)

--- a/sys/src/9/bcm64/rebootcode.s

+++ b/sys/src/9/bcm64/rebootcode.s

@@ -8,7 +8,7 @@

 	MOV	$setSB(SB), R28

 	MRS	MPIDR_EL1, R27

-	ANDW	$(MAXMACH-1), R27

+	AND	$(MPIDMASK), R27

 	LSL	$3, R27

 	ADD	$(SPINTABLE-KZERO), R27

--- a/sys/src/9/imx8/l.s

+++ b/sys/src/9/imx8/l.s

@@ -21,17 +21,21 @@

 	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

@@ -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	aaa<>(SB), 1, $-4

 xxx:

--- a/sys/src/9/imx8/main.c

+++ b/sys/src/9/imx8/main.c

@@ -237,9 +237,25 @@

 	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;

@@ -246,11 +262,13 @@

 	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);

--- 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)