ref: 4705576f844f536cdf46d581d4d1ad8776aaa4e9
parent: 44c0e7259800885d0028d8c5352f6760f670132b
author: cinap_lenrek <cinap_lenrek@felloff.net>
date: Sat Nov 11 11:07:06 EST 2023
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)