From cb8e638d2a352271c4dfd701a7cdeaafc0e430cb Mon Sep 17 00:00:00 2001
From: cinap_lenrek <cinap_lenrek@felloff.net>
Date: Sun, 8 May 2022 20:26:56 +0000
Subject: [PATCH] imx8: implement psci calls for system reset and multicore
 startup

---
 sys/src/9/imx8/fns.h  |  3 ++-
 sys/src/9/imx8/gic.c  |  5 -----
 sys/src/9/imx8/l.s    | 31 +++++++++++++++++++++++++++++++
 sys/src/9/imx8/main.c | 36 ++++++++++++++++++++++++++++++++----
 4 files changed, 65 insertions(+), 10 deletions(-)

diff --git a/sys/src/9/imx8/fns.h b/sys/src/9/imx8/fns.h
index 35ea996bb..146216073 100644
--- a/sys/src/9/imx8/fns.h
+++ b/sys/src/9/imx8/fns.h
@@ -16,6 +16,7 @@ extern void noteret(void);
 extern void returnto(void*);
 extern void fpsaveregs(void*);
 extern void fploadregs(void*);
+extern void smccall(Ureg*);
 
 extern void setttbr(uintptr pa);
 extern uintptr getfar(void);
@@ -136,4 +137,4 @@ extern void setconfenv(void);
 extern void writeconf(void);
 
 extern int isaconfig(char*, int, ISAConf*);
-extern void links(void);
\ No newline at end of file
+extern void links(void);
diff --git a/sys/src/9/imx8/gic.c b/sys/src/9/imx8/gic.c
index b788a0082..bfff2eada 100644
--- a/sys/src/9/imx8/gic.c
+++ b/sys/src/9/imx8/gic.c
@@ -139,18 +139,15 @@ intrinit(void)
 
 		/* clear all interrupts */
 		n = ((dregs[GICD_TYPER] & 0x1F)+1) << 5;
-print("nirq %d\n", n);
 		for(i = 32; i < n; i += 32){
 			dregs[GICD_IGROUPR0 + (i/32)] = -1;
 
 			dregs[GICD_ISENABLER0 + (i/32)] = -1;
 			while(dregs[GICD_CTLR]&(1<<31))
 				;
-print("%d: distributor stuck disabled: %.8ux\n", i, ~dregs[GICD_ISENABLER0 + (i/32)]);
 			dregs[GICD_ICENABLER0 + (i/32)] = -1;
 			while(dregs[GICD_CTLR]&(1<<31))
 				;
-print("%d: distributor stuck enabled:  %.8ux\n", i, dregs[GICD_ISENABLER0 + (i/32)]);
 			dregs[GICD_ICACTIVER0 + (i/32)] = -1;
 		}
 		for(i = 0; i < n; i += 4){
@@ -174,11 +171,9 @@ print("%d: distributor stuck enabled:  %.8ux\n", i, dregs[GICD_ISENABLER0 + (i/3
 		rregs[GICR_ISENABLER0 + (i/32)] = -1;
 		while(rregs[GICR_CTLR]&(1<<3))
 			;
-print("%d: re-distributor stuck disabled: %.8ux\n", i, ~rregs[GICR_ISENABLER0 + (i/32)]);
 		rregs[GICR_ICENABLER0 + (i/32)] = -1;
 		while(dregs[GICD_CTLR]&(1<<31))
 			;
-print("%d: re-distributor stuck enabled:  %.8ux\n", i, rregs[GICR_ISENABLER0 + (i/32)]);
 		rregs[GICR_ICACTIVER0 + (i/32)] = -1;
 	}
 	for(i = 0; i < n; i += 4){
diff --git a/sys/src/9/imx8/l.s b/sys/src/9/imx8/l.s
index 16681df62..d51555d58 100644
--- a/sys/src/9/imx8/l.s
+++ b/sys/src/9/imx8/l.s
@@ -679,3 +679,34 @@ _peekloop:
 	SUBS	$1, R0
 	BNE	_peekloop
 	RETURN
+
+TEXT smccall(SB), 1, $32
+	/* save extern registers */
+	MOVP	R26, R27, (RSP)
+
+	/* R0 = Ureg */
+	MOV	R0, R8
+
+	/* save ureg pointer */
+	MOV	R8, 16(RSP)
+
+	MOVP	0(R8), R0, R1
+	MOVP	16(R8), R2, R3
+	MOVP	32(R8), R4, R5
+	MOVP	48(R8), R6, R7
+
+	SMC
+
+	/* restore ureg pointer */
+	MOV	16(RSP), R8
+
+	MOVP	R0, R1, 0(R8)
+	MOVP	R2, R3, 16(R8)
+
+	/* restore extern registers */
+	MOVP	(RSP), R26, R27
+
+	RETURN
+
+
+	
diff --git a/sys/src/9/imx8/main.c b/sys/src/9/imx8/main.c
index 6f9406564..01e4ce20c 100644
--- a/sys/src/9/imx8/main.c
+++ b/sys/src/9/imx8/main.c
@@ -8,6 +8,7 @@
 #include "pool.h"
 #include "io.h"
 #include "sysreg.h"
+#include "ureg.h"
 
 Conf conf;
 
@@ -46,7 +47,7 @@ confinit(void)
 	char *p;
 	int i;
 
-	conf.nmach = 1;
+	conf.nmach = MAXMACH;
 
 	if(p = getconf("service")){
 		if(strcmp(p, "cpu") == 0)
@@ -116,6 +117,29 @@ machinit(void)
 	active.machs[m->machno] = 1;
 }
 
+void
+mpinit(void)
+{
+	extern void _start(void);
+	int i;
+
+	splhi();
+	for(i = 1; i < conf.nmach; i++){
+		Ureg u = {0};
+
+		MACHP(i)->machno = i;
+		cachedwbinvse(MACHP(i), MACHSIZE);
+
+		u.r0 = 0x84000003;
+		u.r1 = (sysrd(MPIDR_EL1) & ~0xFF) | i;
+		u.r2 = PADDR(_start);
+		u.r3 = i;
+		smccall(&u);
+	}
+	synccycles();
+	spllo();
+}
+
 void
 main(void)
 {
@@ -125,7 +149,7 @@ main(void)
 		fpuinit();
 		intrinit();
 		clockinit();
-		// cpuidprint();
+		print("cpu%d: UP!\n", m->machno);
 		synccycles();
 		timersinit();
 		flushtlb();
@@ -152,7 +176,7 @@ main(void)
 	links();
 	chandevreset();
 	userinit();
-//	mpinit();
+	mpinit();
 	mmu0clear((uintptr*)L1);
 	flushtlb();
 	mmu1init();
@@ -162,9 +186,13 @@ main(void)
 void
 exit(int)
 {
+	Ureg u = { .r0 = 0x84000009 };
+
 	cpushutdown();
 	splfhi();
-	for(;;);
+
+	/* system reset */
+	smccall(&u);
 }
 
 int