imx8: implement psci calls for system reset and multicore startup

This commit is contained in:
cinap_lenrek 2022-05-08 20:26:56 +00:00
parent cac30a565e
commit 7aa8c6d47e
4 changed files with 65 additions and 10 deletions

View file

@ -16,6 +16,7 @@ extern void noteret(void);
extern void returnto(void*); extern void returnto(void*);
extern void fpsaveregs(void*); extern void fpsaveregs(void*);
extern void fploadregs(void*); extern void fploadregs(void*);
extern void smccall(Ureg*);
extern void setttbr(uintptr pa); extern void setttbr(uintptr pa);
extern uintptr getfar(void); extern uintptr getfar(void);

View file

@ -139,18 +139,15 @@ intrinit(void)
/* clear all interrupts */ /* clear all interrupts */
n = ((dregs[GICD_TYPER] & 0x1F)+1) << 5; n = ((dregs[GICD_TYPER] & 0x1F)+1) << 5;
print("nirq %d\n", n);
for(i = 32; i < n; i += 32){ for(i = 32; i < n; i += 32){
dregs[GICD_IGROUPR0 + (i/32)] = -1; dregs[GICD_IGROUPR0 + (i/32)] = -1;
dregs[GICD_ISENABLER0 + (i/32)] = -1; dregs[GICD_ISENABLER0 + (i/32)] = -1;
while(dregs[GICD_CTLR]&(1<<31)) 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; dregs[GICD_ICENABLER0 + (i/32)] = -1;
while(dregs[GICD_CTLR]&(1<<31)) 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; dregs[GICD_ICACTIVER0 + (i/32)] = -1;
} }
for(i = 0; i < n; i += 4){ 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; rregs[GICR_ISENABLER0 + (i/32)] = -1;
while(rregs[GICR_CTLR]&(1<<3)) 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; rregs[GICR_ICENABLER0 + (i/32)] = -1;
while(dregs[GICD_CTLR]&(1<<31)) 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; rregs[GICR_ICACTIVER0 + (i/32)] = -1;
} }
for(i = 0; i < n; i += 4){ for(i = 0; i < n; i += 4){

View file

@ -679,3 +679,34 @@ _peekloop:
SUBS $1, R0 SUBS $1, R0
BNE _peekloop BNE _peekloop
RETURN 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

View file

@ -8,6 +8,7 @@
#include "pool.h" #include "pool.h"
#include "io.h" #include "io.h"
#include "sysreg.h" #include "sysreg.h"
#include "ureg.h"
Conf conf; Conf conf;
@ -46,7 +47,7 @@ confinit(void)
char *p; char *p;
int i; int i;
conf.nmach = 1; conf.nmach = MAXMACH;
if(p = getconf("service")){ if(p = getconf("service")){
if(strcmp(p, "cpu") == 0) if(strcmp(p, "cpu") == 0)
@ -116,6 +117,29 @@ machinit(void)
active.machs[m->machno] = 1; 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 void
main(void) main(void)
{ {
@ -125,7 +149,7 @@ main(void)
fpuinit(); fpuinit();
intrinit(); intrinit();
clockinit(); clockinit();
// cpuidprint(); print("cpu%d: UP!\n", m->machno);
synccycles(); synccycles();
timersinit(); timersinit();
flushtlb(); flushtlb();
@ -152,7 +176,7 @@ main(void)
links(); links();
chandevreset(); chandevreset();
userinit(); userinit();
// mpinit(); mpinit();
mmu0clear((uintptr*)L1); mmu0clear((uintptr*)L1);
flushtlb(); flushtlb();
mmu1init(); mmu1init();
@ -162,9 +186,13 @@ main(void)
void void
exit(int) exit(int)
{ {
Ureg u = { .r0 = 0x84000009 };
cpushutdown(); cpushutdown();
splfhi(); splfhi();
for(;;);
/* system reset */
smccall(&u);
} }
int int