irq

Author: ZR233

platforms/axplat-aarch64-dyn/Cargo.toml

@@ -8,12 +8,16 @@ name = "axplat-aarch64-dyn"
 repository.workspace = true
 version = "0.1.0"
 
+[features]
+fp-simd = ["axcpu/fp-simd"]
+smp = ["axplat/smp"]
+
 [dependencies]
 aarch64-cpu = "10"
 any-uart = "0.2.11"
 axconfig-macros = "0.2"
 axcpu = {workspace = true}
-axplat = {workspace = true}
+axplat = {workspace = true, features = ["irq"]}
 fdt-parser = "0.4"
 heapless = "0.8"
 log = "0.4"
@@ -22,3 +26,4 @@ pie-boot = {version = "0.2.9"}
 rdrive = "0.15"
 smccc = "0.2"
 spin = "0.10"
+lazyinit = "0.2"

platforms/axplat-aarch64-dyn/src/driver.rs

@@ -13,8 +13,6 @@ pub fn setup() {
 
     init(Platform::Fdt { addr: fdt }).unwrap();
 
-    register_append(&driver_registers());
-
     probe_pre_kernel().unwrap();
 }
 

platforms/axplat-aarch64-dyn/src/init.rs

@@ -39,6 +39,7 @@ impl InitIf for InitIfImpl {
     /// Initializes the platform at the early stage for secondary cores.
     ///
     /// See [`init_early`] for details.
+    #[cfg(feature = "smp")]
     fn init_early_secondary(cpu_id: usize) {
         axcpu::init::init_trap();
     }
@@ -69,12 +70,15 @@ impl InitIf for InitIfImpl {
     /// * Other platform devices are initialized.
     fn init_later(cpu_id: usize, arg: usize) {
         driver::setup();
-
-        driver::probe_all(true).unwrap();
+        crate::irq::init();
+        crate::irq::init_current_cpu();
     }
 
     /// Initializes the platform at the later stage for secondary cores.
     ///
     /// See [`init_later`] for details.
-    fn init_later_secondary(cpu_id: usize) {}
+    #[cfg(feature = "smp")]
+    fn init_later_secondary(cpu_id: usize) {
+        crate::irq::init_current_cpu();
+    }
 }

platforms/axplat-aarch64-dyn/src/irq.rs

@@ -1,36 +1,117 @@
-use axplat::irq::{IrqHandler, IrqIf};
+use aarch64_cpu::registers::*;
+use axplat::irq::{HandlerTable, IrqHandler, IrqIf};
+use lazyinit::LazyInit;
+use log::{debug, trace, warn};
+use rdrive::{Device, driver::intc::*};
+
+const SPI_START: usize = 32;
+/// The maximum number of IRQs.
+const MAX_IRQ_COUNT: usize = 1024;
+
+fn is_irq_private(irq_num: usize) -> bool {
+    irq_num < SPI_START
+}
+
+// per-CPU, no lock
+static CPU_IF: LazyInit<local::Boxed> = LazyInit::new();
+static IRQ_HANDLER_TABLE: HandlerTable<MAX_IRQ_COUNT> = HandlerTable::new();
 
 struct IrqIfImpl;
 
 #[impl_plat_interface]
 impl IrqIf for IrqIfImpl {
     /// Enables or disables the given IRQ.
-    fn set_enable(irq: usize, enabled: bool) {
-        todo!()
+    fn set_enable(irq_raw: usize, enabled: bool) {
+        trace!("IRQ set enable: {:#x} {}", irq_raw, enabled);
+        let irq: IrqId = irq_raw.into();
+        if is_irq_private(irq_raw)
+            && let local::Capability::ConfigLocalIrq(c) = CPU_IF.capability()
+        {
+            if enabled {
+                c.irq_enable(irq).expect("failed to enable local IRQ");
+            } else {
+                c.irq_disable(irq).expect("failed to disable local IRQ");
+            }
+        } else {
+            let mut intc = get_gicd().lock().unwrap();
+            if enabled {
+                intc.irq_enable(irq).expect("failed to enable IRQ");
+            } else {
+                intc.irq_disable(irq).expect("failed to disable IRQ");
+            }
+
+            if !is_irq_private(irq_raw) {
+                // For private IRQs, we need to acknowledge the interrupt
+                // controller.
+                intc.set_target_cpu(irq, current_cpu().into());
+            }
+        }
     }
 
     /// Registers an IRQ handler for the given IRQ.
     ///
     /// It also enables the IRQ if the registration succeeds. It returns `false`
     /// if the registration failed.
-    fn register(irq: usize, handler: IrqHandler) -> bool {
-        todo!()
+    fn register(irq_num: usize, handler: IrqHandler) -> bool {
+        trace!("register handler IRQ {}", irq_num);
+        if IRQ_HANDLER_TABLE.register_handler(irq_num, handler) {
+            Self::set_enable(irq_num, true);
+            return true;
+        }
+        warn!("register handler for IRQ {} failed", irq_num);
+        false
     }
 
     /// Unregisters the IRQ handler for the given IRQ.
     ///
     /// It also disables the IRQ if the unregistration succeeds. It returns the
     /// existing handler if it is registered, `None` otherwise.
-    fn unregister(irq: usize) -> Option<IrqHandler> {
-        todo!()
+    fn unregister(irq_num: usize) -> Option<IrqHandler> {
+        trace!("unregister handler IRQ {}", irq_num);
+        Self::set_enable(irq_num, false);
+        IRQ_HANDLER_TABLE.unregister_handler(irq_num)
     }
 
     /// Handles the IRQ.
     ///
     /// It is called by the common interrupt handler. It should look up in the
     /// IRQ handler table and calls the corresponding handler. If necessary, it
     /// also acknowledges the interrupt controller after handling.
-    fn handle(irq: usize) {
-        todo!()
+    fn handle(_unused: usize) {
+        let Some(irq) = CPU_IF.ack() else {
+            return;
+        };
+        let irq_num: usize = irq.into();
+        trace!("IRQ {}", irq_num);
+        if !IRQ_HANDLER_TABLE.handle(irq_num as _) {
+            warn!("Unhandled IRQ {}", irq_num);
+        }
+
+        CPU_IF.eoi(irq);
+        if CPU_IF.get_eoi_mode() {
+            CPU_IF.dir(irq);
+        }
     }
 }
+
+pub(crate) fn init() {
+    let intc = get_gicd();
+    intc.lock().unwrap().open().unwrap();
+    debug!("GICD initialized");
+}
+
+pub(crate) fn init_current_cpu() {
+    let intc = rdrive::get_one::<Intc>().expect("no interrupt controller found");
+    let cpu_if = intc.lock().unwrap().cpu_local().unwrap();
+    cpu_if.set_eoi_mode(true);
+    CPU_IF.init_once(cpu_if);
+    debug!("GIC initialized for current CPU");
+}
+
+fn get_gicd() -> Device<Intc> {
+    rdrive::get_one().expect("no interrupt controller found")
+}
+
+fn current_cpu() -> usize {
+    MPIDR_EL1.get() as usize & 0xffffff
+}

platforms/axplat-aarch64-dyn/src/mem.rs

@@ -1,8 +1,8 @@
 use core::ops::Range;
 
-use axplat::mem::{MemIf, RawRange};
+use axplat::mem::{MemIf, PhysAddr, RawRange, VirtAddr};
 use heapless::Vec;
-use memory_addr::{MemoryAddr, PhysAddr, VirtAddr};
+use memory_addr::MemoryAddr;
 use pie_boot::{KIMAGE_VADDR, KLINER_OFFSET, MemoryRegionKind, boot_info};
 use spin::Once;
 

platforms/axplat-aarch64-dyn/src/power.rs

@@ -22,6 +22,7 @@ impl PowerIf for PowerImpl {
     ///
     /// Where `cpu_id` is the logical CPU ID (0, 1, ..., N-1, N is the number of
     /// CPU cores on the platform).
+    #[cfg(feature = "smp")]
     fn cpu_boot(cpu_id: usize, stack_top_paddr: usize) {
         todo!()
     }

platforms/axplat-aarch64-dyn/src/time.rs

@@ -27,14 +27,22 @@ impl TimeIf for TimeIfImpl {
     /// Return epoch offset in nanoseconds (wall time offset to monotonic
     /// clock start).
     fn epochoffset_nanos() -> u64 {
-        todo!()
+        0
     }
 
     /// Set a one-shot timer.
     ///
     /// A timer interrupt will be triggered at the specified monotonic time
     /// deadline (in nanoseconds).
     fn set_oneshot_timer(deadline_ns: u64) {
-        todo!()
+        let cnptct = CNTPCT_EL0.get();
+        let cnptct_deadline = Self::nanos_to_ticks(deadline_ns);
+        if cnptct < cnptct_deadline {
+            let interval = cnptct_deadline - cnptct;
+            debug_assert!(interval <= u32::MAX as u64);
+            CNTP_TVAL_EL0.set(interval);
+        } else {
+            CNTP_TVAL_EL0.set(0);
+        }
     }
 }