feat: send IPIs using GIC

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<u32, ITARGETSR::Register>; 248]),
         (0xC00 => _res2),
         (0xC08 => ICFGR: [ReadWrite<u32>; 62]),
-        (0xC0C => @END),
+        (0xD00 => _res3),
+        (0xF00 => SGIR: WriteOnly<u32>),
+        (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();
 

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) {

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) };

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();

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].

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");
+        }
+    }
 }

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<T> IrqSafeSpinLock<T> {
         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();
         }