implement init_cpu and init_plat

src/arch/aarch64/arm_gic/gic_v2/gic_v2.rs

@@ -5,6 +5,7 @@ use tock_registers::interfaces::Writeable;
 
 static GIC_DIST: Gic_Dist_Map = Gic_Dist_Map::new(GIC_V2_DISTRIBUTOR_PPTR as *mut u8);
 static GIC_CPUIFACE: Gic_Cpu_Iface_Map = Gic_Cpu_Iface_Map::new(GIC_V2_CONTROLLER_PPTR as *mut u8);
+
 // This is for aarch64 only
 pub fn cpu_iface_init() {
  GIC_DIST.regs().enable_clr[0].set(IRQ_SET_ALL);
@@ -19,11 +20,11 @@ pub fn cpu_iface_init() {
  }
 
  GIC_CPUIFACE.regs().icontrol.set(0);
- GIC_CPUIFACE.regs().pri_msk_c.set(0);
- GIC_CPUIFACE.regs().pb_c.set(0);
+ GIC_CPUIFACE.regs().pri_msk_c.set(0x000000f0);
+ GIC_CPUIFACE.regs().pb_c.set(0x00000003);
  let mut i = GIC_CPUIFACE.regs().int_ack.get();
  while (i & IRQ_MASK) != IRQ_NONE {
- GIC_CPUIFACE.regs().eoi.set(0);
+ GIC_CPUIFACE.regs().eoi.set(i);
  i = GIC_CPUIFACE.regs().int_ack.get();
  }
  GIC_CPUIFACE.regs().icontrol.set(1);
@@ -49,3 +50,122 @@ pub fn gic_int_ack() -> usize {
 pub fn ack_irq(irq: usize) {
  GIC_CPUIFACE.regs().eoi.set(irq as _);
 }
+
+pub fn dist_init() {
+ let nirqs = 32 * ((GIC_DIST.regs().ic_type.get() & 0x1f) + 1) as usize;
+
+ GIC_DIST.regs().enable.set(0);
+
+ for i in (0..nirqs).step_by(32) {
+ GIC_DIST.regs().enable_clr[i >> 5].set(IRQ_SET_ALL);
+ GIC_DIST.regs().pending_clr[i >> 5].set(IRQ_SET_ALL);
+ }
+
+ for i in (32..nirqs).step_by(4) {
+ GIC_DIST.regs().priority[i >> 2].set(0);
+ }
+
+ let target = infer_cpu_gic_id(nirqs);
+
+ for i in (0..nirqs).step_by(4) {
+ GIC_DIST.regs().targets[i >> 2].set(TARGET_CPU_ALLINT(target));
+ }
+
+ for i in (64..nirqs).step_by(32) {
+ GIC_DIST.regs().config[i >> 5].set(0x55555555);
+ }
+
+ for i in (0..nirqs).step_by(32) {
+ GIC_DIST.regs().security[i >> 5].set(0);
+ }
+
+ GIC_DIST.regs().enable.set(1);
+}
+
+// BOOT_CODE static void dist_init(void)
+// {
+// word_t i;
+// int nirqs = 32 * ((gic_dist->ic_type & 0x1f) + 1);
+// gic_dist->enable = 0;
+
+// for (i = 0; i < nirqs; i += 32) {
+// /* disable */
+// gic_dist->enable_clr[i >> 5] = IRQ_SET_ALL;
+// /* clear pending */
+// gic_dist->pending_clr[i >> 5] = IRQ_SET_ALL;
+// }
+
+// /* reset interrupts priority */
+// for (i = 32; i < nirqs; i += 4) {
+// if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT)) {
+// gic_dist->priority[i >> 2] = 0x80808080;
+// } else {
+// gic_dist->priority[i >> 2] = 0;
+// }
+// }
+
+// /*
+// * reset int target to current cpu
+// * We query which id that the GIC uses for us and use that.
+// */
+// uint8_t target = infer_cpu_gic_id(nirqs);
+// for (i = 0; i < nirqs; i += 4) {
+// gic_dist->targets[i >> 2] = TARGET_CPU_ALLINT(target);
+// }
+
+// /* level-triggered, 1-N */
+// for (i = 64; i < nirqs; i += 32) {
+// gic_dist->config[i >> 5] = 0x55555555;
+// }
+
+// /* group 0 for secure; group 1 for non-secure */
+// for (i = 0; i < nirqs; i += 32) {
+// if (config_set(CONFIG_ARM_HYPERVISOR_SUPPORT) && !config_set(CONFIG_PLAT_QEMU_ARM_VIRT)) {
+// gic_dist->security[i >> 5] = 0xffffffff;
+// } else {
+// gic_dist->security[i >> 5] = 0;
+// }
+// }
+// /* enable the int controller */
+// gic_dist->enable = 1;
+// }
+
+// BOOT_CODE static uint8_t infer_cpu_gic_id(int nirqs)
+// {
+// word_t i;
+// uint32_t target = 0;
+// for (i = 0; i < nirqs; i += 4) {
+// target = gic_dist->targets[i >> 2];
+// target |= target >> 16;
+// target |= target >> 8;
+// if (target) {
+// break;
+// }
+// }
+// if (!target) {
+// printf("Warning: Could not infer GIC interrupt target ID, assuming 0.\n");
+// target = BIT(0);
+// }
+// // return target & 0xff;
+// }
+
+fn infer_cpu_gic_id(nirqs: usize) -> u8 {
+ let mut target = 0;
+ for i in (0..nirqs).step_by(4) {
+ target = GIC_DIST.regs().targets[i >> 2].get();
+ target |= target >> 16;
+ target |= target >> 8;
+ if target != 0 {
+ break;
+ }
+ }
+ if target == 0 {
+ log::warn!("Warning: Could not infer GIC interrupt target ID, assuming 0.");
+ target = 1;
+ }
+ (target & 0xff) as u8
+}
+
+fn TARGET_CPU_ALLINT(CPU: u8) -> u32 {
+ ((CPU as u32) << 0) | ((CPU as u32) << 8) | ((CPU as u32) << 16) | ((CPU as u32) << 24)
+}

src/arch/aarch64/boot.rs

@@ -4,7 +4,7 @@ use sel4_common::{sel4_config::PAGE_BITS, BIT};
 use sel4_task::create_idle_thread;
 use sel4_vspace::{kpptr_to_paddr, rust_map_kernel_window};
 
-use crate::arch::aarch64::platform::{cleanInvalidateL1Caches, invalidateLocalTLB};
+use crate::arch::aarch64::platform::{cleanInvalidateL1Caches, init_cpu, invalidateLocalTLB};
 
 use crate::{
  arch::init_freemem,
@@ -16,6 +16,8 @@ use crate::{
  structures::{p_region_t, seL4_SlotRegion, v_region_t},
 };
 
+use super::platform::initIRQController;
+
 pub fn try_init_kernel(
  ui_p_reg_start: usize,
  ui_p_reg_end: usize,
@@ -48,18 +50,17 @@ pub fn try_init_kernel(
  let extra_bi_frame_vptr = bi_frame_vptr + BIT!(BI_FRAME_SIZE_BITS);
 
  // Map kernel window area
- // sel4_common::ffi_call!(map_kernel_window);
  rust_map_kernel_window();
 
  // Initialize cpu
- let inited = sel4_common::ffi_call!(init_cpu -> bool);
-
+ let inited = init_cpu();
  // Initialize the drivers used by the kernel.
  driver_collect::init();
  log::debug!("init_cpu: {}", inited);
 
  // Initialize platform
- sel4_common::ffi_call!(init_plat);
+ // sel4_common::ffi_call!(init_plat);
+ init_plat();
 
  let dtb_p_reg = init_dtb(dtb_size, dtb_phys_addr, &mut extra_bi_size);
  if dtb_p_reg.is_none() {
@@ -130,3 +131,7 @@ pub fn try_init_kernel(
 
  true
 }
+
+fn init_plat() {
+ initIRQController()
+}

src/arch/aarch64/c_traps.rs

@@ -98,7 +98,6 @@ pub fn c_handle_syscall(_cptr: usize, _msgInfo: usize, syscall: usize) {
  clh_lock_acquire(cpu_id(), false);
  }
  entry_hook();
- // ffi_call!(c_entry_hook);
  // if hart_id() == 0 {
  // debug!("c_handle_syscall: syscall: {},", syscall as isize);
  // }

src/arch/aarch64/ffi.rs

@@ -1,4 +1,3 @@
 extern "C" {
- pub fn arm_vector_table();
  pub fn initTimer();
 }

src/arch/aarch64/platform.rs

@@ -1,11 +1,10 @@
-use aarch64_cpu::registers::Writeable;
-use aarch64_cpu::registers::{TPIDR_EL1, VBAR_EL1};
+use aarch64_cpu::registers::TPIDR_EL1;
+use aarch64_cpu::registers::{Writeable, CNTKCTL_EL1};
 use core::arch::asm;
 use sel4_common::arch::config::{KERNEL_ELF_BASE, PADDR_TOP};
 use sel4_common::ffi::kernel_stack_alloc;
 use sel4_common::ffi_addr;
 use sel4_common::sel4_config::{wordBits, CONFIG_KERNEL_STACK_BITS};
-use sel4_common::utils::cpu_id;
 
 use super::ffi::*;
 use crate::boot::{
@@ -14,36 +13,40 @@ use crate::boot::{
 };
 use crate::config::*;
 use crate::structures::*;
+use crate::utils::{fpsimd_HWCapTest, setVTable};
 use log::debug;
 use sel4_vspace::*;
 
-use super::arm_gic::gic_v2::gic_v2::cpu_initLocalIRQController;
+use super::arm_gic::gic_v2::gic_v2::{cpu_initLocalIRQController, dist_init};
 
 #[allow(unused)]
 pub fn init_cpu() -> bool {
- // use arch::aarch64::arm_gic::gic_v2;
+ activate_kernel_vspace();
 
- // Setup kernel stack pointer.
+ // CPU's exception vector table
+ unsafe {
+ setVTable(ffi_addr!(arm_vector_table));
+ }
 
- // Wrapping_add, first argument is CURRENT_CPU_INDEX
- //
- let mut stack_top =
- unsafe { &kernel_stack_alloc.data[0][0 + (1 << CONFIG_KERNEL_STACK_BITS)] as *const u8 }
- as u64;
- stack_top |= cpu_id() as u64; //the judge of enable smp have done in cpu_id
+ // Setup kernel stack pointer.
+ let mut stack_top = unsafe {
+ kernel_stack_alloc.data.as_ptr().add(0) as usize
+ + sel4_common::BIT!(CONFIG_KERNEL_STACK_BITS)
+ } as u64;
 
- TPIDR_EL1.set(stack_top);
  // CPU's exception vector table
  unsafe {
- asm!("dsb sy;"); // DSB SY
- VBAR_EL1.set(arm_vector_table as u64);
- asm!("isb;"); // ISB SY
+ setVTable(ffi_addr!(arm_vector_table));
  }
+ TPIDR_EL1.set(stack_top);
+
+ let haveHWFPU = fpsimd_HWCapTest();
+
  // initLocalIRQController
  cpu_initLocalIRQController();
+
  // armv_init_user_access
- // user_access::armv_init_user_access();
- //initTimer
+ armv_init_user_access();
 
  unsafe {
  initTimer();
@@ -184,3 +187,11 @@ fn readCacheSize(level: usize) -> usize {
  }
  size
 }
+
+fn armv_init_user_access() {
+ CNTKCTL_EL1.set(0);
+}
+
+pub fn initIRQController() {
+ dist_init();
+}

src/utils.rs

@@ -1,3 +1,11 @@
+#[cfg(target_arch="aarch64")]
+use sel4_common::{
+ sel4_config::{ID_AA64PFR0_EL1_ASIMD, ID_AA64PFR0_EL1_FP},
+ MASK,
+};
+#[cfg(target_arch="aarch64")]
+use sel4_vspace::{dsb, isb};
+
 use crate::BIT;
 
 #[cfg(target_arch = "riscv64")]
@@ -51,4 +59,34 @@ pub fn clear_memory_pt(ptr: *mut u8, bits: usize) {
  pptr_to_paddr(ptr as usize),
  );
  }
-}
+}
+
+#[inline]
+#[cfg(target_arch="aarch64")]
+pub fn setVTable(addr: usize) {
+ dsb();
+ unsafe {
+ core::arch::asm!("MSR vbar_el1, {0}", in(reg) addr);
+ }
+ isb();
+}
+
+#[inline]
+#[cfg(target_arch="aarch64")]
+pub fn fpsimd_HWCapTest() -> bool {
+ let mut id_aa64pfr0: usize;
+
+ // 读取系统寄存器
+ unsafe {
+ core::arch::asm!("mrs {}, id_aa64pfr0_el1", out(reg) id_aa64pfr0);
+ }
+
+ // 检查硬件是否支持FP和ASIMD
+ if ((id_aa64pfr0 >> ID_AA64PFR0_EL1_FP) & MASK!(4)) == MASK!(4)
+ || ((id_aa64pfr0 >> ID_AA64PFR0_EL1_ASIMD) & MASK!(4)) == MASK!(4)
+ {
+ return false;
+ }
+
+ true
+}