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/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.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/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 = .; 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" { 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 a5bd4f1e6..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"); @@ -579,14 +587,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); @@ -594,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"); - printf("FDT: CPU %d MPIDR=0x%lx release-addr=0x%lx\n", cpu, mpidr, release_addr); + 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 uses PSCI\n", cpu, mpidr); + } next_cpu: cpu++; @@ -2746,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) { @@ -2774,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()) @@ -2847,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/main.c b/src/main.c index d4e828c54..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 @@ -206,6 +207,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]); 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); } 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 diff --git a/src/smp.c b/src/smp.c index 7db76b6b0..41d982fd8 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,16 +41,36 @@ 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; 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]; + +// 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; @@ -59,19 +78,36 @@ 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) @@ -135,9 +171,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 @@ -227,34 +261,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: @@ -300,7 +338,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) @@ -346,7 +384,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; @@ -372,62 +410,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]; - - if (!node) - continue; + struct cpu_info *cpu = &cpu_info[i]; - u32 reg; - u64 cpu_impl_reg[2]; - if (ADT_GETPROP(adt, node, "reg", ®) < 0) + if (!cpu->valid || i == boot_cpu_idx) 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 029697864..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" @@ -10,12 +11,13 @@ #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); +int smp_init(void); void smp_start_secondaries(void); void smp_stop_secondaries(bool deep_sleep); 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]