rpi400: smp startup

This commit is contained in:
K. Lange 2022-02-18 18:43:57 +09:00
parent 4c13c92180
commit 975b388962
2 changed files with 112 additions and 28 deletions

View File

@ -255,17 +255,14 @@ static void exception_handlers(void) {
asm volatile("msr VBAR_EL1, %0" :: "r"(&_exception_vector));
}
#if 0
#define GICD_BASE 0x08000000
#define GICC_BASE 0x08010000
#else
#define GICD_BASE 0xff841000
#define GICC_BASE 0xff842000
#endif
static void gic_map_regs(void) {
gic_regs = (volatile uint32_t*)mmu_map_mmio_region(GICD_BASE, 0x1000);
gicc_regs = (volatile uint32_t*)mmu_map_mmio_region(GICC_BASE, 0x2000);
static void gic_map_regs(uintptr_t rpi_tag) {
if (rpi_tag) {
gic_regs = (volatile uint32_t*)mmu_map_mmio_region(0xff841000, 0x1000);
gicc_regs = (volatile uint32_t*)mmu_map_mmio_region(0xff842000, 0x2000);
} else {
gic_regs = (volatile uint32_t*)mmu_map_mmio_region(0x08000000, 0x1000);
gicc_regs = (volatile uint32_t*)mmu_map_mmio_region(0x08010000, 0x2000);
}
}
void aarch64_sync_enter(struct regs * r) {
@ -620,9 +617,17 @@ int kmain(uintptr_t dtb_base, uintptr_t phys_base, uintptr_t rpi_tag) {
dprintf("rpi: mmu reinitialized\n");
_arch_args = "vid=preset start=live-session root=/dev/ram0";
/**
* TODO there's a mailbox command for this, can we try that?
* I'm not sure why I've been unable to get it out of a 'chosen' tag...
*/
_arch_args = "vid=preset start=live-session migrate root=/dev/ram0";
} else {
/*
* TODO virt shim should load the ramdisk for us, so we can use the same code
* as we have for the RPi above and not have to use fwcfg to load it...
*/
fwcfg_load_initrd(&ramdisk_phys_base, &ramdisk_size);
/* Probe DTB for memory layout. */
extern char end[];
@ -639,7 +644,7 @@ int kmain(uintptr_t dtb_base, uintptr_t phys_base, uintptr_t rpi_tag) {
dtb_locate_cmdline();
}
gic_map_regs();
gic_map_regs(rpi_tag);
/* Set up all the other arch-specific stuff here */
fpu_enable();
@ -661,21 +666,25 @@ int kmain(uintptr_t dtb_base, uintptr_t phys_base, uintptr_t rpi_tag) {
/* Load MIDR */
aarch64_processor_data();
/* Start other cores here */
//aarch64_smp_start();
processor_count = 1;
/* Set up the system virtual timer to produce interrupts for userspace scheduling */
timer_start();
#if 0
/* Install drivers that may need to sleep here */
virtio_input();
/* Start other cores here */
if (!rpi_tag ){
aarch64_smp_start();
} else {
extern void rpi_smp_init(void);
rpi_smp_init();
}
/* Set up serial input */
extern void pl011_start(void);
pl011_start();
#endif
if (!rpi_tag) {
/* Install drivers that may need to sleep here */
virtio_input();
/* Set up serial input */
extern void pl011_start(void);
pl011_start();
}
generic_main();

View File

@ -12,6 +12,7 @@
#include <kernel/string.h>
#include <kernel/printf.h>
#include <kernel/mmu.h>
#include <kernel/time.h>
#include <sys/ptrace.h>
@ -27,7 +28,7 @@ static int method = 0;
static volatile uint32_t _smp_mutex = 0;
volatile uintptr_t * aarch64_jmp_target = 0;
volatile uintptr_t aarch64_jmp_target = 0;
volatile uint64_t aarch64_sctlr = 0;
volatile uint64_t aarch64_tcr = 0;
volatile uint64_t aarch64_mair = 0;
@ -37,6 +38,7 @@ volatile uintptr_t aarch64_ttbr1 = 0;
volatile uintptr_t aarch64_stack = 0;
void ap_start(uint64_t core_id) {
dprintf("smp: core %zu is online\n", core_id);
extern void arch_set_core_base(uintptr_t base);
@ -62,7 +64,6 @@ void ap_start(uint64_t core_id) {
switch_next();
}
void smp_bootstrap(void) {
asm volatile (
/* Store x0, which is our core ID */
@ -98,6 +99,7 @@ void smp_bootstrap(void) {
"mov x0, x3\n"
/* Jump to C entrypoint */
"br x1\n");
__builtin_unreachable();
}
static void start_cpu(uint32_t * node) {
@ -108,7 +110,7 @@ static void start_cpu(uint32_t * node) {
if (method == 0x637668) {
_smp_mutex = 0;
aarch64_stack = (uintptr_t)sbrk(4096);
aarch64_stack = (uintptr_t)sbrk(4096) + 4096;
uint64_t x0 = cpu_on;
uint64_t x1 = num;
uint64_t x2 = mmu_map_to_physical(NULL, (uintptr_t)&smp_bootstrap);
@ -161,7 +163,7 @@ void aarch64_smp_start(void) {
return;
}
aarch64_jmp_target = (void*)(uintptr_t)ap_start;
aarch64_jmp_target = (uintptr_t)ap_start;
asm volatile ("mrs %0, MAIR_EL1" : "=r"(aarch64_mair));
asm volatile ("mrs %0, TCR_EL1" : "=r"(aarch64_tcr));
asm volatile ("mrs %0, SCTLR_EL1" : "=r"(aarch64_sctlr));
@ -184,3 +186,76 @@ void aarch64_smp_start(void) {
dtb_callback_direct_children(cpus, start_cpu);
}
void rpi_smp_exit_el2(void) {
asm volatile (
"ldr x0, =0x1004\n"
"mrs x1, SCTLR_EL2\n"
"orr x1, x1, x0\n"
"msr SCTLR_EL2, x1\n"
"ldr x0, =0x30d01804\n"
"msr SCTLR_EL1, x0\n"
"ldr x0, =0x80000000\n"
"msr HCR_EL2, x0\n"
"ldr x0, =0x3c5\n"
"msr SPSR_EL2, x0\n"
"adr x0, smp_bootstrap\n"
"msr ELR_EL2, x0\n"
"mov x0, x6\n"
"eret\n"
::: "x0", "x1");
__builtin_unreachable();
}
void rpi_smp_init(void) {
aarch64_jmp_target = (uintptr_t)ap_start;
asm volatile ("mrs %0, MAIR_EL1" : "=r"(aarch64_mair));
asm volatile ("mrs %0, TCR_EL1" : "=r"(aarch64_tcr));
asm volatile ("mrs %0, SCTLR_EL1" : "=r"(aarch64_sctlr));
asm volatile ("mrs %0, VBAR_EL1" : "=r"(aarch64_vbar));
startup_ttbr0[0][0].raw = mmu_map_to_physical(NULL, (uintptr_t)&startup_ttbr0[1]) | (0x3) | (1 << 10);
for (long i = 0; i < 512; ++i) {
startup_ttbr0[1][i].raw = (i << 30) | (2 << 2) | 1 | (1 << 10);
}
aarch64_ttbr0 = mmu_map_to_physical(NULL, (uintptr_t)&startup_ttbr0[0]);
aarch64_ttbr1 = mmu_map_to_physical(NULL, (uintptr_t)mmu_get_kernel_directory());
asm volatile (
"dsb ishst\n"
"tlbi vmalle1is\n"
"dsb ish\n"
"isb\n"
);
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_jmp_target));
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_mair));
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_tcr));
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_sctlr));
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_vbar));
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_ttbr0));
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_ttbr1));
asm volatile ("dc cvac, %0\n" :: "r"(&startup_ttbr0[0]));
asm volatile ("dc cvac, %0\n" :: "r"(&startup_ttbr0[1]));
uintptr_t spinners[] = {0xd8, 0xe0, 0xe8, 0xf0};
uintptr_t low_mem = (uintptr_t)mmu_map_mmio_region(0, 0x1000);
union PML * p = mmu_get_page(low_mem, 0);
p->bits.page = 0;
asm volatile ("dsb ishst\ntlbi vmalle1is\ndsb ish\nisb" ::: "memory");
dprintf("smp: zero page mapped at %#zx, page is %#zx\n",
low_mem, mmu_map_to_physical(mmu_get_kernel_directory(), low_mem));
for (int i = 1; i < 4; ++i) {
_smp_mutex = 0;
aarch64_stack = (uintptr_t)sbrk(4096) + 4096;
asm volatile ("dc cvac, %0\n" :: "r"(&aarch64_stack));
uintptr_t target = mmu_map_to_physical(NULL, (uintptr_t)&rpi_smp_exit_el2);
*(volatile uintptr_t*)(low_mem + spinners[i]) = target;
asm volatile ("dmb sy\nisb\ndc cvac, %0\nisb\nsev" :: "r"(low_mem) : "memory");
while (_smp_mutex == 0);
processor_count = i + 1;
}
}