From 6d5e5786ad8ed2ee310252c67889e8fa673b2c51 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Tue, 7 Jul 2026 18:10:30 +0200 Subject: [PATCH 1/8] main: Turn off all iodevs except for UART and dockchannel We're about to add PSCI code that will be called after the OS has booted and taken over control over most hardware, especially the USB ports. From all the available iodevs only dockchannel and the Samsung UART are simple enough to still be used in that context. Signed-off-by: Sven Peter --- src/main.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main.c b/src/main.c index d4e828c54..97f6988cc 100644 --- a/src/main.c +++ b/src/main.c @@ -206,6 +206,12 @@ void m1n1_main(void) printf("Vectoring to next stage...\n"); + for (iodev_id_t id = 0; id < IODEV_NUM; id++) { + if (id == IODEV_UART || id == IODEV_DOCKCHANNEL_UART) + continue; + iodev_set_usage(id, 0); + } + next_stage.entry(next_stage.args[0], next_stage.args[1], next_stage.args[2], next_stage.args[3], next_stage.args[4]); From c3e42b6925df49293e9a51be17ac63dd558ef847 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Tue, 7 Jul 2026 18:10:31 +0200 Subject: [PATCH 2/8] ld: Align boundaries to 64KiB Pre-M1 SoCs support 64 KiB pagesizes and we will need to map m1n1's text and data sections from inside the booted OS kernel to provide PSCI support. Align all section to 64 KiB so that this is cleanly possible. Signed-off-by: Sven Peter --- m1n1-raw.ld | 22 +++++++++++----------- m1n1.ld | 24 ++++++++++++------------ 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/m1n1-raw.ld b/m1n1-raw.ld index 961ed3ae3..b01177483 100644 --- a/m1n1-raw.ld +++ b/m1n1-raw.ld @@ -18,19 +18,19 @@ SECTIONS { _base = .; _text_start = .; - .init : ALIGN(0x4000) { + .init : ALIGN(0x10000) { *(.init) *(.init.*) } :text - .text : ALIGN(0x4000) { + .text : ALIGN(0x10000) { *(.text) *(.text.*) . = ALIGN(8); *(.got.plt) - . = ALIGN(0x4000); + . = ALIGN(0x10000); } :text _text_size = . - _text_start; - .rodata : ALIGN(0x4000) { + .rodata : ALIGN(0x10000) { *(.rodata) *(.rodata.*) . = ALIGN(8); @@ -50,21 +50,21 @@ SECTIONS { *(.rela.rodata*) *(.rela.dyn) _rela_end = .; - . = ALIGN(0x4000); + . = ALIGN(0x10000); } :rodata _rodata_end = .; _data_start = .; - .data : ALIGN(0x4000) { + .data : ALIGN(0x10000) { *(.data) *(.data.*) . = ALIGN(8); _got_start = .; *(.got) _got_end = .; - . = ALIGN(0x4000); + . = ALIGN(0x10000); _file_end = .; } :data - .bss : ALIGN(0x4000) { + .bss : ALIGN(0x10000) { _bss_start = .; *(.bss) *(.bss.*) @@ -72,19 +72,19 @@ SECTIONS { *(COMMON) _bss_end = .; } : data - .stack_el3 : ALIGN(0x4000) { + .stack_el3 : ALIGN(0x10000) { PROVIDE(_stack_top_el3 = .); . += _stack_size_el3 - 8; QUAD(0x5176694b43415453); PROVIDE(_stack_bot_el3 = .); } - .stack : ALIGN(0x4000) { + .stack : ALIGN(0x10000) { PROVIDE(_stack_top = .); . += _stack_size - 8; QUAD(0x544f424b43415453); PROVIDE(_stack_bot = .); } :data - ASSERT(ALIGN(0x4000) == ., "Stack size is not aligned!") + ASSERT(ALIGN(0x10000) == ., "Stack size is not aligned!") _data_size = . - _data_start; _end = .; _payload_start = .; diff --git a/m1n1.ld b/m1n1.ld index 890c9e947..c391506ef 100644 --- a/m1n1.ld +++ b/m1n1.ld @@ -119,23 +119,23 @@ SECTIONS { _cmd_end = .; - . = ALIGN(0x4000); + . = ALIGN(0x10000); _hdr_end = .; } :hdr _text_start = .; - .init : ALIGN(0x4000) { + .init : ALIGN(0x10000) { *(.init) *(.init.*) } :text - .text : ALIGN(0x4000) { + .text : ALIGN(0x10000) { *(.text) *(.text.*) . = ALIGN(8); *(.got.plt) - . = ALIGN(0x4000); + . = ALIGN(0x10000); } :text _text_size = . - _text_start; - .rodata : ALIGN(0x4000) { + .rodata : ALIGN(0x10000) { *(.rodata) *(.rodata.*) . = ALIGN(8); @@ -155,36 +155,36 @@ SECTIONS { *(.rela.rodata*) *(.rela.dyn) _rela_end = .; - . = ALIGN(0x4000); + . = ALIGN(0x10000); } :rodata _rodata_end = .; _data_start = .; - .data : ALIGN(0x4000) { + .data : ALIGN(0x10000) { *(.data) *(.data.*) . = ALIGN(8); _got_start = .; *(.got) _got_end = .; - . = ALIGN(0x4000); + . = ALIGN(0x10000); _file_end = .; } :data - .bss : ALIGN(0x4000) { + .bss : ALIGN(0x10000) { _bss_start = .; *(.bss) *(.bss.*) *(.dynbss) *(COMMON) - . = ALIGN(0x4000); + . = ALIGN(0x10000); _bss_end = .; PROVIDE(_stack_top_el3 = .); . += _stack_size_el3; PROVIDE(_stack_bot_el3 = .); - . = ALIGN(0x4000); + . = ALIGN(0x10000); PROVIDE(_stack_top = .); . += _stack_size; PROVIDE(_stack_bot = .); - . = ALIGN(0x4000); + . = ALIGN(0x10000); } :data _data_size = . - _data_start; _end = .; From 523d36eaaecb53b625754ee600d51af7cc97f933 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Tue, 7 Jul 2026 18:10:32 +0200 Subject: [PATCH 3/8] smp: Allocate stacks statically inside .bss instead of using the heap We're about to add PSCI code that will run inside the OS kernel with the kernel having full control over the pagetables. When stacks are allocated on the heap we have to specifically map them as EFI RUNTIME descriptors which is a bit annoying to get right. Instead, just put them into .bss which will be mapped anyway. This increases .bss by a bit more than 1MB which is negligible on these machines with >8GiB RAM. Signed-off-by: Sven Peter --- src/kboot.c | 8 -------- src/smp.c | 7 ++----- src/smp.h | 4 ++-- 3 files changed, 4 insertions(+), 15 deletions(-) diff --git a/src/kboot.c b/src/kboot.c index a5bd4f1e6..3d55f3bb1 100644 --- a/src/kboot.c +++ b/src/kboot.c @@ -579,14 +579,6 @@ static int dt_set_cpus(void) cpu++; node = next; continue; - } else { - printf("FDT: Reserving stack for CPU %d 0x%lx\n", cpu, (uint64_t)secondary_stacks[cpu]); - fdt_add_mem_rsv(dt, (uint64_t)secondary_stacks[cpu], SECONDARY_STACK_SIZE); - if (has_el3()) { - printf("FDT: Reserving EL3 stack for CPU %d 0x%lx\n", cpu, - (uint64_t)secondary_stacks_el3[cpu]); - fdt_add_mem_rsv(dt, (uint64_t)secondary_stacks_el3[cpu], SECONDARY_STACK_SIZE); - } } u64 mpidr = smp_get_mpidr(cpu); diff --git a/src/smp.c b/src/smp.c index 7db76b6b0..6139ed83d 100644 --- a/src/smp.c +++ b/src/smp.c @@ -5,7 +5,6 @@ #include "aic.h" #include "aic_regs.h" #include "cpu_regs.h" -#include "malloc.h" #include "memory.h" #include "pmgr.h" #include "soc.h" @@ -42,8 +41,8 @@ void *_reset_stack_el1; u8 dummy_stack[DUMMY_STACK_SIZE]; // Highest EL u8 dummy_stack_el1[DUMMY_STACK_SIZE]; // EL1 stack if EL3 exists -u8 *secondary_stacks[MAX_CPUS] = {dummy_stack}; -u8 *secondary_stacks_el3[MAX_EL3_CPUS]; +u8 secondary_stacks[MAX_CPUS][SECONDARY_STACK_SIZE] ALIGNED(0x4000); +u8 secondary_stacks_el3[MAX_EL3_CPUS][SECONDARY_STACK_SIZE] ALIGNED(0x4000); static bool wfe_mode = false; @@ -135,9 +134,7 @@ static void smp_start_cpu(int index, int die, int cluster, int core, u64 impl, u memset(&spin_table[index], 0, sizeof(struct spin_table)); target_cpu = index; - secondary_stacks[index] = memalign(0x4000, SECONDARY_STACK_SIZE); if (has_el3()) { - secondary_stacks_el3[index] = memalign(0x4000, SECONDARY_STACK_SIZE); _reset_stack = secondary_stacks_el3[index] + SECONDARY_STACK_SIZE; // EL3 _reset_stack_el1 = secondary_stacks[index] + SECONDARY_STACK_SIZE; // EL1 diff --git a/src/smp.h b/src/smp.h index 029697864..45998c69e 100644 --- a/src/smp.h +++ b/src/smp.h @@ -10,8 +10,8 @@ #define MAX_EL3_CPUS 4 #define SECONDARY_STACK_SIZE 0x10000 -extern u8 *secondary_stacks[MAX_CPUS]; -extern u8 *secondary_stacks_el3[MAX_EL3_CPUS]; +extern u8 secondary_stacks[MAX_CPUS][SECONDARY_STACK_SIZE]; +extern u8 secondary_stacks_el3[MAX_EL3_CPUS][SECONDARY_STACK_SIZE]; void smp_secondary_entry(void); void smp_secondary_prep_el3(void); From 8e4f1012242a36317650c02389df7472a0fabdda Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Tue, 7 Jul 2026 18:10:34 +0200 Subject: [PATCH 4/8] psci: Add initial implementation for cpu start and suspend Signed-off-by: Sven Peter --- Makefile | 1 + config.h | 3 ++ src/psci.c | 120 +++++++++++++++++++++++++++++++++++++++++++++++++++++ src/psci.h | 59 ++++++++++++++++++++++++++ 4 files changed, 183 insertions(+) create mode 100644 src/psci.c create mode 100644 src/psci.h diff --git a/Makefile b/Makefile index 1ee603e58..df0ad91a6 100644 --- a/Makefile +++ b/Makefile @@ -160,6 +160,7 @@ OBJECTS := \ pcie.o \ pmgr.o \ proxy.o \ + psci.o \ ringbuffer.o \ rtkit.o \ sart.o \ diff --git a/config.h b/config.h index d826a0b92..e72be0075 100644 --- a/config.h +++ b/config.h @@ -26,6 +26,9 @@ // Switch the DFU USB-C port to debugusb // #define USE_DEBUG_USB +// Enable debug printf inside PSCI code +#define PSCI_DEBUG + #ifdef RELEASE # define FB_SILENT_MODE # ifdef CHAINLOADING diff --git a/src/psci.c b/src/psci.c new file mode 100644 index 000000000..36a31bc53 --- /dev/null +++ b/src/psci.c @@ -0,0 +1,120 @@ +/* SPDX-License-Identifier: MIT */ + +#include "psci.h" +#include "smp.h" +#include "types.h" +#include "utils.h" + +#ifdef PSCI_DEBUG +#define psci_printf(fmt, ...) printf("PSCI: " fmt, ##__VA_ARGS__) +#else +#define psci_printf(...) \ + do { \ + } while (0) +#endif + +enum psci_affinity_info_t { + PSCI_AFFINITY_INFO_ON = 0, + PSCI_AFFINITY_INFO_OFF = 1, + PSCI_AFFINITY_INFO_ON_PENDING = 2, +}; + +static unsigned long psci_cpu_suspend(unsigned long power_state, unsigned long entry_point, + unsigned long context_id) +{ + switch (power_state) { + case 0: + if (!cpu_features->apple_sysregs_unlocked) + return PSCI_RET_NOT_SUPPORTED; + sysop("dsb sy"); + sysop("wfi"); + return PSCI_RET_SUCCESS; + break; + case 1: + deep_wfi(); + return PSCI_RET_SUCCESS; + break; + default: + psci_printf("CPU_SUSPEND(power_state=0x%lx, entry_point=0x%lx, context_id=0x%lx)\n", + power_state, entry_point, context_id); + return PSCI_RET_NOT_SUPPORTED; + } +} + +static unsigned long psci_cpu_off(void) +{ + psci_printf("CPU_OFF()\n"); + return PSCI_RET_NOT_SUPPORTED; +} + +static unsigned long psci_cpu_on(unsigned long target_cpu, unsigned long entry_point, + unsigned long context_id) +{ + int cpu = -1; + + if (!entry_point) + return PSCI_RET_INVALID_ADDRESS; + + for (int i = 0; i < MAX_CPUS; i++) { + if (!smp_is_alive(i)) + continue; + if (smp_get_mpidr(i) == (target_cpu & 0xFFFFFF)) { + cpu = i; + break; + } + } + + if (cpu == -1) + return PSCI_RET_INVALID_PARAMS; + + if (read64(smp_get_release_addr(cpu))) + return PSCI_RET_ALREADY_ON; + + psci_printf("CPU_ON: releasing CPU %d (mpidr=0x%lx) to 0x%lx\n", cpu, target_cpu, entry_point); + + smp_call1(cpu, (void *)entry_point, context_id); + + return PSCI_RET_SUCCESS; +} + +static unsigned long psci_affinity_info(unsigned long target_affinity, + unsigned long lowest_affinity_level) +{ + psci_printf("AFFINITY_INFO(target_affinity=0x%lx, lowest_affinity_level=0x%lx)\n", + target_affinity, lowest_affinity_level); + return PSCI_AFFINITY_INFO_OFF; +} + +unsigned long efi_psci_handler(unsigned long fid, unsigned long arg0, unsigned long arg1, + unsigned long arg2) +{ + switch (fid) { + case PSCI_0_2_FN64_CPU_SUSPEND: + return psci_cpu_suspend(arg0, arg1, arg2); + case PSCI_0_2_FN_CPU_OFF: + return psci_cpu_off(); + case PSCI_0_2_FN64_CPU_ON: + return psci_cpu_on(arg0, arg1, arg2); + case PSCI_0_2_FN64_AFFINITY_INFO: + return psci_affinity_info(arg0, arg1); + default: + psci_printf("unsupported fid 0x%lx (arg0=0x%lx, arg1=0x%lx, arg2=0x%lx)\n", fid, arg0, + arg1, arg2); + return PSCI_RET_NOT_SUPPORTED; + } +} + +const struct psci_efi_table psci_efi_table = { + .version = PSCI_VERSION_1_0, + .handler = efi_psci_handler, + .features = + { + [0 ... PSCI_MAX_FN - 1] = PSCI_RET_NOT_SUPPORTED, + [PSCI_FN(PSCI_0_2_FN_PSCI_VERSION)] = PSCI_RET_SUCCESS, + [PSCI_FN(PSCI_1_0_FN_PSCI_FEATURES)] = PSCI_RET_SUCCESS, + [PSCI_FN(PSCI_0_2_FN64_CPU_SUSPEND)] = PSCI_RET_SUCCESS, + [PSCI_FN(PSCI_0_2_FN_CPU_OFF)] = PSCI_RET_SUCCESS, + [PSCI_FN(PSCI_0_2_FN64_CPU_ON)] = PSCI_RET_SUCCESS, + [PSCI_FN(PSCI_0_2_FN64_AFFINITY_INFO)] = PSCI_RET_SUCCESS, + }, +}; diff --git a/src/psci.h b/src/psci.h new file mode 100644 index 000000000..7267104ca --- /dev/null +++ b/src/psci.h @@ -0,0 +1,59 @@ +/* SPDX-License-Identifier: MIT */ + +#ifndef PSCI_H +#define PSCI_H + +#include "types.h" + +#define PSCI_VERSION_1_0 0x00010000 + +/* + * PSCI function numbers are the low byte of a function ID and are identical for + * the SMC32 and SMC64 variants. All PSCI 1.x functions stay below this, so the + * feature table is indexed by function number rather than by full fid. + */ +#define PSCI_MAX_FN 0x20 + +#define PSCI_FN(fid) ((fid) & 0xff) + +enum psci_fn_id_t { + PSCI_0_2_FN_PSCI_VERSION = 0x84000000, + PSCI_0_2_FN64_CPU_SUSPEND = 0xc4000001, + PSCI_0_2_FN_CPU_OFF = 0x84000002, + PSCI_0_2_FN64_CPU_ON = 0xc4000003, + PSCI_0_2_FN64_AFFINITY_INFO = 0xc4000004, + PSCI_1_0_FN_PSCI_FEATURES = 0x8400000a, +}; + +enum psci_ret_t { + PSCI_RET_SUCCESS = 0, + PSCI_RET_NOT_SUPPORTED = -1, + PSCI_RET_INVALID_PARAMS = -2, + PSCI_RET_DENIED = -3, + PSCI_RET_ALREADY_ON = -4, + PSCI_RET_ON_PENDING = -5, + PSCI_RET_INTERNAL_FAILURE = -6, + PSCI_RET_NOT_PRESENT = -7, + PSCI_RET_DISABLED = -8, + PSCI_RET_INVALID_ADDRESS = -9, +}; + +unsigned long efi_psci_handler(unsigned long fid, unsigned long arg0, unsigned long arg1, + unsigned long arg2); + +/* + * Payload of the LINUX_EFI_ARM_PSCI_HANDLER_TABLE EFI config table, which lets + * the kernel call into the resident PSCI implementation via a plain function + * pointer instead of SMC/HVC. Layout must match the kernel's + * struct efi_arm_psci_handler_table. + */ +struct psci_efi_table { + u32 version; + unsigned long (*handler)(unsigned long fid, unsigned long arg0, unsigned long arg1, + unsigned long arg2); + s32 features[PSCI_MAX_FN]; +}; + +extern const struct psci_efi_table psci_efi_table; + +#endif From ad71585b3507f89d6e99ad8a817b556ece384b90 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Tue, 7 Jul 2026 18:10:35 +0200 Subject: [PATCH 5/8] adt.rs: Add ADTProperty.bytes Signed-off-by: Sven Peter --- rust/src/adt.rs | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rust/src/adt.rs b/rust/src/adt.rs index e1b9c516f..5e7320f32 100644 --- a/rust/src/adt.rs +++ b/rust/src/adt.rs @@ -707,6 +707,12 @@ impl ADTProperty { Ok(u64::from_ne_bytes(self.value[..8].try_into().unwrap())) } + + pub fn bytes(&self) -> &[u8] { + // SAFETY: The value is self.size bytes long and we never reach this code if we + // aren't already certain that we are a valid property + unsafe { core::slice::from_raw_parts(self.value.as_ptr(), self.size as usize) } + } } extern "C" { From 42d81ec3cd00e0c3fa9b113d21626adb0222cb65 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Tue, 7 Jul 2026 18:10:36 +0200 Subject: [PATCH 6/8] efi: Add stub EFI runtime services to expose PSCI Signed-off-by: Sven Peter --- m1n1.conf.example | 10 + rust/src/efi.rs | 565 ++++++++++++++++++++++++++++++++++++++++++++++ rust/src/lib.rs | 1 + src/efi.h | 10 + src/kboot.c | 69 +++++- src/kboot.h | 7 + src/payload.c | 7 + 7 files changed, 665 insertions(+), 4 deletions(-) create mode 100644 rust/src/efi.rs create mode 100644 src/efi.h diff --git a/m1n1.conf.example b/m1n1.conf.example index b2e056a25..4878d4f50 100644 --- a/m1n1.conf.example +++ b/m1n1.conf.example @@ -25,3 +25,13 @@ # - gofetch: Mitigate the GoFetch vulnerability (disable the DMP systemwide) # Affected CPUs: M1 and M2 series (M3 and later are unaffected) #mitigations=gofetch + +# PSCI modes +# Available options: +# - off: Use spin-table for CPU bringup and leave sleep states to OS +# - direct: Expose PSCI-via-EFI for CPU bringup and sleep states and expose +# a minimal EFI implementation with EFI runtime services to the +# OS. This mode is only meant for development when chainloading +# the OS kernel directly and will break when another second stage +# loader like e.g. u-boot is used. +#psci=off diff --git a/rust/src/efi.rs b/rust/src/efi.rs new file mode 100644 index 000000000..26ae897b3 --- /dev/null +++ b/rust/src/efi.rs @@ -0,0 +1,565 @@ +// SPDX-License-Identifier: MIT + +use alloc::vec::Vec; +use core::cell::UnsafeCell; +use core::ffi::{c_char, c_int, c_uint, c_void, CStr}; +use core::mem::size_of; +use core::ptr; + +use crate::adt::ADTNode; +use crate::println; + +const EFI_PAGE_SIZE: u64 = 4096; +const EFI_RT_REGION_ALIGN: u64 = 0x10000; + +const EFI_SYSTEM_TABLE_SIGNATURE: u64 = 0x5453595320494249; +const EFI_RUNTIME_SERVICES_SIGNATURE: u64 = 0x56524553544e5552; +const EFI_SYSTEM_TABLE_REVISION: u32 = 2 << 16; + +const EFI_RUNTIME_SERVICES_CODE: u32 = 4; +const EFI_RUNTIME_SERVICES_DATA: u32 = 5; +const EFI_CONVENTIONAL_MEMORY: u32 = 7; +const EFI_MEMORY_MAPPED_IO: u32 = 11; + +const EFI_MAX_MEM_DESC: usize = 128; + +const EFI_MEMORY_UC: u64 = 0x0000000000000001; +const EFI_MEMORY_WB: u64 = 0x0000000000000008; +const EFI_MEMORY_XP: u64 = 0x0000000000004000; +const EFI_MEMORY_RO: u64 = 0x0000000000020000; +const EFI_MEMORY_RUNTIME: u64 = 0x8000000000000000; +const EFI_MEMORY_ISA_VALID: u64 = 0x4000000000000000; +const EFI_MEMORY_ISA_SHIFT: u64 = 44; + +const fn efi_memory_isa(mair: u16) -> u64 { + EFI_MEMORY_ISA_VALID | ((mair as u64) << EFI_MEMORY_ISA_SHIFT) +} + +#[allow(non_upper_case_globals)] +const EFI_RT_MMIO_nGnRnE: u64 = EFI_MEMORY_UC | efi_memory_isa(0x00) | EFI_MEMORY_RUNTIME; + +const EFI_INVALID_TABLE_ADDR: u64 = !0; + +const EFI_RT_PROPERTIES_TABLE_VERSION: u16 = 0x1; + +extern "C" { + static _base: u8; + static _data_start: u8; + static _end: u8; + + static psci_efi_table: u8; + + fn fdt_path_offset(fdt: *const c_void, path: *const c_char) -> c_int; + fn fdt_getprop( + fdt: *const c_void, + nodeoffset: c_int, + name: *const c_char, + lenp: *mut c_int, + ) -> *const c_void; + fn fdt_setprop( + fdt: *mut c_void, + nodeoffset: c_int, + name: *const c_char, + val: *const c_void, + len: c_int, + ) -> c_int; + + fn tinf_crc32(data: *const c_void, length: c_uint) -> c_uint; +} + +const ARM_PSCI_TABLE_GUID: [u8; 16] = [ + 0x51, 0x76, 0xb4, 0xf9, 0x74, 0x46, 0xd9, 0x4e, 0x94, 0x3b, 0x5c, 0xea, 0xd2, 0x10, 0x9a, 0x8b, +]; +const RT_PROPERTIES_TABLE_GUID: [u8; 16] = [ + 0x8a, 0x91, 0x66, 0xeb, 0xef, 0x7e, 0x2a, 0x40, 0x84, 0x2e, 0x93, 0x1d, 0x21, 0xc3, 0x8a, 0xe9, +]; + +#[allow(dead_code)] +#[repr(C)] +struct TableHeader { + signature: u64, + revision: u32, + headersize: u32, + crc32: u32, + reserved: u32, +} + +impl TableHeader { + const fn new(signature: u64) -> Self { + Self { + signature, + revision: EFI_SYSTEM_TABLE_REVISION, + headersize: 0, + crc32: 0, + reserved: 0, + } + } +} + +#[repr(C)] +struct MemoryDescriptor { + ty: u32, + pad: u32, + phys_addr: u64, + virt_addr: u64, + num_pages: u64, + attribute: u64, +} + +impl MemoryDescriptor { + const ZERO: Self = Self { + ty: 0, + pad: 0, + phys_addr: 0, + virt_addr: 0, + num_pages: 0, + attribute: 0, + }; +} + +#[allow(dead_code)] +#[repr(C)] +struct RuntimeServices { + hdr: TableHeader, + services: [u64; 14], +} + +#[allow(dead_code)] +#[repr(C)] +struct SystemTable { + hdr: TableHeader, + fw_vendor: u64, + fw_revision: u32, + pad: u32, + con_in_handle: u64, + con_in: u64, + con_out_handle: u64, + con_out: u64, + stderr_handle: u64, + stderr: u64, + runtime: u64, + boottime: u64, + nr_tables: u64, + tables: u64, +} + +#[allow(dead_code)] +#[repr(C)] +struct ConfigTable { + guid: [u8; 16], + table: u64, +} + +#[allow(dead_code)] +#[repr(C)] +struct RtPropertiesTable { + version: u16, + length: u16, + runtime_services_supported: u32, +} + +#[repr(transparent)] +struct SyncUnsafeCell(UnsafeCell); + +// SAFETY: must be specified everywhere this is used +unsafe impl Sync for SyncUnsafeCell {} + +impl SyncUnsafeCell { + const fn new(value: T) -> Self { + Self(UnsafeCell::new(value)) + } + + const fn get(&self) -> *mut T { + self.0.get() + } + + fn addr(&self) -> u64 { + self.get() as u64 + } +} + +static EFI_VENDOR: [u16; 5] = [b'm' as u16, b'1' as u16, b'n' as u16, b'1' as u16, 0]; + +// SAFETY: m1n1 runs single-threaded and this table is only used in dt_setup_efi() +static EFI_MEMMAP: SyncUnsafeCell<[MemoryDescriptor; EFI_MAX_MEM_DESC]> = + SyncUnsafeCell::new([MemoryDescriptor::ZERO; EFI_MAX_MEM_DESC]); + +// SAFETY: m1n1 runs single-threaded and this table is only used in dt_setup_efi() +static EFI_RUNTIME: SyncUnsafeCell = SyncUnsafeCell::new(RuntimeServices { + hdr: TableHeader::new(EFI_RUNTIME_SERVICES_SIGNATURE), + services: [EFI_INVALID_TABLE_ADDR; 14], +}); + +static EFI_RT_PROPERTIES: RtPropertiesTable = RtPropertiesTable { + version: EFI_RT_PROPERTIES_TABLE_VERSION, + length: size_of::() as u16, + runtime_services_supported: 0, +}; + +// SAFETY: m1n1 runs single-threaded and this table is only used in dt_setup_efi() +static EFI_CONFIG_TABLES: SyncUnsafeCell<[ConfigTable; 2]> = SyncUnsafeCell::new([ + ConfigTable { + guid: ARM_PSCI_TABLE_GUID, + table: 0, + }, + ConfigTable { + guid: RT_PROPERTIES_TABLE_GUID, + table: 0, + }, +]); + +// SAFETY: m1n1 runs single-threaded and this table is only used in dt_setup_efi() +static EFI_SYSTAB: SyncUnsafeCell = SyncUnsafeCell::new(SystemTable { + hdr: TableHeader::new(EFI_SYSTEM_TABLE_SIGNATURE), + fw_vendor: 0, + fw_revision: 0, + pad: 0, + con_in_handle: 0, + con_in: 0, + con_out_handle: 0, + con_out: 0, + stderr_handle: 0, + stderr: 0, + runtime: 0, + boottime: 0, + nr_tables: 0, + tables: 0, +}); + +/// # Safety +/// +/// Implementors must be `#[repr(C)]` with no padding bytes: the whole table +/// is fed to tinf_crc32, so every byte must be initialized. `table_update_crc()` +/// verifies this against `UEFI_EXPECTED_SIZE` at compile time. +unsafe trait Table: Sized { + // expected size from the UEFI specification + const UEFI_EXPECTED_SIZE: usize; + fn header_mut(&mut self) -> &mut TableHeader; +} + +unsafe impl Table for RuntimeServices { + const UEFI_EXPECTED_SIZE: usize = 136; + fn header_mut(&mut self) -> &mut TableHeader { + &mut self.hdr + } +} + +unsafe impl Table for SystemTable { + const UEFI_EXPECTED_SIZE: usize = 120; + fn header_mut(&mut self) -> &mut TableHeader { + &mut self.hdr + } +} + +fn table_update_crc(table: &mut T) { + const { + assert!(size_of::() == T::UEFI_EXPECTED_SIZE); + } + table.header_mut().headersize = size_of::() as u32; + table.header_mut().crc32 = 0; + // SAFETY: `table` is a fully initialized repr(C) struct without padding + // (checked above); tinf_crc32 only reads size_of::() bytes from it. + let crc = unsafe { tinf_crc32(ptr::from_ref(table).cast(), size_of::() as c_uint) }; + table.header_mut().crc32 = crc; +} + +trait PreviousMultipleOf { + fn previous_multiple_of(self, align: Self) -> Self; +} + +impl PreviousMultipleOf for u64 { + fn previous_multiple_of(self, align: u64) -> u64 { + self & !(align - 1) + } +} + +#[derive(Clone, Copy)] +struct MemRange { + base: u64, + end: u64, +} + +impl MemRange { + fn size(&self) -> u64 { + self.end - self.base + } + + fn overlaps(&self, other: MemRange) -> bool { + self.base < other.end && self.end > other.base + } +} + +struct MemoryMapBuilder { + entries: &'static mut [MemoryDescriptor; EFI_MAX_MEM_DESC], + used: usize, +} + +impl MemoryMapBuilder { + fn add(&mut self, ty: u32, base: u64, size: u64, attribute: u64) -> Result<(), c_int> { + if size == 0 { + return Ok(()); + } + + // TODO: ensure size is aligned + + if self.used >= self.entries.len() { + println!( + "FDT: EFI memory map overflow, can't add region 0x{:x}..0x{:x}", + base, + base + size + ); + return Err(-1); + } + + self.entries[self.used] = MemoryDescriptor { + ty, + pad: 0, + phys_addr: base, + virt_addr: if attribute & EFI_MEMORY_RUNTIME != 0 { + base + } else { + 0 + }, + num_pages: size / EFI_PAGE_SIZE, + attribute, + }; + self.used += 1; + Ok(()) + } +} + +fn fdt_node(dt: *mut c_void, path: &CStr) -> Result { + // SAFETY: dt points to a valid FDT via dt_setup_efi() + let node = unsafe { fdt_path_offset(dt, path.as_ptr()) }; + if node < 0 { + Err(node) + } else { + Ok(node) + } +} + +fn fdt_set_prop(dt: *mut c_void, node: c_int, name: &CStr, val: &[u8]) -> Result<(), c_int> { + // SAFETY: dt points to a valid FDT via dt_setup_efi() + let ret = unsafe { + fdt_setprop( + dt, + node, + name.as_ptr(), + val.as_ptr().cast(), + val.len() as c_int, + ) + }; + if ret == 0 { + Ok(()) + } else { + Err(ret) + } +} + +fn fdt_set_u64(dt: *mut c_void, node: c_int, name: &CStr, val: u64) -> Result<(), c_int> { + fdt_set_prop(dt, node, name, &val.to_be_bytes()) +} + +fn fdt_set_u32(dt: *mut c_void, node: c_int, name: &CStr, val: u32) -> Result<(), c_int> { + fdt_set_prop(dt, node, name, &val.to_be_bytes()) +} + +fn efi_init_tables() { + // SAFETY: Single-threaded and only called once before the tables are handed to the kernel + unsafe { + let tables = &mut *EFI_CONFIG_TABLES.get(); + tables[0].table = &raw const psci_efi_table as u64; + tables[1].table = &raw const EFI_RT_PROPERTIES as u64; + + table_update_crc(&mut *EFI_RUNTIME.get()); + + let systab = &mut *EFI_SYSTAB.get(); + systab.fw_vendor = EFI_VENDOR.as_ptr() as u64; + systab.runtime = EFI_RUNTIME.addr(); + systab.nr_tables = tables.len() as u64; + systab.tables = EFI_CONFIG_TABLES.addr(); + table_update_crc(systab); + } +} + +fn memmap_add_mmio(map: &mut MemoryMapBuilder) -> Result<(), c_int> { + let Ok(ranges) = ADTNode::from_path("/arm-io").and_then(|node| node.named_prop("ranges")) + else { + println!("FDT: EFI: no /arm-io ranges"); + return Err(-1); + }; + + // Grab all MMIO ranges from the ADT and align to the highest page size + // the kernel might use (64 KiB for pre-M1 SoCs) + // TODO: this assumes #{address,size}-cells = 2 and is probably going to + // fail in funny ways otherwise + let (entries, _) = ranges.bytes().as_chunks::<8>().0.as_chunks::<3>(); + let mut mmio: Vec = entries + .iter() + .filter_map(|&[_bus, phys, size]| { + let (phys, size) = (u64::from_ne_bytes(phys), u64::from_ne_bytes(size)); + (size != 0).then_some(MemRange { + base: phys.previous_multiple_of(EFI_RT_REGION_ALIGN), + end: (phys + size).next_multiple_of(EFI_RT_REGION_ALIGN), + }) + }) + .collect(); + + // Merge overlapping or adjacent windows because the kernel will BUG_ON if + // we have any of those + mmio.sort_unstable_by_key(|range| range.base); + mmio.dedup_by(|range, last| { + if range.base <= last.end { + last.end = last.end.max(range.end); + true + } else { + false + } + }); + + for range in &mmio { + // Technically we would need to map PCIe BAR space as Device-nGnRE here + // but since that's not used in the PSCI code we just ignore that for now... + map.add( + EFI_MEMORY_MAPPED_IO, + range.base, + range.size(), + EFI_RT_MMIO_nGnRnE, + )?; + } + + Ok(()) +} + +fn fdt_get_memory_ranges(dt: *mut c_void) -> Result, c_int> { + let memory = fdt_node(dt, c"/memory")?; + let mut reg_len: c_int = 0; + // SAFETY: On failure fdt_getprop returns NULL and stores the FDT + // error in reg_len, otherwise the pointer is valid for reg_len bytes + // until the FDT is next modified and it is consumed within this + // function where we don't modify the FDT. + let reg = unsafe { + let reg = fdt_getprop(dt, memory, c"reg".as_ptr(), &mut reg_len); + if reg.is_null() { + return Err(reg_len); + } + core::slice::from_raw_parts(reg.cast::(), reg_len as usize) + }; + let (pairs, _) = reg.as_chunks::<8>().0.as_chunks::<2>(); + Ok(pairs + .iter() + .map(|&[base, size]| { + let base = u64::from_be_bytes(base); + MemRange { + base, + end: base + u64::from_be_bytes(size), + } + }) + .collect()) +} + +fn efi_publish_to_fdt(dt: *mut c_void, map: &MemoryMapBuilder) -> Result<(), c_int> { + let chosen = fdt_node(dt, c"/chosen")?; + fdt_set_u64(dt, chosen, c"linux,uefi-system-table", EFI_SYSTAB.addr())?; + fdt_set_u64(dt, chosen, c"linux,uefi-mmap-start", EFI_MEMMAP.addr())?; + fdt_set_u32( + dt, + chosen, + c"linux,uefi-mmap-size", + (map.used * size_of::()) as u32, + )?; + fdt_set_u32( + dt, + chosen, + c"linux,uefi-mmap-desc-size", + size_of::() as u32, + )?; + fdt_set_u32(dt, chosen, c"linux,uefi-mmap-desc-ver", 1)?; + + Ok(()) +} + +fn efi_setup(dt: *mut c_void) -> Result<(), c_int> { + efi_init_tables(); + + let m1n1_base = &raw const _base as u64; + let m1n1_data_start = &raw const _data_start as u64; + let m1n1_end = &raw const _end as u64; + + if (m1n1_base | m1n1_data_start | m1n1_end) & (EFI_RT_REGION_ALIGN - 1) != 0 { + println!( + "FDT: EFI: m1n1 image not 0x{:x}-aligned (base 0x{:x}, data 0x{:x}, end 0x{:x})", + EFI_RT_REGION_ALIGN, m1n1_base, m1n1_data_start, m1n1_end + ); + // TODO: can't fail here because base of image might only be 16KiB aligned + //return Err(-1); + } + + // SAFETY: Single-threaded and this is the only mutable reference to EFI_MEMMAP + let mut map = MemoryMapBuilder { + entries: unsafe { &mut *EFI_MEMMAP.get() }, + used: 0, + }; + + // Add /memory from FDT but carve out m1n1 because we later add it as runtime memory + // and the UEFI spec does not allow overlapping memory ranges + let m1n1 = MemRange { + base: m1n1_base, + end: m1n1_end, + }; + for range in &fdt_get_memory_ranges(dt)? { + let mut pos = range.base; + let end = range.end; + + if m1n1.overlaps(*range) { + if m1n1.base > pos { + map.add(EFI_CONVENTIONAL_MEMORY, pos, m1n1.base - pos, EFI_MEMORY_WB)?; + } + pos = m1n1.end; + } + if pos < end { + map.add(EFI_CONVENTIONAL_MEMORY, pos, end - pos, EFI_MEMORY_WB)?; + } + } + + // add m1n1 code and rodata mapped as RX during EFI runtime calls + map.add( + EFI_RUNTIME_SERVICES_CODE, + m1n1_base, + m1n1_data_start - m1n1_base, + EFI_MEMORY_WB | EFI_MEMORY_RO | EFI_MEMORY_RUNTIME, + )?; + + // add m1n1 data and bss mapped as RW during EFI runtime calls + map.add( + EFI_RUNTIME_SERVICES_DATA, + m1n1_data_start, + m1n1_end - m1n1_data_start, + EFI_MEMORY_WB | EFI_MEMORY_XP | EFI_MEMORY_RUNTIME, + )?; + + // add all MMIO mapped as RW with Device-nGnRnE semantics during runtime calls + memmap_add_mmio(&mut map)?; + + // and finally publish our EFI stub in the FDT + efi_publish_to_fdt(dt, &map)?; + + println!( + "FDT: EFI system table prepared at 0x{:x}", + EFI_SYSTAB.addr() + ); + + Ok(()) +} + +/// # Safety +/// +/// * `dt` must point to a valid FDT. +/// * /memory must've already been filled by dt_set_memory() +#[no_mangle] +pub unsafe extern "C" fn dt_setup_efi(dt: *mut c_void) -> c_int { + match efi_setup(dt) { + Ok(()) => 0, + Err(err) => err, + } +} diff --git a/rust/src/lib.rs b/rust/src/lib.rs index 5f27bae85..f0fd6f4b3 100644 --- a/rust/src/lib.rs +++ b/rust/src/lib.rs @@ -15,6 +15,7 @@ pub mod adt; #[cfg(feature = "chainload")] pub mod chainload; pub mod dlmalloc; +pub mod efi; pub mod float; #[cfg(feature = "chainload")] pub mod gpt; diff --git a/src/efi.h b/src/efi.h new file mode 100644 index 000000000..69c31600e --- /dev/null +++ b/src/efi.h @@ -0,0 +1,10 @@ +/* SPDX-License-Identifier: MIT */ + +#ifndef EFI_H +#define EFI_H + +#include "types.h" + +int dt_setup_efi(void *dt); + +#endif diff --git a/src/kboot.c b/src/kboot.c index 3d55f3bb1..98c06c9c0 100644 --- a/src/kboot.c +++ b/src/kboot.c @@ -9,6 +9,7 @@ #include "dapf.h" #include "devicetree.h" #include "display.h" +#include "efi.h" #include "exception.h" #include "firmware.h" #include "iodev.h" @@ -49,6 +50,8 @@ static char *uboot_config[MAX_UBOOT_CONFIGS][2]; extern const char *const m1n1_version; +static enum kboot_psci_mode psci_mode = PSCI_DEFAULT; + int dt_set_gpu(void *dt); #define DT_ALIGN 16384 @@ -347,6 +350,11 @@ static int dt_set_uboot_config(void) return 0; } +void kboot_set_psci_mode(enum kboot_psci_mode mode) +{ + psci_mode = mode; +} + static int dt_set_memory(void) { int anode = adt_path_offset(adt, "/chosen"); @@ -586,11 +594,24 @@ static int dt_set_cpus(void) if (dt_mpidr != mpidr) bail_cleanup("FDT: DT CPU %d MPIDR mismatch: 0x%lx != 0x%lx\n", cpu, dt_mpidr, mpidr); - u64 release_addr = smp_get_release_addr(cpu); - if (fdt_setprop_inplace_u64(dt, node, "cpu-release-addr", release_addr)) - bail_cleanup("FDT: couldn't set cpu-release-addr property\n"); + if (psci_mode == PSCI_OFF) { + u64 release_addr = smp_get_release_addr(cpu); + if (fdt_setprop_u64(dt, node, "cpu-release-addr", release_addr)) + bail_cleanup("FDT: couldn't set cpu-release-addr property\n"); + + fdt_setprop_string(dt, node, "enable-method", "spin-table"); + fdt_delprop(dt, node, "cpu-idle-states"); + + printf("FDT: CPU %d MPIDR=0x%lx release-addr=0x%lx\n", cpu, mpidr, release_addr); + } else { + fdt_delprop(dt, node, "cpu-release-addr"); + fdt_setprop_string(dt, node, "enable-method", "psci"); + + if (!fdt_getprop(dt, node, "cpu-idle-states", NULL)) + printf("FDT: WARNING: CPU %d has no cpu-idle-states\n", cpu); - printf("FDT: CPU %d MPIDR=0x%lx release-addr=0x%lx\n", cpu, mpidr, release_addr); + printf("FDT: CPU %d MPIDR=0x%lx uses PSCI\n", cpu, mpidr); + } next_cpu: cpu++; @@ -2738,6 +2759,26 @@ static int dt_setup_mtd_phram(void) return 0; } +static void dt_setup_psci_mode(void) +{ + /* + * For now default to psci=off for all SoCs since older kernels do not support the EFI conduit + * yet. If PSCI-via-EFI lands before M4 is usable we probably want to default to on here then + * for M4+. + */ + if (psci_mode == PSCI_DEFAULT) + psci_mode = PSCI_OFF; + + if (psci_mode == PSCI_OFF) + return; + + if (fdt_path_offset(dt, "/psci") < 0) { + printf("FDT: /psci node not found but PSCI requested; forcing psci=off instead\n"); + psci_mode = PSCI_OFF; + return; + } +} + int kboot_prepare_dt(void *fdt) { if (dt) { @@ -2766,6 +2807,9 @@ int kboot_prepare_dt(void *fdt) /* setup console log buffer early to capture as much log as possible */ dt_setup_mtd_phram(); + /* figure out if we can and want to use PSCI */ + dt_setup_psci_mode(); + if (dt_set_chosen()) return -1; if (dt_set_uboot_config()) @@ -2839,6 +2883,23 @@ int kboot_prepare_dt(void *fdt) if (dt_set_memory()) return -1; + switch (psci_mode) { + case PSCI_DIRECT: + /* + * When installing the EFI stub to support PSCI-via-EFI we also need to setup + * the EFI memory map as a clone of the one in /memory since the kernel will + * ignore the latter in this case. dt_setup_efi will read it back and take care + * of this and thus has to be called after dt_set_memory(). + */ + if (dt_setup_efi(dt)) + bail("FDT: couldn't set up EFI runtime services\n"); + break; + case PSCI_OFF: + break; + case PSCI_DEFAULT: + bail("FDT: PSCI_DEFAULT should not be reachable in %s:%d\n", __FILE__, __LINE__); + } + if (fdt_pack(dt)) bail("FDT: fdt_pack() failed\n"); diff --git a/src/kboot.h b/src/kboot.h index c67c22038..676dc4ad8 100644 --- a/src/kboot.h +++ b/src/kboot.h @@ -20,9 +20,16 @@ struct kernel_header { u32 res5; /* reserved (used for PE COFF offset) */ }; +enum kboot_psci_mode { + PSCI_DEFAULT, + PSCI_OFF, + PSCI_DIRECT, +}; + void kboot_set_initrd(void *start, size_t size); int kboot_set_chosen(const char *name, const char *value); int kboot_set_uboot(const char *name, const char *value); +void kboot_set_psci_mode(enum kboot_psci_mode mode); int kboot_prepare_dt(void *fdt); int kboot_boot(void *kernel); diff --git a/src/payload.c b/src/payload.c index 5b1547fde..2e2f165c7 100644 --- a/src/payload.c +++ b/src/payload.c @@ -205,6 +205,13 @@ static bool check_var(u8 **p) mitigations_configure(val); } else if (IS_VAR("tso=")) { enable_tso = val[0] == '1'; + } else if (IS_VAR("psci=")) { + if (!strcmp(val, "off")) + kboot_set_psci_mode(PSCI_OFF); + else if (!strcmp(val, "direct")) + kboot_set_psci_mode(PSCI_DIRECT); + else + printf("Unknown configuration option: psci=%s\n", val); } else { printf("Unknown variable %s\n", *p); } From 9f7d9bc1be7f1c7854f271a18e532f07deb72e91 Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Wed, 8 Jul 2026 20:24:10 +0200 Subject: [PATCH 7/8] smp: Cache data from ADT The ADT is no longer available once we're called into from PSCI but we may need to bring up or down cores. Just cache all the information we need. Signed-off-by: Sven Peter --- src/main.c | 1 + src/smp.c | 103 +++++++++++++++++++++++++++++++---------------------- src/smp.h | 1 + 3 files changed, 62 insertions(+), 43 deletions(-) diff --git a/src/main.c b/src/main.c index 97f6988cc..e57f6a9b5 100644 --- a/src/main.c +++ b/src/main.c @@ -157,6 +157,7 @@ void m1n1_main(void) mcc_init(); mmu_init(); aic_init(); + smp_init(); #endif wdt_disable(); #ifndef BRINGUP diff --git a/src/smp.c b/src/smp.c index 6139ed83d..168dd55fa 100644 --- a/src/smp.c +++ b/src/smp.c @@ -49,8 +49,18 @@ static bool wfe_mode = false; static int target_cpu; static int cpu_nodes[MAX_CPUS]; static struct spin_table spin_table[MAX_CPUS]; -static u64 pmgr_reg; -static u64 cpu_start_off; + +struct cpu_info { + bool valid; + u8 die; + u8 cluster; + u8 core; + u64 impl_reg; +}; + +static bool smp_initialized = false; +static u64 cpu_start_base; +static struct cpu_info cpu_info[MAX_CPUS]; extern u8 _vectors_start[0]; int boot_cpu_idx = -1; @@ -224,34 +234,38 @@ static void smp_stop_cpu(int index, int die, int cluster, int core, u64 impl, u6 } } -void smp_start_secondaries(void) +int smp_init(void) { - printf("Starting secondary CPUs...\n"); + if (smp_initialized) + return 0; int pmgr_path[8]; + u64 pmgr_reg; + u64 cpu_start_off; if (adt_path_offset_trace(adt, "/arm-io/pmgr", pmgr_path) < 0) { printf("Error getting /arm-io/pmgr node\n"); - return; + return -1; } if (adt_get_reg(adt, pmgr_path, "reg", 0, &pmgr_reg, NULL) < 0) { printf("Error getting /arm-io/pmgr regs\n"); - return; + return -1; } int arm_io_node; if ((arm_io_node = adt_path_offset(adt, "/arm-io")) < 0) { printf("Error getting /arm-io node\n"); - return; + return -1; } int node = adt_path_offset(adt, "/cpus"); if (node < 0) { printf("Error getting /cpus node\n"); - return; + return -1; } memset(cpu_nodes, 0, sizeof(cpu_nodes)); + memset(cpu_info, 0, sizeof(cpu_info)); switch (chip_id) { case S5L8960X: @@ -297,7 +311,7 @@ void smp_start_secondaries(void) break; default: printf("CPU start offset is unknown for this SoC!\n"); - return; + return -1; } ADT_FOREACH_CHILD(adt, node) @@ -343,7 +357,7 @@ void smp_start_secondaries(void) if (boot_cpu_idx == -1) { printf( "Could not find currently running CPU in cpu table, can't start other processors!\n"); - return; + return -1; } spin_table[boot_cpu_idx].mpidr = mrs(MPIDR_EL1) & 0xFFFFFF; @@ -369,62 +383,65 @@ void smp_start_secondaries(void) memcpy(cpu_impl_reg, ®s[index], 16); } + cpu_info[i].valid = true; + cpu_info[i].core = FIELD_GET(CPU_REG_CORE, reg); + cpu_info[i].cluster = FIELD_GET(CPU_REG_CLUSTER, reg); + cpu_info[i].die = FIELD_GET(CPU_REG_DIE, reg); + cpu_info[i].impl_reg = cpu_impl_reg[0]; + } + + cpu_start_base = pmgr_reg + cpu_start_off; + smp_initialized = true; + + return 0; +} + +void smp_start_secondaries(void) +{ + printf("Starting secondary CPUs...\n"); + + if (!smp_initialized) + return; + + for (int i = 0; i < MAX_CPUS; i++) { + struct cpu_info *cpu = &cpu_info[i]; + + if (!cpu->valid) + continue; + if (i == boot_cpu_idx) { // Check if already locked - if (FIELD_GET(RVBAR_LOCK, read64(cpu_impl_reg[0]))) + if (FIELD_GET(RVBAR_LOCK, read64(cpu->impl_reg))) continue; // Unlocked, write _vectors_start into boot CPU's rvbar - write64(cpu_impl_reg[0], (u64)_vectors_start); + write64(cpu->impl_reg, (u64)_vectors_start); sysop("dmb sy"); continue; } - u8 core = FIELD_GET(CPU_REG_CORE, reg); - u8 cluster = FIELD_GET(CPU_REG_CLUSTER, reg); - u8 die = FIELD_GET(CPU_REG_DIE, reg); - - smp_start_cpu(i, die, cluster, core, cpu_impl_reg[0], pmgr_reg + cpu_start_off); + smp_start_cpu(i, cpu->die, cpu->cluster, cpu->core, cpu->impl_reg, cpu_start_base); } } void smp_stop_secondaries(bool deep_sleep) { printf("Stopping secondary CPUs...\n"); - int arm_io_node; - if ((arm_io_node = adt_path_offset(adt, "/arm-io")) < 0) { - printf("Error getting /arm-io node\n"); + + if (!smp_initialized) return; - } + smp_set_wfe_mode(true); for (int i = 0; i < MAX_CPUS; i++) { - int node = cpu_nodes[i]; + struct cpu_info *cpu = &cpu_info[i]; - if (!node) + if (!cpu->valid || i == boot_cpu_idx) continue; - u32 reg; - u64 cpu_impl_reg[2]; - if (ADT_GETPROP(adt, node, "reg", ®) < 0) - continue; - if (ADT_GETPROP_ARRAY(adt, node, "cpu-impl-reg", cpu_impl_reg) < 0) { - u32 reg_len; - const u64 *regs = adt_getprop(adt, arm_io_node, "reg", ®_len); - if (!regs) - continue; - u32 index = 2 * i + 2; - if (reg_len < index) - continue; - memcpy(cpu_impl_reg, ®s[index], 16); - } - - u8 core = FIELD_GET(CPU_REG_CORE, reg); - u8 cluster = FIELD_GET(CPU_REG_CLUSTER, reg); - u8 die = FIELD_GET(CPU_REG_DIE, reg); - - smp_stop_cpu(i, die, cluster, core, cpu_impl_reg[0], pmgr_reg + cpu_start_off, deep_sleep); + smp_stop_cpu(i, cpu->die, cpu->cluster, cpu->core, cpu->impl_reg, cpu_start_base, + deep_sleep); } } diff --git a/src/smp.h b/src/smp.h index 45998c69e..a9137553b 100644 --- a/src/smp.h +++ b/src/smp.h @@ -16,6 +16,7 @@ extern u8 secondary_stacks_el3[MAX_EL3_CPUS][SECONDARY_STACK_SIZE]; void smp_secondary_entry(void); void smp_secondary_prep_el3(void); +int smp_init(void); void smp_start_secondaries(void); void smp_stop_secondaries(bool deep_sleep); From d30913b713a4c0e86935f8a9919ec099d495929b Mon Sep 17 00:00:00 2001 From: Sven Peter Date: Thu, 9 Jul 2026 18:24:22 +0200 Subject: [PATCH 8/8] smp: Identify stack by MPIDR Signed-off-by: Sven Peter --- src/smp.c | 39 +++++++++++++++++++++++++++++++++------ src/smp.h | 1 + src/smp_asm.h | 11 +++++++++++ src/start.S | 31 +++++++++++++++++++++++++++++++ 4 files changed, 76 insertions(+), 6 deletions(-) create mode 100644 src/smp_asm.h diff --git a/src/smp.c b/src/smp.c index 168dd55fa..41d982fd8 100644 --- a/src/smp.c +++ b/src/smp.c @@ -62,25 +62,52 @@ static bool smp_initialized = false; static u64 cpu_start_base; static struct cpu_info cpu_info[MAX_CPUS]; +// Used from start.S to find the correct stack after the first entry +struct smp_reset_stack { + u64 mpidr; + u64 stack; +}; + +struct smp_reset_stack smp_reset_stacks[MAX_CPUS] = { + [0 ... MAX_CPUS - 1] = {.mpidr = ~0ULL, .stack = 0}, +}; + extern u8 _vectors_start[0]; int boot_cpu_idx = -1; u64 boot_cpu_mpidr = 0; void smp_secondary_entry(void) { - struct spin_table *me = &spin_table[target_cpu]; + u64 mpidr = mrs(MPIDR_EL1) & 0xFFFFFF; + int index = target_cpu; + + // target_cpu identifies us during the initial start handshake, but may + // be stale on an RVBAR re-entry after deep wfi + for (int i = 0; i < MAX_CPUS; i++) { + if (smp_reset_stacks[i].mpidr == mpidr) { + index = i; + break; + } + } + + smp_reset_stacks[index].stack = (u64)secondary_stacks[index] + SECONDARY_STACK_SIZE; + smp_reset_stacks[index].mpidr = mpidr; + dc_civac_range(&smp_reset_stacks[index], sizeof(smp_reset_stacks[index])); + sysop("dsb sy"); + + struct spin_table *me = &spin_table[index]; if (in_el2()) - msr(TPIDR_EL2, target_cpu); + msr(TPIDR_EL2, index); else - msr(TPIDR_EL1, target_cpu); + msr(TPIDR_EL1, index); - printf(" Index: %d (table: %p)\n\n", target_cpu, me); + printf(" Index: %d (table: %p)\n\n", index, me); - me->mpidr = mrs(MPIDR_EL1) & 0xFFFFFF; + me->mpidr = mpidr; sysop("dmb sy"); - me->flag = 1; + me->flag++; sysop("dmb sy"); u64 target; if (!cpu_features->fast_ipi) diff --git a/src/smp.h b/src/smp.h index a9137553b..cacb7096b 100644 --- a/src/smp.h +++ b/src/smp.h @@ -3,6 +3,7 @@ #ifndef __SMP_H__ #define __SMP_H__ +#include "smp_asm.h" #include "types.h" #include "utils.h" diff --git a/src/smp_asm.h b/src/smp_asm.h new file mode 100644 index 000000000..8659945f1 --- /dev/null +++ b/src/smp_asm.h @@ -0,0 +1,11 @@ +/* SPDX-License-Identifier: MIT */ + +#ifndef __SMP_ASM_H__ +#define __SMP_ASM_H__ + +#define MAX_CPUS 24 +#define MAX_EL3_CPUS 4 + +#define SECONDARY_STACK_SIZE 0x10000 + +#endif diff --git a/src/start.S b/src/start.S index aa11e18ba..3f230204f 100644 --- a/src/start.S +++ b/src/start.S @@ -1,5 +1,6 @@ /* SPDX-License-Identifier: MIT */ +#include "smp_asm.h" #include "soc.h" #define UTRSTAT 0x010 @@ -13,6 +14,7 @@ .extern _v_sp0_serr .extern _reset_stack .extern _cpu_reset_c +.extern smp_reset_stacks .extern wdt_reboot .section .init, "ax" @@ -150,9 +152,38 @@ cpu_reset: mov w0, 'O' bl debug_putc + // for EL3 just keep the shared reset stack for now + mrs x8, CurrentEL + cmp x8, #0xc + beq 2f + + // check if there's already an entry in the stack table for our MPIDR + mrs x3, mpidr_el1 + and x3, x3, #0xffffff + adrp x4, smp_reset_stacks + add x4, x4, :lo12:smp_reset_stacks + ldr x5, =MAX_CPUS +1: + cbz x5, 2f // end if table, MPIDR not found -> use shared reset stack + sub x5, x5, #1 + + // x6 = MPIDR, x7 = stack + ldp x6, x7, [x4], #16 + + // check if the current entry's MPIDR matches ours + cmp x6, x3 + bne 1b + + // entry is valid + cbz x7, 2f // entry has no stack -> use shared reset stack + mov x1, x7 // entry has a valid stack -> use it + b 3f + +2: adrp x1, _reset_stack add x1, x1, :lo12:_reset_stack ldr x1, [x1] +3: mov sp, x1 ldr x2, [sp, #-8]