@@ -15,8 +15,9 @@ use core::sync::atomic::{AtomicBool, AtomicU64, Ordering};
1515/// Maximum number of CPUs supported.
1616pub const MAX_CPUS : usize = 8 ;
1717
18- /// PSCI function IDs (SMCCC compliant, 64-bit ).
18+ /// PSCI function IDs (SMCCC compliant).
1919const PSCI_CPU_ON_64 : u64 = 0xC400_0003 ;
20+ const PSCI_CPU_ON_32 : u64 = 0x8400_0003 ;
2021
2122extern "C" {
2223 /// Physical address of secondary_cpu_entry, stored in .rodata by boot.S.
@@ -37,6 +38,11 @@ extern "C" {
3738 static SMP_TTBR1_PTR : u64 ;
3839 static SMP_MAIR_PTR : u64 ;
3940 static SMP_TCR_PTR : u64 ;
41+
42+ /// Pointer to SMP_STACK_BASE_PHYS (in .bss.boot). CPU 0 writes the
43+ /// physical base address of the per-CPU stack region here before PSCI CPU_ON.
44+ /// On QEMU/Parallels: 0x4100_0000; on VMware: 0x8100_0000.
45+ static SMP_STACK_BASE_PTR : u64 ;
4046}
4147
4248/// Write CPU 0's actual MMU configuration to .bss.boot variables so
@@ -122,6 +128,27 @@ pub fn set_uart_phys(addr: u64) {
122128 }
123129}
124130
131+ /// Set the physical base address of the per-CPU stack region.
132+ /// Must be called before `release_cpu()`.
133+ ///
134+ /// The stack base is `ram_base + 0x0100_0000` (16MB into RAM).
135+ /// On QEMU/Parallels (ram at 0x40000000): 0x4100_0000.
136+ /// On VMware (ram at 0x80000000): 0x8100_0000.
137+ pub fn set_stack_base_phys ( addr : u64 ) {
138+ unsafe {
139+ let phys = core:: ptr:: read_volatile ( & SMP_STACK_BASE_PTR ) ;
140+ let virt = phys + 0xFFFF_0000_0000_0000u64 ;
141+ let ptr = virt as * mut u64 ;
142+ core:: ptr:: write_volatile ( ptr, addr) ;
143+ core:: arch:: asm!(
144+ "dc cvac, {addr}" ,
145+ "dsb ish" ,
146+ addr = in( reg) ptr,
147+ options( nostack) ,
148+ ) ;
149+ }
150+ }
151+
125152/// Number of CPUs currently online (starts at 1 for the boot CPU).
126153static CPUS_ONLINE : AtomicU64 = AtomicU64 :: new ( 1 ) ;
127154
@@ -160,6 +187,38 @@ fn psci_cpu_on(target_cpu: u64, entry_point: u64, context_id: u64) -> i64 {
160187 ret
161188}
162189
190+ /// PSCI CPU_ON with 32-bit function ID via HVC.
191+ fn psci_cpu_on_32 ( target_cpu : u64 , entry_point : u64 , context_id : u64 ) -> i64 {
192+ let ret: i64 ;
193+ unsafe {
194+ core:: arch:: asm!(
195+ "hvc #0" ,
196+ inout( "x0" ) PSCI_CPU_ON_32 => ret,
197+ in( "x1" ) target_cpu,
198+ in( "x2" ) entry_point,
199+ in( "x3" ) context_id,
200+ options( nomem, nostack) ,
201+ ) ;
202+ }
203+ ret
204+ }
205+
206+ /// PSCI CPU_ON with 64-bit function ID via SMC (EL3 firmware conduit).
207+ fn psci_cpu_on_smc ( target_cpu : u64 , entry_point : u64 , context_id : u64 ) -> i64 {
208+ let ret: i64 ;
209+ unsafe {
210+ core:: arch:: asm!(
211+ "smc #0" ,
212+ inout( "x0" ) PSCI_CPU_ON_64 => ret,
213+ in( "x1" ) target_cpu,
214+ in( "x2" ) entry_point,
215+ in( "x3" ) context_id,
216+ options( nomem, nostack) ,
217+ ) ;
218+ }
219+ ret
220+ }
221+
163222/// Release a secondary CPU using PSCI CPU_ON.
164223///
165224/// The CPU will start executing at `secondary_cpu_entry` in boot.S,
@@ -182,12 +241,21 @@ pub fn release_cpu(cpu_id: usize) -> i64 {
182241 // Context ID: pass cpu_id so the new CPU knows who it is
183242 let context_id = cpu_id as u64 ;
184243
185- let ret = psci_cpu_on ( target_mpidr, entry_phys, context_id) ;
244+ // Try 64-bit PSCI CPU_ON via HVC first (standard for ARM64 hypervisors)
245+ let mut ret = psci_cpu_on ( target_mpidr, entry_phys, context_id) ;
246+
247+ // If 64-bit failed, try 32-bit function ID (some hypervisors only support this)
248+ if ret != 0 {
249+ crate :: serial_println!( "[smp] CPU {}: HVC64 failed (ret={}), trying HVC32..." , cpu_id, ret) ;
250+ ret = psci_cpu_on_32 ( target_mpidr, entry_phys, context_id) ;
251+ }
252+
253+ // Note: SMC conduit not attempted — on VMware (EL1 guest, no EL3),
254+ // SMC would trap to EL2 and likely fault. HVC is the correct conduit.
186255
187256 if ret != 0 {
188- // PSCI error — emit raw UART error indicator
189- raw_uart_char ( b'E' ) ;
190- raw_uart_char ( b'0' + cpu_id as u8 ) ;
257+ crate :: serial_println!( "[smp] PSCI CPU_ON failed for CPU {}: ret={} (MPIDR={:#x} entry={:#x})" ,
258+ cpu_id, ret, target_mpidr, entry_phys) ;
191259 }
192260
193261 ret
@@ -233,14 +301,13 @@ pub extern "C" fn secondary_cpu_entry_rust(cpu_id: u64) -> ! {
233301 crate :: per_cpu_aarch64:: init_cpu ( cpu_id as usize ) ;
234302
235303 // Set kernel stack top for this CPU.
236- // boot.S sets SP to 0x41000000 + (cpu_id+1)*0x200000 (physical),
304+ // boot.S sets SP to SMP_STACK_BASE_PHYS + (cpu_id+1)*0x200000 (physical),
237305 // then adds KERNEL_VIRT_BASE after enabling MMU.
238306 // This value is critical: when a user thread runs on this CPU and an
239307 // exception occurs, the kernel needs to switch to this stack.
240- const HHDM_BASE : u64 = 0xFFFF_0000_0000_0000 ;
241- const STACK_REGION_BASE : u64 = 0x4100_0000 ;
308+ let stack_base = super :: constants:: percpu_stack_region_base ( ) ;
242309 const STACK_SIZE : u64 = 0x20_0000 ; // 2MB per CPU
243- let kernel_stack_top = HHDM_BASE + STACK_REGION_BASE + ( cpu_id + 1 ) * STACK_SIZE ;
310+ let kernel_stack_top = stack_base + ( cpu_id + 1 ) * STACK_SIZE ;
244311 crate :: per_cpu_aarch64:: set_kernel_stack_top ( kernel_stack_top) ;
245312
246313 // Initialize GIC CPU interface (GICC registers are banked per-CPU)
@@ -281,12 +348,10 @@ fn create_and_register_idle_thread(cpu_id: usize) {
281348 use crate :: memory:: arch_stub:: VirtAddr ;
282349
283350 // Boot stack addresses — must match boot.S layout.
284- // HHDM_BASE + STACK_REGION_BASE + (cpu_id + 1) * STACK_SIZE
285- const HHDM_BASE : u64 = 0xFFFF_0000_0000_0000 ;
286- const STACK_REGION_BASE : u64 = 0x4100_0000 ;
351+ let stack_base = super :: constants:: percpu_stack_region_base ( ) ;
287352 const STACK_SIZE : u64 = 0x20_0000 ; // 2MB per CPU
288- let boot_stack_top = VirtAddr :: new ( HHDM_BASE + STACK_REGION_BASE + ( ( cpu_id as u64 ) + 1 ) * STACK_SIZE ) ;
289- let boot_stack_bottom = VirtAddr :: new ( HHDM_BASE + STACK_REGION_BASE + ( cpu_id as u64 ) * STACK_SIZE ) ;
353+ let boot_stack_top = VirtAddr :: new ( stack_base + ( ( cpu_id as u64 ) + 1 ) * STACK_SIZE ) ;
354+ let boot_stack_bottom = VirtAddr :: new ( stack_base + ( cpu_id as u64 ) * STACK_SIZE ) ;
290355 let dummy_tls = VirtAddr :: zero ( ) ;
291356
292357 let mut idle_task = Box :: new ( Thread :: new (
0 commit comments