bugfix:gicr bug

Author: buhenxihuan

Cargo.toml

@@ -14,3 +14,7 @@ tock-registers = "0.8"
 bitflags = "2.0"
 cfg-if = "1.0"
 aarch64-cpu = "9.3"
+log = "0.4"
+
+[features]
+el2 = []

src/gic_v2.rs

@@ -232,6 +232,7 @@ impl GenericArmGic for GicV2 {
 
     /// Enables the interrupt with the given ID.
     fn enable_interrupt(&mut self, intid: IntId) {
+        // log::trace!("enable private interrupt {} ", intid.0);
         let index = intid.0 / 32;
         let bit = 1 << (intid.0 % 32);
         self.gicd.regs().ISENABLER[index].set(bit);

src/gic_v3.rs

@@ -3,6 +3,7 @@
 //! The official documentation: <https://developer.arm.com/documentation/ihi0048/latest/>
 
 use aarch64_cpu::registers::MPIDR_EL1;
+use log::{debug, info,trace};
 use core::hint::spin_loop;
 use core::ptr::NonNull;
 use tock_registers::interfaces::{Readable, Writeable};
@@ -250,13 +251,10 @@ impl GicDistributor {
         for i in (IntId::SPI_START..self.support_irqs).step_by(32) {
             self.regs().ICACTIVER[i / 32].set(u32::MAX);
             self.regs().ICENABLER[i / 32].set(u32::MAX);
+            self.regs().ICPENDR[i / 32].set(u32::MAX);
         }
 
-        // Enable affinity routing and group1
-        self.regs()
-            .CTLR
-            .set((GicdCtlr::ARE_S | GicdCtlr::EnableGrp1NS).bits());
-        self.wait_rwp();
+
 
         // Set all global interrupts to current cpu.
         let mpidr: u64 = MPIDR_EL1.get() & 0xff00ffffff;
@@ -270,6 +268,10 @@ impl GicDistributor {
             // Set external interrupts to target cpu 0
             self.regs().IROUTERnE[i].set(Self::mpidr_to_affinity_level(mpidr));
         }
+
+        // Enable affinity routing and group1
+        self.regs().CTLR.set((GicdCtlr::ARE_S | GicdCtlr::EnableGrp1NS).bits());
+        self.wait_rwp();
     }
 
     /// Configures the trigger type for the interrupt with the given ID.
@@ -289,6 +291,31 @@ impl GicDistributor {
     }
 }
 
+
+impl core::ops::Index<usize> for GicRedistributor {
+    type Output = GicRedistributor;
+
+    fn index(&self, index: usize) -> &Self::Output {
+        if index >= 9 {
+            panic!("GicRedistributor index out of range");
+        }
+        // SAFRTY: GicRedistributor has constructed with correct address,and we have checked the index
+        unsafe { &*(self as *const GicRedistributor).add(index) }
+    }
+}
+
+use core::arch::asm;
+fn get_mpidr() -> u64 {
+    let mpidr: u64;
+    unsafe {
+        asm!(
+            "mrs {mpidr}, MPIDR_EL1", // 从 MPIDR_EL1 读取
+            mpidr = out(reg) mpidr
+        );
+    }
+    mpidr
+}
+
 impl GicRedistributor {
     /// Construct a new GIC Redistributor instance from the base address.
     pub const fn new(base: *mut u8) -> Self {
@@ -300,6 +327,8 @@ impl GicRedistributor {
 
     const fn gicr_regs(&self) -> &GicRedistributorRegs {
         unsafe { self.gicr_base.as_ref() }
+        // let current_cpu_id = (get_mpidr() >> 8) as usize;
+        // unsafe {self[current_cpu_id]}
     }
 
     const fn sgi_regs(&self) -> &GicSgiRegs {
@@ -312,21 +341,42 @@ impl GicRedistributor {
         }
     }
 
+    fn get_current_cpu_id(&self) -> usize {
+        ((get_mpidr() & 0xfff) >> 8) as usize
+    }
+
+    fn get_gicr_regs(&self) -> &GicRedistributorRegs {
+        let mpidr = get_mpidr();
+        // unsafe {trace!("this is my cpu_id {:x} gicr {:?} mpidr {:x}",self.get_current_cpu_id(),self.gicr_base.as_ptr().byte_add(self.get_current_cpu_id() * 0x2_0000 ), mpidr);}
+        unsafe { NonNull::new(self.gicr_base.as_ptr().byte_add(self.get_current_cpu_id() * 0x2_0000 ) ).unwrap().cast().as_ref()}
+    }
+
+    fn get_sgi_regs(&self) -> &GicSgiRegs {
+        // SAFETY: Writing to this system register doesn't access memory in any way
+        unsafe {
+            let gicr_addr = self.gicr_base.as_ptr().byte_add(self.get_current_cpu_id() * 0x2_0000 );
+            let sgi_base: NonNull<GicSgiRegs> =
+                NonNull::new(gicr_addr.byte_add(SGI_OFFSET)).unwrap().cast();
+                sgi_base.as_ref()
+
+    }
+}
+
     fn redis_enable(&self) {
-        let mut waker = self.gicr_regs().WAKER.get();
+        let mut waker = self.get_gicr_regs().WAKER.get();
         // Wake up this CPU redistributor
         waker &= !(WakerFlags::PROCESSOR_SLEEP.bits());
-        self.gicr_regs().WAKER.set(waker);
+        self.get_gicr_regs().WAKER.set(waker);
 
-        while WakerFlags::from_bits_truncate(self.gicr_regs().WAKER.get())
+        while WakerFlags::from_bits_truncate(self.get_gicr_regs().WAKER.get())
             .contains(WakerFlags::CHILDREN_ASLEEP)
         {
             spin_loop();
         }
     }
 
     fn base_init(&mut self) {
-        let typer = self.gicr_regs().TYPER.get() as usize;
+        let typer = self.get_gicr_regs().TYPER.get() as usize;
         let mut ppinum = typer >> 27 & 0x1f;
         ppinum = match ppinum {
             0 => 16,
@@ -338,23 +388,24 @@ impl GicRedistributor {
     }
 
     fn init(&mut self) {
+        // debug!("this is my gicr {:?}",self.gicr_base.as_ptr());
         self.base_init();
         self.redis_enable();
         // Configure SGIs/PPIs as non-secure Group-1
         for i in (0..self.support_ppi + 16).step_by(32) {
-            self.sgi_regs().IGROUPR0[i / 32].set(u32::MAX);
+            self.get_sgi_regs().IGROUPR0[i / 32].set(u32::MAX);
         }
 
         // Deactivate and disable all private interrupts
         for i in (0..self.support_ppi + 16).step_by(32) {
-            self.sgi_regs().ICACTIVER[i / 32].set(u32::MAX);
-            self.sgi_regs().ICENABLER[i / 32].set(u32::MAX);
+            self.get_sgi_regs().ICACTIVER[i / 32].set(u32::MAX);
+            self.get_sgi_regs().ICENABLER[i / 32].set(u32::MAX);
         }
 
         // Set priority on private interrupts
         for i in (0..self.support_ppi + 16).step_by(4) {
             // once time set 4 interrupts
-            self.sgi_regs().IPRIORITYR[i / 4].set(0xa0_a0_a0_a0);
+            self.get_sgi_regs().IPRIORITYR[i / 4].set(0xa0_a0_a0_a0);
         }
     }
 
@@ -365,13 +416,13 @@ impl GicRedistributor {
         let index = id >> 4;
         let bit_shift = ((id & 0xf) << 1) + 1;
 
-        let mut reg_val = self.sgi_regs().ICFGR[index].get();
+        let mut reg_val = self.get_sgi_regs().ICFGR[index].get();
         match tm {
             TriggerMode::Edge => reg_val |= 1 << bit_shift,
             TriggerMode::Level => reg_val &= !(1 << bit_shift),
         }
 
-        self.sgi_regs().ICFGR[index].set(reg_val);
+        self.get_sgi_regs().ICFGR[index].set(reg_val);
     }
 }
 
@@ -403,25 +454,31 @@ impl GicV3 {
         // SAFETY: Writing to this system register doesn't access memory in any way.
         unsafe {
             // Enable system register access.
-            write_sysreg!(icc_sre_el1, 0x01);
+            write_sysreg!(icc_sre_el2, 0x01);
         }
 
         unsafe {
             // Enable system register access.
             write_sysreg!(icc_pmr_el1, 0xf0);
         }
-
-        // SAFETY: Writing to this system register doesn't access memory in any way.
         unsafe {
-            // Disable use of `ICC_PMR_EL1` as a hint for interrupt distribution, configure a write
-            // to an EOI register to also deactivate the interrupt, and configure preemption groups
-            // for group 0 and group 1 interrupts separately.
-            write_sysreg!(icc_ctlr_el1, 0x00);
+            write_sysreg!(icc_bpr1_el1, 0x0);
+        }
+        unsafe {
+            write_sysreg!(icc_ctlr_el1, 0x02);
         }
 
+        // SAFETY: Writing to this system register doesn't access memory in any way.
+        // unsafe {
+        //     // Disable use of `ICC_PMR_EL1` as a hint for interrupt distribution, configure a write
+        //     // to an EOI register to also deactivate the interrupt, and configure preemption groups
+        //     // for group 0 and group 1 interrupts separately.
+        //     write_sysreg!(icc_ctlr_el1, 0x00);
+        // }
+
         unsafe {
             // Enable non-secure group 1.
-            write_sysreg!(icc_igrpen1_el1, 0x00000001);
+            write_sysreg!(icc_igrpen1_el1, 0x01);
         }
     }
 }
@@ -442,9 +499,9 @@ impl GenericArmGic for GicV3 {
     fn enable_interrupt(&mut self, intid: IntId) {
         let index = intid.0 / 32;
         let bit = 1 << (intid.0 % 32);
-
+        // let mpidr = get_mpidr();
         if intid.is_private() {
-            self.gicr.sgi_regs().ISENABLER[index].set(bit);
+            self.gicr.get_sgi_regs().ISENABLER[index].set(bit);
         } else {
             self.gicd.regs().ISENABLER[index].set(bit);
         }
@@ -455,7 +512,7 @@ impl GenericArmGic for GicV3 {
         let bit = 1 << (intid.0 % 32);
 
         if intid.is_private() {
-            self.gicr.sgi_regs().ICENABLER[index].set(bit);
+            self.gicr.get_sgi_regs().ICENABLER[index].set(bit);
         } else {
             self.gicd.regs().ICENABLER[index].set(bit);
         }
@@ -488,5 +545,7 @@ impl GenericArmGic for GicV3 {
     fn end_interrupt(&self, intid: IntId) {
         // SAFETY: Writing to this system register doesn't access memory in any way.
         unsafe { write_sysreg!(icc_eoir1_el1, intid.0 as u64) }
+        unsafe { write_sysreg!(icc_dir_el1, intid.0 as u64) }
     }
 }
+