diff --git a/kernel/src/arch/aarch64/irq/gic/gicd.rs b/kernel/src/arch/aarch64/irq/gic/gicd.rs index af34da0..fad98c2 100644 --- a/kernel/src/arch/aarch64/irq/gic/gicd.rs +++ b/kernel/src/arch/aarch64/irq/gic/gicd.rs @@ -1,7 +1,7 @@ use crate::mem::virt::DeviceMemoryIo; use crate::sync::IrqSafeSpinLock; use tock_registers::interfaces::{Readable, Writeable}; -use tock_registers::registers::{ReadOnly, ReadWrite}; +use tock_registers::registers::{ReadOnly, WriteOnly, ReadWrite}; use tock_registers::{register_bitfields, register_structs}; register_bitfields! { @@ -31,7 +31,9 @@ register_structs! { (0x820 => ITARGETSR: [ReadWrite; 248]), (0xC00 => _res2), (0xC08 => ICFGR: [ReadWrite; 62]), - (0xC0C => @END), + (0xD00 => _res3), + (0xF00 => SGIR: WriteOnly), + (0xF04 => @END), } } @@ -103,6 +105,14 @@ impl Gicd { } } + pub fn set_sgir(&self, filter: bool, mask: u32, intid: u32) { + let mut value = (mask << 16) | intid; + if filter { + value |= 1 << 24; + } + self.shared_regs.lock().SGIR.set(value); + } + pub fn enable_irq(&self, irq: super::IrqNumber) { let irq = irq.get(); diff --git a/kernel/src/arch/aarch64/irq/gic/mod.rs b/kernel/src/arch/aarch64/irq/gic/mod.rs index 99533dd..d3a056e 100644 --- a/kernel/src/arch/aarch64/irq/gic/mod.rs +++ b/kernel/src/arch/aarch64/irq/gic/mod.rs @@ -1,7 +1,7 @@ //! ARM Generic Interrupt Controller use crate::dev::{ - irq::{IntController, IntSource, IrqContext}, + irq::{IntController, IntSource, IrqContext, IpiSender}, Device, }; use crate::mem::virt::{DeviceMemory, DeviceMemoryIo}; @@ -17,6 +17,8 @@ use gicd::Gicd; /// Maximum available IRQ number pub const MAX_IRQ: usize = 300; +const SGI_IRQ: u32 = 2; + /// Range-checked IRQ number type #[repr(transparent)] #[derive(Copy, Clone)] @@ -87,6 +89,12 @@ impl IntController for Gic { return; } + if irq_number == 1 { + gicc.clear_irq(irq_number as u32, ic); + debugln!("Received IPI"); + loop {} + } + if self.scheduler_irq.0 == irq_number { use crate::proc::sched; use cortex_a::registers::{CNTP_TVAL_EL0, CNTP_CTL_EL0}; @@ -132,6 +140,12 @@ impl IntController for Gic { } } +impl IpiSender for Gic { + fn send_to_mask(&self, exclude_self: bool, target: u32, data: u64) { + self.gicd.get().set_sgir(exclude_self, target, 1); + } +} + impl Gic { /// pub unsafe fn enable_secondary(&self) { diff --git a/kernel/src/arch/aarch64/mach_qemu/mod.rs b/kernel/src/arch/aarch64/mach_qemu/mod.rs index ee68d06..a359e32 100644 --- a/kernel/src/arch/aarch64/mach_qemu/mod.rs +++ b/kernel/src/arch/aarch64/mach_qemu/mod.rs @@ -76,6 +76,12 @@ pub fn intc() -> &'static Gic { &GIC } +/// Returns CPU's IPI sender device +#[inline] +pub fn ipi_sender() -> &'static Gic { + &GIC +} + static UART0: Pl011 = unsafe { Pl011::new(UART0_BASE, UART0_IRQ) }; static RTC: Pl031 = unsafe { Pl031::new(RTC_BASE, RTC_IRQ) }; static GIC: Gic = unsafe { Gic::new(GICD_BASE, GICC_BASE, LOCAL_TIMER_IRQ) }; diff --git a/kernel/src/arch/aarch64/smp.rs b/kernel/src/arch/aarch64/smp.rs index 12d4cd0..b7b6bda 100644 --- a/kernel/src/arch/aarch64/smp.rs +++ b/kernel/src/arch/aarch64/smp.rs @@ -1,7 +1,10 @@ #![allow(missing_docs)] -use crate::dev::fdt::{self, DeviceTree}; -use crate::arch::aarch64::cpu; +use crate::arch::{aarch64::cpu, machine}; +use crate::dev::{ + fdt::{self, DeviceTree}, + irq::IpiSender, +}; use crate::mem::{ self, phys::{self, PageUsage}, @@ -11,6 +14,8 @@ use error::Errno; use fdt_rs::prelude::*; use tock_registers::interfaces::Readable; +pub type NodeAddress = u32; + #[derive(Clone, Copy, Debug)] pub enum PsciError { NotSupported, @@ -24,29 +29,6 @@ pub enum PsciError { InvalidAddress, } -const SECONDARY_STACK_PAGES: usize = 4; - -unsafe fn call_smc(mut x0: usize, x1: usize, x2: usize, x3: usize) -> usize { - asm!("smc #0", inout("x0") x0, in("x1") x1, in("x2") x2, in("x3") x3); - x0 -} - -fn wrap_psci_ok(a: usize) -> Result<(), PsciError> { - const NOT_SUPPORTED: isize = -1; - const INVALID_PARAMETERS: isize = -2; - const DENIED: isize = -3; - const ALREADY_ON: isize = -4; - - match a as isize { - 0 => Ok(()), - NOT_SUPPORTED => Err(PsciError::NotSupported), - INVALID_PARAMETERS => Err(PsciError::InvalidParameters), - DENIED => Err(PsciError::Denied), - ALREADY_ON => Err(PsciError::AlreadyOn), - _ => unimplemented!(), - } -} - struct Psci { use_smc: bool, } @@ -83,6 +65,33 @@ impl Psci { } } +const SECONDARY_STACK_PAGES: usize = 4; + +unsafe fn call_smc(mut x0: usize, x1: usize, x2: usize, x3: usize) -> usize { + asm!("smc #0", inout("x0") x0, in("x1") x1, in("x2") x2, in("x3") x3); + x0 +} + +pub unsafe fn send_ipi(exclude_self: bool, target_mask: u32, data: u64) { + machine::ipi_sender().send_to_mask(exclude_self, target_mask, data); +} + +fn wrap_psci_ok(a: usize) -> Result<(), PsciError> { + const NOT_SUPPORTED: isize = -1; + const INVALID_PARAMETERS: isize = -2; + const DENIED: isize = -3; + const ALREADY_ON: isize = -4; + + match a as isize { + 0 => Ok(()), + NOT_SUPPORTED => Err(PsciError::NotSupported), + INVALID_PARAMETERS => Err(PsciError::InvalidParameters), + DENIED => Err(PsciError::Denied), + ALREADY_ON => Err(PsciError::AlreadyOn), + _ => unimplemented!(), + } +} + pub unsafe fn enable_secondary_cpus(dt: &DeviceTree) { extern "C" { fn _entry_secondary(); diff --git a/kernel/src/dev/irq.rs b/kernel/src/dev/irq.rs index c05e3ab..373c49f 100644 --- a/kernel/src/dev/irq.rs +++ b/kernel/src/dev/irq.rs @@ -1,4 +1,5 @@ //! Interrupt controller and handler interfaces +use crate::arch::platform::smp::NodeAddress; use crate::dev::Device; use core::marker::PhantomData; use error::Errno; @@ -27,6 +28,12 @@ pub trait IntController: Device { fn handle_pending_irqs<'irq_context>(&'irq_context self, ic: &IrqContext<'irq_context>); } +/// Inter-processor interrupt delivery method +pub trait IpiSender: Device { + /// Raise an IPI for the target CPU mask, optionally excluding source CPU + fn send_to_mask(&self, except_self: bool, target: u32, data: u64); +} + /// Interface for peripherals capable of emitting IRQs pub trait IntSource: Device { /// Handles pending IRQs, if any, of this [IntSource]. diff --git a/kernel/src/main.rs b/kernel/src/main.rs index 56e116b..a9afcc1 100644 --- a/kernel/src/main.rs +++ b/kernel/src/main.rs @@ -13,7 +13,7 @@ alloc_error_handler, linked_list_cursors, const_btree_new, - maybe_uninit_uninit_array, + maybe_uninit_uninit_array )] #![no_std] #![no_main] @@ -43,9 +43,19 @@ pub mod util; fn panic_handler(pi: &core::panic::PanicInfo) -> ! { unsafe { asm!("msr daifset, #2"); + use crate::arch::platform::cpu::{self, Cpu}; + + crate::arch::platform::smp::send_ipi(true, (1 << cpu::count()) - 1, 0); } - errorln!("Panic: {:?}", pi); + use cortex_a::registers::MPIDR_EL1; + use tock_registers::interfaces::Readable; + + errorln!("Panic on node{}: {:?}", MPIDR_EL1.get() & 0xF, pi); // TODO - loop {} + loop { + unsafe { + asm!("wfe"); + } + } } diff --git a/kernel/src/sync.rs b/kernel/src/sync.rs index 4fdefd2..8fa9f97 100644 --- a/kernel/src/sync.rs +++ b/kernel/src/sync.rs @@ -1,7 +1,5 @@ //! Synchronization facilities module -#![allow(missing_docs)] - use crate::arch::platform::{irq_mask_save, irq_restore}; use core::cell::UnsafeCell; use core::fmt; @@ -39,15 +37,15 @@ impl IrqSafeSpinLock { let irq_state = unsafe { irq_mask_save() }; let id = MPIDR_EL1.get() & 0xF; - while let Err(e) = self.state.compare_exchange_weak( - usize::MAX, - id as usize, - Ordering::Acquire, - Ordering::Relaxed, - ) { - if e == id as usize { - break; - } + while let Err(e) = self.state.compare_exchange_weak( + usize::MAX, + id as usize, + Ordering::Acquire, + Ordering::Relaxed, + ) { + // if e == id as usize { + // break; + // } cortex_a::asm::wfe(); }