Merge pull request #1705 from stlankes/smp

enable SMP support for aarch64

Author: mkroening

.github/workflows/ci.yml

@@ -196,8 +196,6 @@ jobs:
         if: matrix.arch == 'x86_64'
       - run: cargo xtask ci rs --arch ${{ matrix.arch }} --profile ${{ matrix.profile }} --package rusty_demo qemu ${{ matrix.flags }}
       - run: cargo xtask ci rs --arch ${{ matrix.arch }} --profile ${{ matrix.profile }} --package rusty_demo --smp 4 qemu ${{ matrix.flags }}
-        # https://github.com/hermit-os/kernel/issues/737
-        if: matrix.arch != 'aarch64'
         # https://github.com/hermit-os/kernel/issues/1286
         continue-on-error: ${{ matrix.arch == 'riscv64' }}
       - run: cargo xtask ci rs --arch ${{ matrix.arch }} --profile ${{ matrix.profile }} --package rusty_demo --smp 4 qemu ${{ matrix.flags }} --uefi

Cargo.lock

@@ -170,7 +170,7 @@ memory_addresses = { version = "0.2.2", default-features = false, features = [
 
 [target.'cfg(target_arch = "aarch64")'.dependencies]
 aarch64 = { version = "0.0.14", default-features = false }
-arm-gic = { version = "0.3" }
+arm-gic = { version = "0.4" }
 hermit-dtb = { version = "0.1" }
 semihosting = { version = "0.1", optional = true }
 memory_addresses = { version = "0.2.2", default-features = false, features = [

Cargo.toml

@@ -6,7 +6,7 @@ use core::sync::atomic::{AtomicU64, Ordering};
 
 use aarch64::regs::*;
 use ahash::RandomState;
-use arm_gic::gicv3::GicV3;
+use arm_gic::gicv3::{GicV3, SgiTarget};
 use arm_gic::{IntId, Trigger};
 use hashbrown::HashMap;
 use hermit_dtb::Dtb;
@@ -55,10 +55,7 @@ pub fn enable() {
 	}
 }
 
-/// Enable Interrupts and wait for the next interrupt (HLT instruction)
-/// According to <https://lists.freebsd.org/pipermail/freebsd-current/2004-June/029369.html>, this exact sequence of assembly
-/// instructions is guaranteed to be atomic.
-/// This is important, because another CPU could call wakeup_core right when we decide to wait for the next interrupt.
+/// Enable all interrupts and wait for the next interrupt (wfi instruction)
 #[inline]
 pub fn enable_and_wait() {
 	unsafe {
@@ -94,13 +91,8 @@ pub(crate) fn install_handlers() {
 		debug!("Handle timer interrupt");
 
 		// disable timer
-		unsafe {
-			asm!(
-				"msr cntp_cval_el0, xzr",
-				"msr cntp_ctl_el0, xzr",
-				options(nostack, nomem),
-			);
-		}
+		CNTP_CVAL_EL0.set(0);
+		CNTP_CTL_EL0.write(CNTP_CTL_EL0::ENABLE::CLEAR);
 	}
 
 	for (key, value) in get_interrupt_handlers().into_iter() {
@@ -237,8 +229,19 @@ pub(crate) extern "C" fn do_error(_state: &State) -> ! {
 	scheduler::abort()
 }
 
-pub fn wakeup_core(_core_to_wakeup: CoreId) {
-	todo!("wakeup_core stub");
+pub fn wakeup_core(core_id: CoreId) {
+	debug!("Wakeup core {core_id}");
+	let reschedid = IntId::sgi(SGI_RESCHED.into());
+
+	GicV3::send_sgi(
+		reschedid,
+		SgiTarget::List {
+			affinity3: 0,
+			affinity2: 0,
+			affinity1: 0,
+			target_list: 1 << core_id,
+		},
+	);
 }
 
 pub(crate) fn init() {
@@ -261,21 +264,31 @@ pub(crate) fn init() {
 	let (slice, _residual_slice) = residual_slice.split_at(core::mem::size_of::<u64>());
 	let gicr_size = u64::from_be_bytes(slice.try_into().unwrap());
 
-	let gicr_stride = dtb
-		.get_property("/intc", "redistributor-stride")
-		.map(|bytes| u64::from_be_bytes(bytes.try_into().unwrap()))
-		.unwrap_or(0);
-
 	let num_cpus = dtb
 		.enum_subnodes("/cpus")
 		.filter(|name| name.contains("cpu@"))
 		.count();
-	let cpu_id = core_id();
+	let cpu_id: usize = core_id().try_into().unwrap();
+
+	let compatible = core::str::from_utf8(
+		dtb.get_property("/intc", "compatible")
+			.unwrap_or(b"unknown"),
+	)
+	.unwrap()
+	.replace('\0', "");
+	let is_gic_v4 = if compatible == "arm,gic-v4" {
+		info!("Found GIC v4 with {num_cpus} cpus");
+		true
+	} else if compatible == "arm,gic-v3" {
+		info!("Found GIC v3 with {num_cpus} cpus");
+		false
+	} else {
+		panic!("{compatible} isn't supported")
+	};
 
-	info!("Found {num_cpus} cpus!");
 	info!("Found GIC Distributor interface at {gicd_start:p} (size {gicd_size:#X})");
 	info!(
-		"Found generic interrupt controller redistributor at {gicr_start:p} (size {gicr_size:#X}, stride {gicr_stride:#X})"
+		"Found generic interrupt controller redistributor at {gicr_start:p} (size {gicr_size:#X})"
 	);
 
 	let gicd_address =
@@ -301,16 +314,16 @@ pub(crate) fn init() {
 		flags,
 	);
 
-	GicV3::set_priority_mask(0xff);
 	let mut gic = unsafe {
 		GicV3::new(
 			gicd_address.as_mut_ptr(),
 			gicr_address.as_mut_ptr(),
 			num_cpus,
-			gicr_stride as usize,
+			is_gic_v4,
 		)
 	};
-	gic.setup(cpu_id as usize);
+	gic.setup(cpu_id);
+	GicV3::set_priority_mask(0xff);
 
 	for node in dtb.enum_subnodes("/") {
 		let parts: Vec<_> = node.split('@').collect();
@@ -349,27 +362,91 @@ pub(crate) fn init() {
 				} else {
 					panic!("Invalid interrupt type");
 				};
-				gic.set_interrupt_priority(timer_irqid, Some(cpu_id as usize), 0x00);
+				gic.set_interrupt_priority(timer_irqid, Some(cpu_id), 0x00);
 				if (irqflags & 0xf) == 4 || (irqflags & 0xf) == 8 {
-					gic.set_trigger(timer_irqid, Some(cpu_id as usize), Trigger::Level);
+					gic.set_trigger(timer_irqid, Some(cpu_id), Trigger::Level);
 				} else if (irqflags & 0xf) == 2 || (irqflags & 0xf) == 1 {
-					gic.set_trigger(timer_irqid, Some(cpu_id as usize), Trigger::Edge);
+					gic.set_trigger(timer_irqid, Some(cpu_id), Trigger::Edge);
 				} else {
 					panic!("Invalid interrupt level!");
 				}
-				gic.enable_interrupt(timer_irqid, Some(cpu_id as usize), true);
+				gic.enable_interrupt(timer_irqid, Some(cpu_id), true);
 			}
 		}
 	}
 
 	let reschedid = IntId::sgi(SGI_RESCHED.into());
-	gic.set_interrupt_priority(reschedid, Some(cpu_id as usize), 0x00);
-	gic.enable_interrupt(reschedid, Some(cpu_id as usize), true);
+	gic.set_interrupt_priority(reschedid, Some(cpu_id), 0x01);
+	gic.enable_interrupt(reschedid, Some(cpu_id), true);
 	IRQ_NAMES.lock().insert(SGI_RESCHED, "Reschedule");
 
 	*GIC.lock() = Some(gic);
 }
 
+// marks the given CPU core as awake
+pub fn init_cpu() {
+	let cpu_id: usize = core_id().try_into().unwrap();
+
+	if let Some(ref mut gic) = *GIC.lock() {
+		debug!("Mark cpu {cpu_id} as awake");
+
+		gic.setup(cpu_id);
+		GicV3::set_priority_mask(0xff);
+
+		let dtb = unsafe {
+			Dtb::from_raw(ptr::with_exposed_provenance(
+				env::boot_info().hardware_info.device_tree.unwrap().get() as usize,
+			))
+			.expect(".dtb file has invalid header")
+		};
+
+		for node in dtb.enum_subnodes("/") {
+			let parts: Vec<_> = node.split('@').collect();
+
+			if let Some(compatible) = dtb.get_property(parts.first().unwrap(), "compatible") {
+				if core::str::from_utf8(compatible).unwrap().contains("timer") {
+					let irq_slice = dtb
+						.get_property(parts.first().unwrap(), "interrupts")
+						.unwrap();
+					/* Secure Phys IRQ */
+					let (_irqtype, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+					let (_irq, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+					let (_irqflags, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+					/* Non-secure Phys IRQ */
+					let (irqtype, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+					let (irq, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+					let (irqflags, _irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+					let irqtype = u32::from_be_bytes(irqtype.try_into().unwrap());
+					let irq = u32::from_be_bytes(irq.try_into().unwrap());
+					let irqflags = u32::from_be_bytes(irqflags.try_into().unwrap());
+
+					// enable timer interrupt
+					let timer_irqid = if irqtype == 1 {
+						IntId::ppi(irq)
+					} else if irqtype == 0 {
+						IntId::spi(irq)
+					} else {
+						panic!("Invalid interrupt type");
+					};
+					gic.set_interrupt_priority(timer_irqid, Some(cpu_id), 0x00);
+					if (irqflags & 0xf) == 4 || (irqflags & 0xf) == 8 {
+						gic.set_trigger(timer_irqid, Some(cpu_id), Trigger::Level);
+					} else if (irqflags & 0xf) == 2 || (irqflags & 0xf) == 1 {
+						gic.set_trigger(timer_irqid, Some(cpu_id), Trigger::Edge);
+					} else {
+						panic!("Invalid interrupt level!");
+					}
+					gic.enable_interrupt(timer_irqid, Some(cpu_id), true);
+				}
+			}
+		}
+
+		let reschedid = IntId::sgi(SGI_RESCHED.into());
+		gic.set_interrupt_priority(reschedid, Some(cpu_id), 0x01);
+		gic.enable_interrupt(reschedid, Some(cpu_id), true);
+	}
+}
+
 static IRQ_NAMES: InterruptTicketMutex<HashMap<u8, &'static str, RandomState>> =
 	InterruptTicketMutex::new(HashMap::with_hasher(RandomState::with_seeds(0, 0, 0, 0)));
 

src/arch/aarch64/kernel/interrupts.rs

@@ -163,11 +163,12 @@ pub fn boot_processor_init() {
 #[allow(dead_code)]
 pub fn application_processor_init() {
 	CoreLocal::install();
+	interrupts::init_cpu();
 	finish_processor_init();
 }
 
 fn finish_processor_init() {
-	debug!("Initialized Processor");
+	debug!("Initialized processor {}", core_id());
 
 	// Allocate stack for the CPU and pass the addresses.
 	let layout = Layout::from_size_align(KERNEL_STACK_SIZE, BasePageSize::SIZE as usize).unwrap();
@@ -177,7 +178,82 @@ fn finish_processor_init() {
 }
 
 pub fn boot_next_processor() {
-	CPU_ONLINE.0.fetch_add(1, Ordering::Release);
+	// This triggers to wake up the next processor (bare-metal/QEMU) or uhyve
+	// to initialize the next processor.
+	#[allow(unused_variables)]
+	let cpu_online = CPU_ONLINE.0.fetch_add(1, Ordering::Release);
+
+	#[cfg(all(target_os = "none", feature = "smp"))]
+	if !env::is_uhyve() && get_possible_cpus() > 1 {
+		use core::arch::asm;
+		use core::hint::spin_loop;
+
+		use hermit_dtb::Dtb;
+
+		use crate::kernel::start::{TTBR0, smp_start};
+		use crate::mm::virtual_to_physical;
+
+		if cpu_online == 0 {
+			let virt_start = VirtAddr::from(smp_start as usize);
+			let phys_start = virtual_to_physical(virt_start).unwrap();
+			assert!(virt_start.as_u64() == phys_start.as_u64());
+
+			trace!("Virtual address of smp_start 0x{virt_start:x}");
+			trace!("Physical address of smp_start 0x{phys_start:x}");
+
+			let dtb = unsafe {
+				Dtb::from_raw(core::ptr::with_exposed_provenance(
+					env::boot_info().hardware_info.device_tree.unwrap().get() as usize,
+				))
+				.expect(".dtb file has invalid header")
+			};
+
+			let cpu_on = u32::from_be_bytes(
+				dtb.get_property("/psci", "cpu_on")
+					.unwrap()
+					.try_into()
+					.unwrap(),
+			);
+			trace!("CPU_ON: 0x{cpu_on:x}");
+			let method =
+				core::str::from_utf8(dtb.get_property("/psci", "method").unwrap_or(b"unknown"))
+					.unwrap()
+					.replace('\0', "");
+
+			let ttbr0: *mut u8;
+			unsafe {
+				asm!(
+					"mrs {}, ttbr0_el1",
+					out(reg) ttbr0,
+				);
+			}
+			TTBR0.store(ttbr0, Ordering::Relaxed);
+
+			for cpu_id in 1..get_possible_cpus() {
+				debug!("Try to wake-up core {cpu_id}");
+
+				if method == "hvc" {
+					// call hypervisor to wakeup next core
+					unsafe {
+						asm!("hvc #0", in("x0") cpu_on, in("x1") cpu_id, in("x2") phys_start.as_u64(), in("x3") cpu_id, options(nomem, nostack));
+					}
+				} else if method == "smc" {
+					// call secure monitor to wakeup next core
+					unsafe {
+						asm!("smc #0", in("x0") cpu_on, in("x1") cpu_id, in("x2") phys_start.as_u64(), in("x3") cpu_id, options(nomem, nostack));
+					}
+				} else {
+					warn!("Method {method} isn't supported!");
+					return;
+				}
+
+				// wait for next core
+				while CPU_ONLINE.0.load(Ordering::Relaxed) < cpu_id + 1 {
+					spin_loop();
+				}
+			}
+		}
+	}
 }
 
 pub fn print_statistics() {

src/arch/aarch64/kernel/mod.rs

@@ -98,10 +98,6 @@ pub fn seed_entropy() -> Option<[u8; 32]> {
 	None
 }
 
-pub(crate) fn run_on_hypervisor() -> bool {
-	true
-}
-
 /// The halt function stops the processor until the next interrupt arrives
 pub fn halt() {
 	unsafe {
@@ -246,8 +242,5 @@ pub fn print_information() {
 	infoheader!(" CPU INFORMATION ");
 	infoentry!("Processor compatibility", str::from_utf8(reg).unwrap());
 	infoentry!("Counter frequency", *CPU_FREQUENCY);
-	if run_on_hypervisor() {
-		info!("Run on hypervisor");
-	}
 	infofooter!();
 }