From 11c014cd76d355969816fe8ea7d32a7ea864e3ce Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Tue, 28 Apr 2026 11:29:44 +0800 Subject: [PATCH 1/5] first verion for virt frame --- kernel/src/arch/aarch64/registers/mair_el2.rs | 76 +++ kernel/src/arch/aarch64/registers/mod.rs | 7 + .../src/arch/aarch64/registers/sctlr_el2.rs | 294 +++++++++ kernel/src/arch/aarch64/registers/spsr_el2.rs | 113 ++++ kernel/src/arch/aarch64/registers/tcr_el2.rs | 104 ++++ .../src/arch/aarch64/registers/ttbr0_el2.rs | 67 +++ kernel/src/arch/aarch64/registers/vtcr_el2.rs | 107 ++++ .../src/arch/aarch64/registers/vttbr_el2.rs | 74 +++ kernel/src/arch/aarch64/virt/exit.rs | 367 ++++++++++++ kernel/src/arch/aarch64/virt/guest.rs | 29 + kernel/src/arch/aarch64/virt/hyper.rs | 104 +++- kernel/src/arch/aarch64/virt/mmu_el2.rs | 117 ++++ kernel/src/arch/aarch64/virt/mmu_s2.rs | 187 ++++++ kernel/src/arch/aarch64/virt/mod.rs | 113 ++++ kernel/src/arch/aarch64/virt/vcpu.rs | 422 +++++++++++++ kernel/src/arch/aarch64/virt/vector.rs | 248 +++++++- kernel/src/arch/aarch64/virt/vgic.rs | 561 ++++++++++++++++++ kernel/src/arch/aarch64/virt/vtimer.rs | 96 +++ .../src/boards/qemu_virt64_aarch64/config.rs | 2 +- kernel/src/boards/qemu_virt64_aarch64/init.rs | 2 + kernel/src/boards/qemu_virt64_aarch64/link.x | 4 +- 21 files changed, 3065 insertions(+), 29 deletions(-) create mode 100644 kernel/src/arch/aarch64/registers/mair_el2.rs create mode 100644 kernel/src/arch/aarch64/registers/sctlr_el2.rs create mode 100644 kernel/src/arch/aarch64/registers/spsr_el2.rs create mode 100644 kernel/src/arch/aarch64/registers/tcr_el2.rs create mode 100644 kernel/src/arch/aarch64/registers/ttbr0_el2.rs create mode 100644 kernel/src/arch/aarch64/registers/vtcr_el2.rs create mode 100644 kernel/src/arch/aarch64/registers/vttbr_el2.rs create mode 100644 kernel/src/arch/aarch64/virt/exit.rs create mode 100644 kernel/src/arch/aarch64/virt/guest.rs create mode 100644 kernel/src/arch/aarch64/virt/mmu_el2.rs create mode 100644 kernel/src/arch/aarch64/virt/mmu_s2.rs create mode 100644 kernel/src/arch/aarch64/virt/vcpu.rs create mode 100644 kernel/src/arch/aarch64/virt/vgic.rs create mode 100644 kernel/src/arch/aarch64/virt/vtimer.rs diff --git a/kernel/src/arch/aarch64/registers/mair_el2.rs b/kernel/src/arch/aarch64/registers/mair_el2.rs new file mode 100644 index 00000000..773746ff --- /dev/null +++ b/kernel/src/arch/aarch64/registers/mair_el2.rs @@ -0,0 +1,76 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{ + interfaces::{Readable, Writeable}, + register_bitfields, +}; + +register_bitfields! {u64, + pub MAIR_EL2 [ + /// Attribute 1 - Normal Memory Outer + Attr1_Normal_Outer OFFSET(12) NUMBITS(4) [ + WriteBack_NonTransient_ReadWriteAlloc = 0b1111 + ], + /// Attribute 1 - Normal Memory Inner + Attr1_Normal_Inner OFFSET(8) NUMBITS(4) [ + WriteBack_NonTransient_ReadWriteAlloc = 0b1111 + ], + /// Attribute 0 - Device Memory + Attr0_Device OFFSET(0) NUMBITS(8) [ + NonGathering_NonReordering_NonEarlyWriteAck = 0b0000_0000, + NonGathering_NonReordering_EarlyWriteAck = 0b0000_0100, + NonGathering_Reordering_EarlyWriteAck = 0b0000_1000, + Gathering_Reordering_EarlyWriteAck = 0b0000_1100 + ] + ] +} + +pub struct MairEl2; + +impl Readable for MairEl2 { + type T = u64; + type R = MAIR_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, mair_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for MairEl2 { + type T = u64; + type R = MAIR_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr mair_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const MAIR_EL2: MairEl2 = MairEl2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/registers/mod.rs b/kernel/src/arch/aarch64/registers/mod.rs index bd0e20cf..361976b6 100644 --- a/kernel/src/arch/aarch64/registers/mod.rs +++ b/kernel/src/arch/aarch64/registers/mod.rs @@ -21,10 +21,17 @@ pub mod daif; pub mod esr_el1; pub mod hcr_el2; pub mod mair_el1; +pub mod mair_el2; pub mod mpidr_el1; pub mod sctlr_el1; +pub mod sctlr_el2; pub mod spsel; +pub mod spsr_el2; pub mod tcr_el1; +pub mod tcr_el2; pub mod ttbr0_el1; +pub mod ttbr0_el2; pub mod ttbr1_el1; pub mod vbar_el1; +pub mod vtcr_el2; +pub mod vttbr_el2; diff --git a/kernel/src/arch/aarch64/registers/sctlr_el2.rs b/kernel/src/arch/aarch64/registers/sctlr_el2.rs new file mode 100644 index 00000000..af34574c --- /dev/null +++ b/kernel/src/arch/aarch64/registers/sctlr_el2.rs @@ -0,0 +1,294 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{interfaces::*, register_bitfields}; + +register_bitfields! {u64, + pub SCTLR_EL2 [ + /// Trap IMPLEMENTATION DEFINED functionality. + TIDCP OFFSET(63) NUMBITS(1) [ + DontTrap = 0, + Trap = 1, + ], + + /// SP Interrupt Mask enable. + SPINTMASK OFFSET(62) NUMBITS(1) [ + Enabled = 0, + Disabled = 1, + ], + + /// Non-maskable Interrupt enable. + NMI OFFSET(61) NUMBITS(1) [], + + /// Tag Checking Store Only. + TCSO OFFSET(59) NUMBITS(1) [], + + /// Enhanced Privileged Access Never. + EPAN OFFSET(57) NUMBITS(1) [], + + /// Enables the Transactional Memory Extension at EL2. + TME OFFSET(53) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// Default PSTATE.SSBS value on Exception Entry. + DSSBS OFFSET(44) NUMBITS(1) [], + + /// When SCR_EL3.ATA == 1, controls access to Allocation Tags and Tag Check operations in EL2. + ATA OFFSET(43) NUMBITS(1) [], + + /// Tag Check Fault in EL2. Controls the effect of Tag Check Faults due to Loads and Stores in EL2. + TCF OFFSET(40) NUMBITS(2) [], + + /// When synchronous exceptions are not being generated by Tag Check Faults, + /// this field controls whether on exception entry into EL2, + /// all Tag Check Faults due to instructions executed before exception entry, + /// that are reported asynchronously, are synchronized into TFSR_EL2 register. + ITFSB OFFSET(37) NUMBITS(1) [], + + /// When FEAT_BTI is implemented: + /// Configures the Branch Type compatibility of the implicit BTI behavior for EL2. + BT1 OFFSET(36) NUMBITS(1) [], + + /// When FEAT_BTI is implemented: + /// Configures the Branch Type compatibility of the implicit BTI behavior for EL2. + BT0 OFFSET(35) NUMBITS(1) [], + + /// When FEAT_FPMR is implemented: + /// Enables direct and indirect accesses to FPMR from EL2. + ENFPM OFFSET(34) NUMBITS(1) [], + + /// When FEAT_MOPS is implemented: + /// Memory Copy and Memory Set instructions Enable. + MSCEN OFFSET(33) NUMBITS(1) [], + + /// Controls cache maintenance instruction permission for EL2. + CMOW OFFSET(32) NUMBITS(1) [], + + /// When FEAT_PAuth is implemented: + /// Controls enabling of pointer authentication of instruction addresses, using the APIAKey_EL2 key. + ENIA OFFSET(31) NUMBITS(1) [ + Disable = 0, + Enabled = 1, + ], + + /// When FEAT_PAuth is implemented: + /// Controls enabling of pointer authentication of instruction addresses, using the APIBKey_EL2 key. + ENIB OFFSET(30) NUMBITS(1) [ + Disable = 0, + Enabled = 1, + ], + + /// When FEAT_LSMAOC is implemented: + /// Load Multiple and Store Multiple Atomicity and Ordering Enable. + LSMAOE OFFSET(29) NUMBITS(1) [ + Disable = 0, + Enabled = 1, + ], + + /// When FEAT_LSMAOC is implemented: + /// No Trap Load Multiple and Store Multiple to Device-nGRE/Device-nGnRE/Device-nGnRnE memory. + NTLSMD OFFSET(28) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// When FEAT_PAuth is implemented: + /// Controls enabling of pointer authentication of instruction addresses, using the APDAKey_EL2 key. + ENDA OFFSET(27) NUMBITS(1) [ + Disable = 0, + Enabled = 1, + ], + + /// Traps EL2 execution of cache maintenance instructions to EL2 (self). + UCI OFFSET(26) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// When FEAT_MixedEnd is implemented: + /// Endianness of data accesses at EL2, and stage 1 translation table walks in the EL2 translation regime. + EE OFFSET(25) NUMBITS(1) [ + LittleEndian = 0, + BigEndian = 1, + ], + + /// When FEAT_PAN is implemented: + /// Set Privileged Access Never, on taking an exception to EL2. + SPAN OFFSET(23) NUMBITS(1) [ + Set = 0, + Unset = 1 + ], + + /// When FEAT_ExS is implemented: + /// Exception Entry is Context Synchronizing. + EIS OFFSET(22) NUMBITS(1) [ + NotContextSynchronizingEvent = 0, + ContextSynchronizingEvent = 1 + ], + + /// When FEAT_IESB is implemented: + /// Implicit Error Synchronization event enable. + IESB OFFSET(21) NUMBITS(1) [ + Disable = 0, + Enable = 1, + ], + + /// Write permission implies XN (Execute-never). + WXN OFFSET(19) NUMBITS(1) [ + Disable = 0, + Enable = 1, + ], + + /// Traps EL2 execution of WFE instructions to EL2 (self). + NTWE OFFSET(18) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// Traps EL2 execution of WFI instructions to EL2 (self). + NTWI OFFSET(16) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// Traps EL2 accesses to the CTR_EL0 to EL2 (self). + UCT OFFSET(15) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// Traps EL2 execution of DC ZVA instructions to EL2 (self). + DZE OFFSET(14) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// When FEAT_PAuth is implemented: + /// Controls enabling of pointer authentication of instruction addresses, using the APDBKey_EL2 key. + ENDB OFFSET(13) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// Instruction access Cacheability control, for accesses at EL2. + I OFFSET(12) NUMBITS(1) [ + NonCacheable = 0, + Cacheable = 1 + ], + + /// When FEAT_ExS is implemented: + /// Exception Exit is Context Synchronizing. + EOS OFFSET(11) NUMBITS(1) [ + NotContextSynchronizingEvent = 0, + ContextSynchronizingEvent = 1 + ], + + /// When FEAT_SPECRES is implemented: + /// Enable EL2 access to the following System instructions. + ENRCTX OFFSET(10) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// User Mask Access. Traps EL2 execution of MSR and MRS instructions + /// that access the PSTATE.{D, A, I, F} masks to EL2. + UMA OFFSET(9) NUMBITS(1) [ + Trap = 0, + DontTrap = 1, + ], + + /// When FEAT_LSE2 is implemented: + /// Non-aligned access. + NAA OFFSET(6) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// CP15BEN - System instruction memory barrier enable for EL2. + CP15BEN OFFSET(5) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// SP Alignment check enable for EL2. + SA0 OFFSET(4) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// SP Alignment check enable. + SA OFFSET(3) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// Stage 1 Cacheability control, for data accesses at EL2. + C OFFSET(2) NUMBITS(1) [ + NonCacheable = 0, + Cacheable = 1 + ], + + /// Alignment check enable. + A OFFSET(1) NUMBITS(1) [ + DisableAlignment = 0, + EnableAlignment = 1 + ], + + /// MMU enable for EL2 stage 1 address translation. + M OFFSET(0) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ] + ] +} + +pub struct SctlrEl2; + +impl Readable for SctlrEl2 { + type T = u64; + type R = SCTLR_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, sctlr_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for SctlrEl2 { + type T = u64; + type R = SCTLR_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr sctlr_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const SCTLR_EL2: SctlrEl2 = SctlrEl2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/registers/spsr_el2.rs b/kernel/src/arch/aarch64/registers/spsr_el2.rs new file mode 100644 index 00000000..a8a3c4f2 --- /dev/null +++ b/kernel/src/arch/aarch64/registers/spsr_el2.rs @@ -0,0 +1,113 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{interfaces::*, register_bitfields}; + +register_bitfields! {u64, + pub SPSR_EL2 [ + /// Debug exception mask. Controls routing of debug exceptions to EL2. + D OFFSET(9) NUMBITS(1) [ + Allow = 0, + Prohibit = 1 + ], + + /// SError interrupt mask. + A OFFSET(8) NUMBITS(1) [ + Allow = 0, + Prohibit = 1 + ], + + /// IRQ interrupt mask. + I OFFSET(7) NUMBITS(1) [ + Allow = 0, + Prohibit = 1 + ], + + /// FIQ interrupt mask. + F OFFSET(6) NUMBITS(1) [ + Allow = 0, + Prohibit = 1 + ], + + /// Exception mask bits. These bits mask PSTATE.A, I, F when taking an exception to EL2. + /// The value saved is OR of the respective mask bits. + E OFFSET(5) NUMBITS(1) [ + LittleEndian = 0, + BigEndian = 1 + ], + + /// Instruction set state. + /// 0: AArch64 + /// 1: AArch32 + IL OFFSET(4) NUMBITS(1) [ + Valid = 0, + Illegal = 1 + ], + + /// Stack Pointer selection. + /// 0: Use SP_EL0 + /// 1: Use SP_EL2 + SP OFFSET(0) NUMBITS(1) [ + EL0 = 0, + EL2 = 1 + ], + + /// Mode/Execution state. + M OFFSET(0) NUMBITS(4) [ + EL0t = 0b0000, + EL1t = 0b0100, + EL1h = 0b0101, + EL2t = 0b1000, + EL2h = 0b1001 + ] + ] +} + +pub struct SpsrEl2; + +impl Readable for SpsrEl2 { + type T = u64; + type R = SPSR_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, spsr_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for SpsrEl2 { + type T = u64; + type R = SPSR_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr spsr_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const SPSR_EL2: SpsrEl2 = SpsrEl2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/registers/tcr_el2.rs b/kernel/src/arch/aarch64/registers/tcr_el2.rs new file mode 100644 index 00000000..e45cb943 --- /dev/null +++ b/kernel/src/arch/aarch64/registers/tcr_el2.rs @@ -0,0 +1,104 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{ + interfaces::{Readable, Writeable}, + register_bitfields, +}; + +// See: https://developer.arm.com/documentation/ddi0601/latest/AArch64-Registers/TCR-EL2--Translation-Control-Register--EL2- +register_bitfields! {u64, + pub TCR_EL2 [ + /// Physical address size + PS OFFSET(16) NUMBITS(3) [ + Bits_32 = 0b000, + Bits_36 = 0b001, + Bits_40 = 0b010, + Bits_42 = 0b011, + Bits_44 = 0b100, + Bits_48 = 0b101 + ], + + /// Granule size for TTBR0_EL2 + TG0 OFFSET(14) NUMBITS(2) [ + KiB_4 = 0b00, + KiB_64 = 0b01, + KiB_16 = 0b10 + ], + + /// Outer cacheability attribute for memory associated with translation table walks using TTBR0_EL2 + ORGN0 OFFSET(10) NUMBITS(2) [ + NonCacheable = 0b00, + WriteBack_ReadAlloc_NoWriteAlloc_Cacheable = 0b01, + WriteThrough_ReadAlloc_NoWriteAlloc_Cacheable = 0b10, + WriteBack_ReadAlloc_WriteAlloc_Cacheable = 0b11 + ], + + /// Inner cacheability attribute for memory associated with translation table walks using TTBR0_EL2 + IRGN0 OFFSET(8) NUMBITS(2) [ + NonCacheable = 0b00, + WriteBack_ReadAlloc_NoWriteAlloc_Cacheable = 0b01, + WriteThrough_ReadAlloc_NoWriteAlloc_Cacheable = 0b10, + WriteBack_ReadAlloc_WriteAlloc_Cacheable = 0b11 + ], + + /// Shareability attribute for memory associated with translation table walks using TTBR0_EL2 + SH0 OFFSET(12) NUMBITS(2) [ + NonShareable = 0b00, + OuterShareable = 0b10, + Inner = 0b11 + ], + + /// The size offset of the memory region addressed by TTBR0_EL2 (region size = 2^(64-T0SZ)) + T0SZ OFFSET(0) NUMBITS(6) [] + ] +} + +pub struct TcrEl2; + +impl Readable for TcrEl2 { + type T = u64; + type R = TCR_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, tcr_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for TcrEl2 { + type T = u64; + type R = TCR_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr tcr_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const TCR_EL2: TcrEl2 = TcrEl2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/registers/ttbr0_el2.rs b/kernel/src/arch/aarch64/registers/ttbr0_el2.rs new file mode 100644 index 00000000..0dbf5d6f --- /dev/null +++ b/kernel/src/arch/aarch64/registers/ttbr0_el2.rs @@ -0,0 +1,67 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{ + interfaces::{Readable, Writeable}, + register_bitfields, +}; + +// See: https://developer.arm.com/documentation/ddi0601/latest/AArch64-Registers/TTBR0-EL2--Translation-Table-Base-Register-0--EL2- +register_bitfields! {u64, + pub TTBR0_EL2 [ + /// Translation table base address + BADDR OFFSET(1) NUMBITS(47) [], + + /// Common not Private + CNP OFFSET(0) NUMBITS(1) [] + ] +} + +pub struct Ttbr0El2; + +impl Readable for Ttbr0El2 { + type T = u64; + type R = TTBR0_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, ttbr0_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for Ttbr0El2 { + type T = u64; + type R = TTBR0_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr ttbr0_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const TTBR0_EL2: Ttbr0El2 = Ttbr0El2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/registers/vtcr_el2.rs b/kernel/src/arch/aarch64/registers/vtcr_el2.rs new file mode 100644 index 00000000..022ffffc --- /dev/null +++ b/kernel/src/arch/aarch64/registers/vtcr_el2.rs @@ -0,0 +1,107 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{ + interfaces::{Readable, Writeable}, + register_bitfields, +}; + +// See: https://developer.arm.com/documentation/ddi0601/latest/AArch64-Registers/VTCR-EL2--Virtualization-Translation-Control-Register +register_bitfields! {u64, + pub VTCR_EL2 [ + /// Physical address size for second stage translation + PS OFFSET(16) NUMBITS(3) [ + PA_32B_4GB = 0b000, + PA_36B_64GB = 0b001, + PA_40B_1TB = 0b010, + PA_42B_4TB = 0b011, + PA_44B_16TB = 0b100, + PA_48B_256TB = 0b101 + ], + + /// Granule size for stage 2 translation + TG0 OFFSET(14) NUMBITS(2) [ + Granule4KB = 0b00, + Granule64KB = 0b01, + Granule16KB = 0b10 + ], + + /// Outer cacheability for stage 2 translation table walks + ORGN0 OFFSET(10) NUMBITS(2) [ + NonCacheable = 0b00, + NormalWBRAWA = 0b01, + NormalWT = 0b10, + NormalWBnWA = 0b11 + ], + + /// Inner cacheability for stage 2 translation table walks + IRGN0 OFFSET(8) NUMBITS(2) [ + NonCacheable = 0b00, + NormalWBRAWA = 0b01, + NormalWT = 0b10, + NormalWBnWA = 0b11 + ], + + /// Shareability for stage 2 translation table walks + SH0 OFFSET(12) NUMBITS(2) [ + NonShareable = 0b00, + OuterShareable = 0b10, + Inner = 0b11 + ], + + /// Starting level of the stage 2 translation lookup + SL0 OFFSET(6) NUMBITS(2) [], + + /// The size offset of the memory region for stage 2 (region size = 2^(64-T0SZ)) + T0SZ OFFSET(0) NUMBITS(6) [] + ] +} + +pub struct VtcrEl2; + +impl Readable for VtcrEl2 { + type T = u64; + type R = VTCR_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, vtcr_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for VtcrEl2 { + type T = u64; + type R = VTCR_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr vtcr_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const VTCR_EL2: VtcrEl2 = VtcrEl2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/registers/vttbr_el2.rs b/kernel/src/arch/aarch64/registers/vttbr_el2.rs new file mode 100644 index 00000000..70031ea8 --- /dev/null +++ b/kernel/src/arch/aarch64/registers/vttbr_el2.rs @@ -0,0 +1,74 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use tock_registers::{interfaces::*, register_bitfields}; + +register_bitfields! {u64, + pub VTTBR_EL2 [ + /// VMID, bits [63:48] - Virtual Machine ID + /// Used to tag ASID for Stage-2 translation + VMID OFFSET(48) NUMBITS(16) [], + + /// BADDR, bits [47:1] - Translation Table Base Address + /// Physical address of the Level-1 table for Stage-2 translation + /// Must be aligned to table size (48-bit address space = 256KB aligned) + BADDR OFFSET(1) NUMBITS(47) [], + + /// CnP, bit [0] - Common Not Private + /// 0 = Not common, each PE uses separate tables + /// 1 = Common, tables can be shared + CnP OFFSET(0) NUMBITS(1) [ + NotCommon = 0, + Common = 1 + ] + ] +} + +pub struct VttbrEl2; + +impl Readable for VttbrEl2 { + type T = u64; + type R = VTTBR_EL2::Register; + + #[inline] + fn get(&self) -> Self::T { + let value; + unsafe { + core::arch::asm!( + "mrs {}, vttbr_el2", + out(reg) value, + options(nomem, nostack) + ); + } + value + } +} + +impl Writeable for VttbrEl2 { + type T = u64; + type R = VTTBR_EL2::Register; + + #[inline] + fn set(&self, value: Self::T) { + unsafe { + core::arch::asm!( + "msr vttbr_el2, {}", + in(reg) value, + options(nomem, nostack) + ); + } + } +} + +pub const VTTBR_EL2: VttbrEl2 = VttbrEl2 {}; \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs new file mode 100644 index 00000000..dee4f53c --- /dev/null +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -0,0 +1,367 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use core::arch::asm; +use super::{vcpu::Vcpu, vgic, hyper}; +use semihosting::println; + +static mut GUEST_SHUTDOWN: bool = false; + +pub const PSCI_VERSION: u32 = 0x8400_0000; +pub const PSCI_SYSTEM_OFF: u32 = 0x8400_0008; +pub const PSCI_SYSTEM_RESET: u32 = 0x8400_0009; +pub const PSCI_FEATURES: u32 = 0x8400_000A; +pub const HVC_VMM_GET_INFO: u64 = 0x11; +pub const HVC_VMM_INJECT_IRQ: u64 = 0x13; +pub const HVC_GUEST_SHUTDOWN: u64 = 0x20; + +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum VmExitReason { + Hvc, + Svc, + DataAbortLowerEL, + InstructionAbortLowerEL, + TrappedWfiWfe, + Unknown(u32), +} + +#[derive(Debug)] +pub struct VmExitInfo { + pub reason: VmExitReason, + pub esr: u64, + pub far: usize, + pub pstate: u64, + pub return_addr: usize, +} + +#[inline] +pub fn parse_exit_reason(esr: u64) -> VmExitReason { + let ec = (esr >> 26) & 0x3F; + + match ec { + 0x16 => VmExitReason::Hvc, + 0x15 => VmExitReason::Svc, + 0x24 => VmExitReason::DataAbortLowerEL, + 0x20 => VmExitReason::InstructionAbortLowerEL, + 0x01 => VmExitReason::TrappedWfiWfe, + _ => VmExitReason::Unknown(ec as u32), + } +} + +pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { + vgic::sync(vcpu.id()); + let _context = vcpu.context_mut(); + let esr = read_esr_el2(); + let elr = read_elr_el2(); + let pstate = read_spsr_el2(); + let reason = parse_exit_reason(esr); + + let exit_info = VmExitInfo { + reason, + esr, + far: elr, + pstate, + return_addr: elr + 4, + }; + + semihosting::println!("[EXIT] VM Exit Happened!"); + semihosting::println!("[EXIT] Reason: {:?}", reason); + semihosting::println!("[EXIT] ESR: {:#x}", esr); + semihosting::println!("[EXIT] EC: {:#x}", (esr >> 26) & 0x3F); + semihosting::println!("[EXIT] FAR: {:#x}", elr); + semihosting::println!("[EXIT] PSTATE: {:#x}", pstate); + + match reason { + VmExitReason::Hvc => { + handle_hvc(vcpu, &exit_info) + } + VmExitReason::Svc => { + handle_svc(vcpu, &exit_info) + } + VmExitReason::DataAbortLowerEL => { + semihosting::println!("[EXIT] Data Abort from Guest (Stage-2 Fault)"); + let iss = esr & 0x1FFFFFF; + let dfsc = iss & 0x3F; + let is_write = (iss & (1 << 6)) != 0; + let faulting_pc = vcpu.context().elr_el2; + + unsafe { + semihosting::println!("====================================="); + semihosting::println!("[EXIT] PoC Guest triggered Data Abort!"); + semihosting::println!("[EXIT] 1. Faulting PC (ELR_EL2) : {}", faulting_pc); + semihosting::println!("[EXIT] 2. Target Addr (FAR_EL2) : {}", { + let far: u64; + core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); + far + }); + if is_write { + semihosting::println!("[EXIT] 3. Access Type : WRITE"); + } else { + semihosting::println!("[EXIT] 3. Access Type : READ"); + } + + semihosting::println!("[EXIT] 4. DFSC Code : {}", dfsc as u64); + let hpfar_el2: u64; + unsafe { core::arch::asm!("mrs {}, hpfar_el2", out(reg) hpfar_el2); } + + // Caclate Linux want to access physical address (IPA) + let fault_ipa = (hpfar_el2 & 0x0000_00FF_FFFF_FFF0) << 8; + println!("Target Physical Addr (IPA): {:#x}", fault_ipa); + semihosting::println!("====================================="); + } + + if (dfsc & 0x3C) == 0x04 || (dfsc & 0x3C) == 0x08 || (dfsc & 0x3C) == 0x0C { + // Translation fault (level 0/1/2/3) - Stage-2 未映射 + unsafe { + semihosting::println!("[EXIT] Stage-2 Translation Fault - skipping instruction"); + let far: u64; + core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); + let hpfar_el2: u64; + core::arch::asm!("mrs {}, hpfar_el2", out(reg) hpfar_el2, options(nostack)); + //Caculate exact PA for vGIC. + let fault_ipa_base = (hpfar_el2 & 0x0000_00FF_FFFF_FFF0) << 8; + let exact_ipa = fault_ipa_base | (far & 0xFFF); + let handled = vgic::handle_data_abort( + vcpu.id(), + esr, + exact_ipa, + &mut vcpu.context_mut().regs + ); + + if handled { + semihosting::println!("[EXIT] MMIO Handled by vGIC"); + vcpu.context_mut().elr_el2 += 4; + vgic::flush(vcpu.id()); + return true; + } else { + semihosting::println!("[EXIT] Unhandled Stage-2 Address!"); + } + } + } + semihosting::println!("[EXIT] Unrecoverable Data Abort, terminating Guest"); + false + } + VmExitReason::InstructionAbortLowerEL => { + semihosting::println!("[EXIT] Instruction Abort from Guest!"); + let iss = esr & 0x1FFFFFF; + let ifsc = iss & 0x3F; + + if (ifsc & 0x3C) == 0x14 { + semihosting::println!("[EXIT] Stage-2 Translation Fault (Instruction)!"); + } + false + } + VmExitReason::TrappedWfiWfe => { + semihosting::println!("[EXIT] Trapped WFI/WFE instruction"); + vcpu.context_mut().elr_el2 += 4; + true + } + VmExitReason::Unknown(ec) => { + semihosting::println!("[EXIT] Unknown Exit Reason: EC = {:#x}", ec); + false + } + } +} + +// hvc from guest. +fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { + let saved_x0 = vcpu.context().regs[0]; + semihosting::println!("[EXIT] Handle HVC Call"); + let vcpu_id = vcpu.id(); + let context = vcpu.context_mut(); + let hvc_num = info.esr & 0xFFFF; + semihosting::println!("[EXIT] HVC#{}", hvc_num); + + let mut need_advance_pc = true; + + // Easy HVC Services. + let result = match hvc_num { + 0x00 => { + let psci_func_id = context.regs[0] as u32; + semihosting::println!("[DEBUG] Returning to Linux: VBAR={:#x}, TTBR1={:#x}, SCTLR={:#x}", context.vbar_el1, context.ttbr1_el1, context.sctlr_el1); + match psci_func_id { + PSCI_VERSION => { + semihosting::println!("[EXIT] HVC#0: Linux requested PSCI_VERSION"); + context.regs[0] = 0x00010001; + context.regs[1] = 0; + context.regs[2] = 0; + context.regs[3] = 0; + + if context.regs[4] == 0 { + context.regs[4] = context.sp - 16; + } + true + } + PSCI_SYSTEM_OFF | PSCI_SYSTEM_RESET => { + unsafe { + semihosting::println!("[EXIT] HVC#0: Linux requested PSCI_SYSTEM_OFF. Shutting down..."); + GUEST_SHUTDOWN = true; + } + // Shutdown needn't "pc + 4". + need_advance_pc = false; + false + } + PSCI_FEATURES => { + let feature_id = context.regs[1] as u32; + // semihosting::println!("[EXIT] HVC#0: Linux requested PSCI_FEATURES for {:#x}", feature_id); + + if feature_id == PSCI_SYSTEM_OFF || feature_id == PSCI_SYSTEM_RESET { + context.regs[0] = 0; + } else { + context.regs[0] = 0xFFFF_FFFF; + } + true + } + _ => { + semihosting::println!("[EXIT] HVC#0: Ignored PSCI call: {:#x}", psci_func_id); + // We don't suuport this function. + context.regs[0] = 0xFFFF_FFFF; + true + } + } + } + HVC_VMM_GET_INFO=> { + context.regs[0] = 0x48495001; + true + } + HVC_GUEST_SHUTDOWN => { + unsafe { + semihosting::println!("[EXIT] HVC#20 Guest Shutdown..."); + need_advance_pc = false; + GUEST_SHUTDOWN = true; + } + + false + + } + _ => { + semihosting::println!("[EXIT] Unknown HVC Number"); + true + } + }; + + if result && need_advance_pc { + context.elr_el2 += 4; + } + + result +} + +fn handle_svc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { + let context = vcpu.context_mut(); + let svc_num = context.regs[0]; + semihosting::println!("[EXIT] SVC Number: {}", svc_num); + + match svc_num { + 0 => { + semihosting::println!("[EXIT] SVC#0: Hello from Guest via SVC!"); + context.regs[0] = 0; + } + 1 => { + semihosting::println!("[EXIT] SVC#1: Get Guest ID"); + context.regs[0] = 1; + } + _ => { + semihosting::println!("[EXIT] Unknown SVC Number"); + context.regs[0] = 0xFFFFFFFF; + } + } + + context.elr_el2 = info.return_addr as u64; + + true +} + +pub fn is_guest_shutdown() -> bool { + unsafe { GUEST_SHUTDOWN } +} + +pub fn clear_guest_shutdown() { + unsafe { GUEST_SHUTDOWN = false; } +} + +#[inline] +fn read_esr_el2() -> u64 { + let esr: u64; + unsafe { + asm!("mrs {}, esr_el2", out(reg) esr, options(nostack)); + } + esr +} + +#[inline] +fn read_elr_el2() -> usize { + let elr: usize; + unsafe { + asm!("mrs {}, elr_el2", out(reg) elr, options(nostack)); + } + elr +} + +#[inline] +fn read_spsr_el2() -> u64 { + let spsr: u64; + unsafe { + asm!("mrs {}, spsr_el2", out(reg) spsr, options(nostack)); + } + spsr +} + +#[cfg(test)] +mod tests { + use super::*; + use blueos_test_macro::test; + + #[test] + fn test_parse_exit_reason_exhaustive() { + assert_eq!(parse_exit_reason(0x16 << 26), VmExitReason::Hvc); + assert_eq!(parse_exit_reason(0x15 << 26), VmExitReason::Svc); + assert_eq!(parse_exit_reason(0x24 << 26), VmExitReason::DataAbortLowerEL); + assert_eq!(parse_exit_reason(0x20 << 26), VmExitReason::InstructionAbortLowerEL); + assert_eq!(parse_exit_reason(0x01 << 26), VmExitReason::TrappedWfiWfe); + } + + #[test] + fn test_handle_hvc_pc_increment_and_psci() { + let mut vcpu = Vcpu::new(0, 0x4000_0000, 0x4100_0000); + let initial_pc = 0x4000_1000; + + // Scenario 1: HVC #0x11 (Get Information), should resume execution and PC + 4 + vcpu.context_mut().elr_el2 = initial_pc; + let info_normal = VmExitInfo { + reason: VmExitReason::Hvc, + esr: (0x16 << 26) | 0x11, + far: 0, pstate: 0, return_addr: initial_pc as usize + 4, + }; + let should_resume = handle_hvc(&mut vcpu, &info_normal); + assert!(should_resume); + assert_eq!(vcpu.context().regs[0], 0x48495001, "Should set magic return value"); + assert_eq!(vcpu.context().elr_el2, initial_pc + 4, "PC MUST be incremented to avoid infinite loop!"); + + // Scenario 2: PSCI SYSTEM OFF (HVC #0, X0 = 0x84000008), should shut down and refuse recovery. + vcpu.context_mut().elr_el2 = initial_pc; + vcpu.context_mut().regs[0] = 0x84000008; + // EC = 0x16, ISS = 0x00 + let info_shutdown = VmExitInfo { + reason: VmExitReason::Hvc, + esr: (0x16 << 26), + far: 0, pstate: 0, return_addr: initial_pc as usize + 4, + }; + clear_guest_shutdown(); + let should_resume_shutdown = handle_hvc(&mut vcpu, &info_shutdown); + assert!(!should_resume_shutdown, "Should refuse to resume on PSCI Shutdown"); + assert!(is_guest_shutdown(), "Global shutdown flag must be set"); + assert_eq!(vcpu.context().elr_el2, initial_pc); + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/guest.rs b/kernel/src/arch/aarch64/virt/guest.rs new file mode 100644 index 00000000..dde4735d --- /dev/null +++ b/kernel/src/arch/aarch64/virt/guest.rs @@ -0,0 +1,29 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use core::arch::asm; + +pub const GUEST_CODE_LOAD_ADDR: usize = 0x4100_0000; +pub const GUEST_STACK_SIZE: usize = 32 * 1024; +pub const GUEST_STACK_TOP: usize = 0x4110_0000 - 16; +const GUEST_STACK_TOP_LO: u16 = (GUEST_STACK_TOP & 0xFFFF) as u16; +const GUEST_STACK_TOP_HI: u16 = ((GUEST_STACK_TOP >> 16) & 0xFFFF) as u16; +pub const LINUX_KERNEL_LOAD_ADDR: usize = 0x4400_0000; +pub const LINUX_DTB_ADDR: usize = 0x4E00_0000; + +/// Get guest stack top address. +#[inline] +pub fn guest_stack_top() -> usize { + GUEST_STACK_TOP +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/hyper.rs b/kernel/src/arch/aarch64/virt/hyper.rs index 4e1302c4..0c3fb3a3 100644 --- a/kernel/src/arch/aarch64/virt/hyper.rs +++ b/kernel/src/arch/aarch64/virt/hyper.rs @@ -12,7 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -use crate::arch::aarch64::{registers::hcr_el2::HCR_EL2, virt::vector}; +use crate::arch::aarch64::{ + registers::hcr_el2::HCR_EL2, + registers::sctlr_el2::SCTLR_EL2, + registers::spsr_el2::SPSR_EL2, + virt::vector, + virt::mmu_el2 +}; use tock_registers::interfaces::{Readable, Writeable}; #[inline] @@ -73,6 +79,32 @@ fn configure_hcr_el2() { HCR_EL2.write(HCR_EL2::RW::EL1AArch64); } +#[inline] +pub fn read_spsr_el2() -> u64 { + let spsr: u64; + unsafe { + core::arch::asm!("mrs {}, spsr_el2", out(reg) spsr); + } + spsr +} + +#[inline] +pub fn configure_hcr_el2_for_guest() { + // Identity map 128MB of RAM starting from 0x4400_0000 so the Guest can run in-place + super::mmu_s2::init_stage2(0x4400_0000, 0x1000_0000); + HCR_EL2.write( + HCR_EL2::VM::Enable + + HCR_EL2::RW::EL1AArch64 + + HCR_EL2::IMO::EL2Handled + + HCR_EL2::FMO::EL2Handled + + HCR_EL2::AMO::EL2Handled + +HCR_EL2::TSC::Trap + ); + unsafe { + core::arch::asm!("isb"); + } +} + #[inline] fn configure_vector_table(vector_base: usize) { unsafe { @@ -84,10 +116,58 @@ fn configure_vector_table(vector_base: usize) { } } +#[inline] +fn configure_sctlr_el2() { + SCTLR_EL2.write( + SCTLR_EL2::M::Enable + + SCTLR_EL2::C::Cacheable + + SCTLR_EL2::I::Cacheable + ); +} + +#[inline] +fn configure_timer_el2() { + // CNTHCTL_EL2: control register for EL2 access to the physical timer and counter registers + // Bit 0: EL1PCTEN (don't trap EL1 access to the physical counter) + // Bit 1: EL1PCEN (don't trap EL1 access to the physical timer) + let cnthctl: u64 = 0x3; + unsafe { + core::arch::asm!("msr CNTHCTL_EL2, {}", in(reg) cnthctl); + } + + // CNTVOFF_EL2: virtual timer offset register + let cntvoff: u64 = 0; + unsafe { + core::arch::asm!("msr CNTVOFF_EL2, {}", in(reg) cntvoff); + } +} + +#[inline] +pub fn shutdown_guest() { + HCR_EL2.write( + HCR_EL2::RW::EL1AArch64 + + HCR_EL2::SWIO::Set + ); + unsafe { + // Disable vGIC CPU interface to restore normal physical IRQ handling + let mut ich_hcr: u64; + core::arch::asm!( + "mrs {tmp}, ich_hcr_el2", + "bic {tmp}, {tmp}, #1", + "msr ich_hcr_el2, {tmp}", + "isb", + tmp = out(reg) ich_hcr, + options(nostack) + ); + } +} + // Hypervisor initialization #[cfg(virtualization)] pub fn hyp_init() { configure_hcr_el2(); + configure_timer_el2(); + mmu_el2::enable_el2_mmu(); unsafe { core::arch::asm!("dsb sy", options(nostack)); core::arch::asm!("isb sy", options(nostack)); @@ -107,3 +187,25 @@ pub fn hyp_init() { core::arch::asm!("isb sy", options(nostack)); } } + +// For GuestOS +#[inline] +pub fn enter_guest(entry: usize, dtb_addr: usize, pstate: u64) { + unsafe { + core::arch::asm!( + "msr elr_el2, {entry}", + "msr spsr_el2, {pstate}", + "mov x0, {dtb}", + "mov x1, xzr", + "mov x2, xzr", + "mov x3, xzr", + "dsb sy", + "isb sy", + "eret", + entry = in(reg) entry as u64, + pstate = in(reg) pstate, + dtb = in(reg) dtb_addr as u64, + options(noreturn) + ); + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/mmu_el2.rs b/kernel/src/arch/aarch64/virt/mmu_el2.rs new file mode 100644 index 00000000..593c620d --- /dev/null +++ b/kernel/src/arch/aarch64/virt/mmu_el2.rs @@ -0,0 +1,117 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +use tock_registers::{interfaces::*, register_bitfields, registers::InMemoryRegister}; +use crate::arch::aarch64::registers::{ + mair_el2::MAIR_EL2, + tcr_el2::TCR_EL2, + sctlr_el2::SCTLR_EL2, + ttbr0_el2::TTBR0_EL2, +}; + +register_bitfields! {u64, + pub PAGE_DESCRIPTOR_EL2 [ + XN OFFSET(54) NUMBITS(1) [ False = 0, True = 1 ], + OUTPUT_ADDR OFFSET(12) NUMBITS(36) [], + AF OFFSET(10) NUMBITS(1) [ False = 0, True = 1 ], + SH OFFSET(8) NUMBITS(2) [ + NonShareable = 0b00, + OuterShareable = 0b10, + InnerShareable = 0b11 + ], + AP OFFSET(6) NUMBITS(2) [ RW = 0b00, RO = 0b10 ], + ATTRINDX OFFSET(2) NUMBITS(3) [], + TYPE OFFSET(1) NUMBITS(1) [ Block = 0, Table = 1 ], + VALID OFFSET(0) NUMBITS(1) [ Invalid = 0, Valid = 1 ] + ] +} + +#[derive(Copy, Clone)] +#[repr(transparent)] +pub struct PageEntry(u64); + +impl PageEntry { + const fn new() -> Self { Self(0) } + + fn set(&mut self, output_addr: u64, device: bool) { + let entry = InMemoryRegister::::new(0); + let mut val = PAGE_DESCRIPTOR_EL2::VALID::Valid + + PAGE_DESCRIPTOR_EL2::AF::True + + PAGE_DESCRIPTOR_EL2::TYPE::Block; + if device { + val += PAGE_DESCRIPTOR_EL2::ATTRINDX.val(0) + + PAGE_DESCRIPTOR_EL2::XN::True; + } else { + val += PAGE_DESCRIPTOR_EL2::ATTRINDX.val(1) + + PAGE_DESCRIPTOR_EL2::SH::InnerShareable; + } + entry.write(val); + self.0 = entry.get() | (output_addr & 0xFFFF_FFFF_C000_0000); + } +} + +#[used] +static mut EL2_PAGE_TABLE: El2PageTable = El2PageTable::new(); + +#[repr(C, align(4096))] +pub struct El2PageTable([PageEntry; 512]); + +impl El2PageTable { + const fn new() -> Self { + El2PageTable([PageEntry::new(); 512]) + } +} + +pub fn enable_el2_mmu() { + unsafe { + EL2_PAGE_TABLE.0[0].set(0x0, true); // 0~1G: Device + EL2_PAGE_TABLE.0[1].set(0x4000_0000, false); // 1~2G: Normal + } + + // MAIR_EL2: Attr0=Device nGnRE, Attr1=Normal WB + MAIR_EL2.write( + MAIR_EL2::Attr0_Device::NonGathering_NonReordering_EarlyWriteAck + + MAIR_EL2::Attr1_Normal_Outer::WriteBack_NonTransient_ReadWriteAlloc + + MAIR_EL2::Attr1_Normal_Inner::WriteBack_NonTransient_ReadWriteAlloc, + ); + + unsafe { + let ttbr = &EL2_PAGE_TABLE as *const _ as u64; + TTBR0_EL2.set(ttbr); + } + + TCR_EL2.write( + TCR_EL2::T0SZ.val(25) + + TCR_EL2::TG0::KiB_4 + + TCR_EL2::SH0::Inner + + TCR_EL2::ORGN0::WriteBack_ReadAlloc_NoWriteAlloc_Cacheable + + TCR_EL2::IRGN0::WriteBack_ReadAlloc_NoWriteAlloc_Cacheable + + TCR_EL2::PS::Bits_32, + ); + + unsafe { + core::arch::asm!("dsb sy", options(nostack, nomem)); + core::arch::asm!("tlbi alle2", options(nostack, nomem)); + core::arch::asm!("dsb sy", options(nostack, nomem)); + core::arch::asm!("isb sy", options(nostack, nomem)); + } + + SCTLR_EL2.modify( + SCTLR_EL2::M::Enable + + SCTLR_EL2::C::Cacheable + + SCTLR_EL2::I::Cacheable, + ); + unsafe { core::arch::asm!("isb sy", options(nostack, nomem)); } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/mmu_s2.rs b/kernel/src/arch/aarch64/virt/mmu_s2.rs new file mode 100644 index 00000000..de8d723a --- /dev/null +++ b/kernel/src/arch/aarch64/virt/mmu_s2.rs @@ -0,0 +1,187 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +use crate::arch::aarch64::{ + registers::{ + vtcr_el2::VTCR_EL2, + vttbr_el2::VTTBR_EL2, + }, +}; +use tock_registers::interfaces::*; +use semihosting::println; + +// Structure of Page Table. +#[repr(align(4096))] +#[derive(Clone, Copy)] +struct S2PageTable([u64; 512]); + +const POOL_SIZE: usize = 256; +static mut PAGE_TABLE_POOL: [S2PageTable; POOL_SIZE] = [S2PageTable([0; 512]); POOL_SIZE]; +static mut POOL_INDEX: usize = 0; +static mut S2_L1: S2PageTable = S2PageTable([0; 512]); + +// Stage-2 Descriptor +const S2_DESC_TABLE: u64 = 3; +const S2_DESC_PAGE: u64 = 3; +const S2_ATTR_NORMAL: u64 = 0xF << 2; +const S2_ATTR_DEVICE: u64 = 1 << 2; +const S2_ATTR_S2AP_RW: u64 = 3 << 6; +const S2_ATTR_SH_INNER: u64 = 3 << 8; +const S2_ATTR_AF: u64 = 1 << 10; + +fn alloc_page_table() -> Option<&'static mut S2PageTable> { + unsafe { + if POOL_INDEX >= POOL_SIZE { return None; } + let table = &mut PAGE_TABLE_POOL[POOL_INDEX]; + POOL_INDEX += 1; + for e in table.0.iter_mut() { *e = 0; } + let start = table as *const _ as usize; + for addr in (start..start + core::mem::size_of::()).step_by(64) { + core::arch::asm!("dc civac, {}", in(reg) addr); + } + core::arch::asm!("dsb sy", options(nostack, nomem)); + Some(table) + } +} + +fn map_page(ipa: usize, pa: usize, device: bool) { + let l1_idx = (ipa >> 30) & 0x1ff; + let l2_idx = (ipa >> 21) & 0x1ff; + let l3_idx = (ipa >> 12) & 0x1ff; + + unsafe { + // L1 → L2 + let l2_table = { + let e = S2_L1.0[l1_idx]; + if (e & 3) == S2_DESC_TABLE { + &mut *((e & 0xFFFFFFFFF000) as *mut S2PageTable) + } else { + let t = alloc_page_table().expect("S2 L2 OOM"); + S2_L1.0[l1_idx] = t as *const _ as u64 | S2_DESC_TABLE; + t + } + }; + + // L2 → L3 + let l3_table = { + let e = l2_table.0[l2_idx]; + if (e & 3) == S2_DESC_TABLE { + &mut *((e & 0xFFFFFFFFF000) as *mut S2PageTable) + } else { + let t = alloc_page_table().expect("S2 L3 OOM"); + l2_table.0[l2_idx] = t as *const _ as u64 | S2_DESC_TABLE; + t + } + }; + + let attr = S2_ATTR_S2AP_RW | S2_ATTR_AF | S2_ATTR_SH_INNER + | if device { S2_ATTR_DEVICE } else { S2_ATTR_NORMAL }; + l3_table.0[l3_idx] = (pa as u64 & 0xFFFFFFFFF000) | attr | S2_DESC_PAGE; + + let addr = &l3_table.0[l3_idx] as *const _ as usize; + core::arch::asm!("dc civac, {}", in(reg) addr); + core::arch::asm!("dsb sy", options(nostack, nomem)); + } +} + +pub fn map_range(ipa: usize, pa: usize, size: usize, device: bool) { + let mut offset = 0; + while offset < size { + map_page(ipa + offset, pa + offset, device); + offset += 4096; + } +} + +pub fn init_stage2(ipa_base: usize, size: usize) { + unsafe { POOL_INDEX = 0; } + + map_range(ipa_base, ipa_base, size, false); + // for guest visting uart + map_range(0x0900_0000, 0x0900_0000, 4096, true); + // for vGICR and vGICD + // map_range(0x0800_0000, 0x0800_0000, 0x0001_0000, true); + // map_range(0x080A_0000, 0x080A_0000, 0x00F6_0000, true); + + semihosting::println!("[S2MMU] init_stage2 done."); + + unsafe { + core::arch::asm!("dsb sy", options(nostack, nomem)); + let start = &S2_L1 as *const _ as usize; + for addr in (start..start + core::mem::size_of::()).step_by(32) { + core::arch::asm!("dc civac, {}", in(reg) addr); + } + let pool_start = &PAGE_TABLE_POOL as *const _ as usize; + let pool_end = pool_start + core::mem::size_of_val(&PAGE_TABLE_POOL); + for addr in (pool_start..pool_end).step_by(32) { + core::arch::asm!("dc civac, {}", in(reg) addr); + } + core::arch::asm!("dsb sy", options(nostack, nomem)); + } + + VTCR_EL2.write( + VTCR_EL2::PS::PA_32B_4GB + + VTCR_EL2::TG0::Granule4KB + + VTCR_EL2::SH0::Inner + + VTCR_EL2::ORGN0::NormalWBRAWA + + VTCR_EL2::IRGN0::NormalWBRAWA + + VTCR_EL2::SL0.val(1) + + VTCR_EL2::T0SZ.val(32), + ); + + let vttbr = unsafe { &S2_L1 as *const _ as u64 } & !0xfff; + VTTBR_EL2.set(vttbr); + + unsafe { + core::arch::asm!("dsb sy", options(nostack, nomem)); + core::arch::asm!("tlbi vmalls12e1", options(nostack, nomem)); + core::arch::asm!("dsb sy", options(nostack, nomem)); + core::arch::asm!("isb sy", options(nostack, nomem)); + } +} + +#[cfg(test)] +mod tests { + use super::*; + use blueos_test_macro::test; + + #[test] + fn test_page_table_allocation() { + unsafe { POOL_INDEX = 0; } + + let table1 = alloc_page_table(); + assert!(table1.is_some(), "Should allocate first table"); + + unsafe { + let idx = POOL_INDEX; + assert_eq!(idx, 1); + } + } + + #[test] + fn test_stage2_attributes_generation() { + // Normal Memory should be 0xF << 2 (Inner & Outer WB Cacheable) + // Device Memory should be 0x1 << 2 (Device-nGnRE) + + let attr_normal = S2_ATTR_S2AP_RW | S2_ATTR_AF | S2_ATTR_SH_INNER | S2_ATTR_NORMAL; + let attr_device = S2_ATTR_S2AP_RW | S2_ATTR_AF | S2_ATTR_SH_INNER | S2_ATTR_DEVICE; + + // Extract MemAttr field (Bits[5:2]) + let mem_attr_normal = (attr_normal >> 2) & 0xF; + let mem_attr_device = (attr_device >> 2) & 0xF; + + assert_eq!(mem_attr_normal, 0xF, "Normal memory attribute must be 0b1111 to prevent Alignment Faults"); + assert_eq!(mem_attr_device, 0x1, "Device memory attribute must be 0b0001"); + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index 9e0f694b..58a246b0 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -12,13 +12,98 @@ // See the License for the specific language governing permissions and // limitations under the License. +pub mod exit; +pub mod guest; +pub mod mmu_el2; +pub mod mmu_s2; pub mod hyper; +pub mod vcpu; pub mod vector; +pub mod vgic; +pub mod vtimer; +pub use exit::{VmExitReason, VmExitInfo}; pub use hyper::{get_current_el, hyp_init}; +pub use vcpu::{Vcpu, VcpuManager, VcpuState}; +pub use vgic::init; +pub use crate::arch::aarch64::psci::hvc_call; +use semihosting::println; + +// PL011 UART addresses for QEMU Virt +const UART0_DR: *mut u32 = 0x0900_0000 as *mut u32; +const UART0_FR: *mut u32 = 0x0900_0018 as *mut u32; // Temporary placeholder #[no_mangle] pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) -> usize { + unsafe{ + let mut ctlr: u64; + core::arch::asm!("mrs {}, ICC_CTLR_EL1", out(reg) ctlr); + if (ctlr & (1 << 1)) == 0{ + // set EOImode. + ctlr |= 1 << 1; + core::arch::asm!("msr ICC_CTLR_EL1, {}", in(reg) ctlr); + } + } + let iar: u64; + unsafe { + core::arch::asm!("mrs {}, ICC_IAR1_EL1", out(reg) iar); + } + + let intid = iar & 0xFFFFFF; + + if intid == 1023 { + // 1023 is Spurious Interrupt of GIC，skip + return 0; + } + + if intid != 27 { + semihosting::println!("[EL2] IRQ trap! INTID: {}", intid); + } + if intid == 33 { + unsafe { + let lr_val: u64 = (1 << 62) | (1 << 61) | (1 << 60) | (0xA0 << 48) | (33 << 32) | 33; + + // Temporarily occupy physical register for uart print in Linux shell + core::arch::asm!("msr ICH_LR1_EL2, {}", in(reg) lr_val); + let mut hcr: u64; + core::arch::asm!("mrs {}, ICH_HCR_EL2", out(reg) hcr); + hcr |= 1; + core::arch::asm!("msr ICH_HCR_EL2, {}", in(reg) hcr); + } + } else if intid == 27 { + unsafe { + let mut ctl: u64; + core::arch::asm!("mrs {}, CNTV_CTL_EL0", out(reg) ctl); + ctl |= 1 << 1; + core::arch::asm!("msr CNTV_CTL_EL0, {}", in(reg) ctl); + } + + if let Some(vcpu_id) = get_current_vcpu_id() { + vgic::inject_irq(vcpu_id, 27); + } + + unsafe{ + core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + } + } else { + semihosting::println!("[EL2] Unhandled Guest IRQ: {}", intid); + // For uninterruptible/unknown interrupts, + // we must manually downgrade and deactivate them; + // otherwise, the interrupt line will be permanently blocked. + unsafe { + core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); + } + } + + unsafe { + core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + } + + if let Some(vcpu_id) = get_current_vcpu_id() { + vgic::flush(vcpu_id); + } + 0 } @@ -27,6 +112,34 @@ pub extern "C" fn hyper_trap_fiq(_context: &mut crate::arch::aarch64::Context) - 0 } +#[repr(align(16))] +pub struct VcpuManagerWrapper(pub vcpu::VcpuManager); + +pub static mut VCPU_MANAGER: VcpuManagerWrapper = + VcpuManagerWrapper(vcpu::VcpuManager::new()); + +#[inline] +pub fn get_current_vcpu_id() -> Option { + unsafe { VCPU_MANAGER.0.current_vcpu_id() } +} + pub fn virt_init() { hyp_init(); } + +pub fn virt_boot_linux() { + // It will be placed here next. + vgic::init(); + vtimer::init_global_vtimer(); + + unsafe { + let vcpu = VCPU_MANAGER.0.create_vcpu(0, guest::LINUX_KERNEL_LOAD_ADDR, 0).unwrap(); + let mut ctx = vcpu.context_mut(); + ctx.regs[0] = guest::LINUX_DTB_ADDR as u64; + ctx.spsr = 0x3C5; + ctx.sctlr_el1 = 0x30d00800; + } + + vtimer::init_vcpu_timer(); + let result = hvc_call(2, 0, 0); +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs new file mode 100644 index 00000000..708c3ee1 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -0,0 +1,422 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use core::arch::asm; +use super::{ + hyper::{read_hcr_el2, write_hcr_el2}, vgic +}; + +/// HCR_EL2_VI: Enable virtual IRQ. +const HCR_EL2_VI: u64 = 1 << 7; +/// HCR_EL2_VF: Enable virtual FIQ. +const HCR_EL2_VF: u64 = 1 << 6; + +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum VcpuState { + Stopped, + Running, + Paused, + Exited, +} + +/// Using #[repr(C)] to ensure compatibility with ARM AAPCS64 calling convention, +/// using #[repr(align(16))] to ensure 16-byte alignment, satisfying SIMD register alignment requirements +#[derive(Debug, Default)] +#[repr(C)] +#[repr(align(16))] +pub struct VcpuStateStruct { + pub regs: [u64; 31], + pub elr_el2: u64, + pub sp: u64, + pub pstate: u64, + pub spsr: u64, + pub vbar_el1: u64, + pub sctlr_el1: u64, + pub ttbr0_el1: u64, + pub ttbr1_el1: u64, + pub tcr_el1: u64, + pub mair_el1: u64, + pub elr_el1: u64, + pub spsr_el1: u64, + pub sp_el0: u64, + pub tpidr_el0: u64, + pub tpidr_el1: u64, + pub esr_el1: u64, + pub far_el1: u64, +} + +impl VcpuStateStruct { + #[inline] + pub fn new() -> Self { + Self::default() + } + + #[inline] + pub fn is_valid(&self) -> bool { + self.elr_el2 != 0 && self.sp != 0 + } + + #[inline] + pub fn reset(&mut self) { + self.regs = [0; 31]; + self.elr_el2 = 0; + self.sp = 0; + self.pstate = 0; + self.spsr = 0; + self.vbar_el1 = 0; + } + + #[inline] + pub fn elr(&self) -> u64 { + self.elr_el2 + } + + #[inline] + pub fn set_elr(&mut self, elr: u64) { + self.elr_el2 = elr; + } + + #[inline] + pub fn spsr(&self) -> u64 { + self.spsr + } + + #[inline] + pub fn set_spsr(&mut self, spsr: u64) { + self.spsr = spsr; + } +} + +#[repr(align(16))] +pub struct Vcpu { + id: usize, + state: VcpuState, + context: VcpuStateStruct, + entry: usize, + stack_top: usize, + pending_irq: bool, + pending_fiq: bool, +} + +impl Vcpu { + #[inline] + pub fn new(id: usize, entry: usize, stack_top: usize) -> Self { + let mut context = VcpuStateStruct::new(); + context.elr_el2 = entry as u64; + context.spsr = 0x3C5; // Default PSTATE: EL1h, DAIF masked + context.vbar_el1 = (entry as u64 + 0x1000) & !0x7FF; + // context.sp = stack_top as u64; + + Self { + id, + state: VcpuState::Stopped, + context, + entry, + stack_top, + pending_irq: false, + pending_fiq: false, + } + } + + #[inline] + pub fn id(&self) -> usize { + self.id + } + + #[inline] + pub fn entry_point(&self) -> usize { + self.entry + } + + #[inline] + pub fn stack_top(&self) -> usize { + self.stack_top + } + + #[inline] + pub fn state(&self) -> VcpuState { + self.state + } + + #[inline] + pub fn context(&self) -> &VcpuStateStruct { + &self.context + } + + #[inline] + pub fn context_mut(&mut self) -> &mut VcpuStateStruct { + &mut self.context + } + + #[inline] + pub fn pending_irq(&self) -> bool { + self.pending_irq + } + + #[inline] + pub fn pending_fiq(&self) -> bool { + self.pending_fiq + } + + #[inline] + pub fn set_pending_irq(&mut self, val: bool) { + self.pending_irq = val; + } + + #[inline] + pub fn set_pending_fiq(&mut self, val: bool) { + self.pending_fiq = val; + } + + #[inline] + pub fn elr(&self) -> u64 { + self.context.elr_el2 + } + + #[inline] + pub fn set_entry(&mut self, entry: usize) { + self.entry = entry; + } + + #[inline] + pub fn set_stack_top(&mut self, stack_top: usize) { + self.stack_top = stack_top; + } + + #[inline] + pub fn set_state(&mut self, state: VcpuState) { + self.state = state; + } + + pub fn prepare_run(&mut self) { + if self.state == VcpuState::Stopped { + #[cfg(not(test))] + vgic::cpu_init(self.id); + } + self.state = VcpuState::Running; + } + + pub fn inject_irq(&mut self) { + self.pending_irq = true; + let hcr = read_hcr_el2(); + write_hcr_el2(hcr | HCR_EL2_VI); + + unsafe{ + core::arch::asm!("isb", options(nomem, nostack)); + } + } + + pub fn inject_fiq(&mut self) { + self.pending_fiq = true; + let hcr = read_hcr_el2(); + write_hcr_el2(hcr | HCR_EL2_VF); + + unsafe{ + core::arch::asm!("isb", options(nomem, nostack)); + } + } + + #[inline] + pub fn can_run(&self) -> bool { + self.state == VcpuState::Stopped || + self.state == VcpuState::Paused || + self.state == VcpuState::Exited + } +} + +#[repr(align(16))] +pub struct VcpuManager { + vcpus: [Option; 4], + count: usize, + current_vcpu: Option, + // Store host context, we change elr_el2 to make "eret" return guest. + pub host_elr: u64, + pub host_sp: u64, + pub host_spsr: u64, + pub host_regs: [u64; 31], + pub host_vbar: u64, + pub host_ttbr0: u64, + pub host_ttbr1: u64, + pub host_tcr: u64, + pub host_mair: u64, + pub host_sctlr: u64, +} + +impl VcpuManager { + #[inline] + pub const fn new() -> Self { + Self { + vcpus: [None, None, None, None], + count: 0, + current_vcpu: None, + host_elr: 0, + host_sp: 0, + host_spsr: 0, + host_regs: [0; 31], + host_vbar: 0, + host_ttbr0: 0, + host_ttbr1: 0, + host_tcr: 0, + host_mair: 0, + host_sctlr: 0, + } + } + + pub fn create_vcpu( + &mut self, + id: usize, + entry: usize, + stack_top: usize + ) -> Result<&mut Vcpu, &'static str> { + if id >= 4 { + return Err("vCPU ID out of range (max 3)"); + } + + // Temporary check for max vCPU count + if self.count >= 4 { + return Err("Reached max vCPU count"); + } + + if self.vcpus[id].is_some() { + return Err("vCPU ID already used"); + } + + let vcpu = Vcpu::new(id, entry, stack_top); + self.vcpus[id] = Some(vcpu); + self.count += 1; + + Ok(self.vcpus[id].as_mut().unwrap()) + } + + pub fn clear_current_vcpu(&mut self) { + self.current_vcpu = None; + } + + #[inline] + pub fn get_vcpu(&mut self, id: usize) -> Option<&mut Vcpu> { + self.vcpus[id].as_mut() + } + + #[inline] + pub fn current_vcpu_id(&self) -> Option { + self.current_vcpu + } + + #[inline] + pub fn set_current_vcpu(&mut self, id: usize) { + self.current_vcpu = Some(id); + } + + #[inline] + pub fn vcpu_count(&self) -> usize { + self.count + } + + #[inline] + pub fn has_running(&self) -> bool { + self.current_vcpu.is_some() + } + + pub fn iter(&mut self) -> impl Iterator { + self.vcpus + .iter_mut() + .enumerate() + .filter_map(|(id, vcpu)| vcpu.as_mut().map(|v| (id, v))) + } +} + +#[derive(Debug)] +pub enum VcpuError { + IdOutOfRange, + MaxLimitReached, + IdAlreadyUsed, + NotFound, + InvalidState, +} + +impl core::fmt::Display for VcpuError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + VcpuError::IdOutOfRange => + write!(f, "vCPU ID out of range (max 3)"), + VcpuError::MaxLimitReached => + write!(f, "Reached max vCPU count"), + VcpuError::IdAlreadyUsed => + write!(f, "vCPU ID already used"), + VcpuError::NotFound => + write!(f, "vCPU not found"), + VcpuError::InvalidState => + write!(f, "vCPU state invalid for this operation"), + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use blueos_test_macro::test; + + #[test] + fn test_vcpu_state_struct_lifecycle() { + let mut state = VcpuStateStruct::new(); + assert!(!state.is_valid()); + + state.set_elr(0x4000_0000); + state.sp = 0x4100_0000; + assert!(state.is_valid()); + + state.set_spsr(0x3C5); + assert_eq!(state.spsr(), 0x3C5); + + state.reset(); + assert_eq!(state.elr(), 0); + assert!(!state.is_valid()); + } + + #[test] + fn test_vcpu_manager_allocation() { + let mut manager = VcpuManager::new(); + assert_eq!(manager.vcpu_count(), 0); + assert!(!manager.has_running()); + + let vcpu0 = manager.create_vcpu(0, 0x4000_0000, 0x4100_0000).expect("Failed to create vCPU 0"); + assert_eq!(vcpu0.id(), 0); + assert_eq!(vcpu0.entry_point(), 0x4000_0000); + assert_eq!(vcpu0.state(), VcpuState::Stopped); + + let err_duplicate = manager.create_vcpu(0, 0x4000_0000, 0x4100_0000); + assert!(err_duplicate.is_err(), "Should not allow duplicate vCPU ID"); + + let err_out_of_bounds = manager.create_vcpu(4, 0x4000_0000, 0x4100_0000); + assert!(err_out_of_bounds.is_err(), "Should reject ID >= MAX_VCPUS"); + + manager.create_vcpu(1, 0x4000_0000, 0x4100_0000).unwrap(); + manager.create_vcpu(2, 0x4000_0000, 0x4100_0000).unwrap(); + manager.create_vcpu(3, 0x4000_0000, 0x4100_0000).unwrap(); + assert_eq!(manager.vcpu_count(), 4); + } + + #[test] + fn test_vcpu_context_switch_preparation() { + let mut manager = VcpuManager::new(); + manager.create_vcpu(0, 0x4000_0000, 0x4100_0000).unwrap(); + + let vcpu = manager.get_vcpu(0).unwrap(); + assert!(vcpu.can_run()); + + vcpu.prepare_run(); + assert_eq!(vcpu.state(), VcpuState::Running); + assert!(!vcpu.can_run(), "Running vCPU should not be marked as can_run to prevent re-entry"); + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index a7aca83e..8afc61db 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -12,7 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -use super::hyper; +use super::{ + VCPU_MANAGER, + guest, + hyper, + vcpu::Vcpu, + vgic, + exit::{ + handle_vm_exit, is_guest_shutdown, clear_guest_shutdown + } +}; use core::arch::asm; static mut PRINTED_ALIGN: bool = false; @@ -105,7 +114,11 @@ pub unsafe extern "C" fn sync_from_lower_el1() { "str x3, [sp, #264]\n", "mov x0, sp\n", "bl sync_from_lower_el1_rust\n", - "cbz x0, 1f\n", + "cbz x0, 3f\n", + // x0 == 2, Guest shutdown, return to Host. + "cmp x0, #2\n", + "b.eq 2f\n", + // x0 == 1, continue running Guest. "ldr x1, [sp, #248]\n", "ldr x2, [sp, #256]\n", "ldr x3, [sp, #264]\n", @@ -131,43 +144,228 @@ pub unsafe extern "C" fn sync_from_lower_el1() { "ldr x30, [sp, #240]\n", "add sp, sp, #272\n", "eret\n", - "1:\n", + "2:\n", + "ldr x1, [sp, #248]\n", // Host ELR + "ldr x2, [sp, #256]\n", // Host SPSR + "ldr x3, [sp, #264]\n", // Host SP_EL1 + "msr elr_el2, x1\n", + "msr spsr_el2, x2\n", + "msr sp_el1, x3\n", + "isb\n", + "ldp x0, x1, [sp, #0]\n", + "ldp x2, x3, [sp, #16]\n", + "ldp x4, x5, [sp, #32]\n", + "ldp x6, x7, [sp, #48]\n", + "ldp x8, x9, [sp, #64]\n", + "ldp x10, x11, [sp, #80]\n", + "ldp x12, x13, [sp, #96]\n", + "ldp x14, x15, [sp, #112]\n", + "ldp x16, x17, [sp, #128]\n", + "ldp x18, x19, [sp, #144]\n", + "ldp x20, x21, [sp, #160]\n", + "ldp x22, x23, [sp, #176]\n", + "ldp x24, x25, [sp, #192]\n", + "ldp x26, x27, [sp, #208]\n", + "ldp x28, x29, [sp, #224]\n", + "ldr x30, [sp, #240]\n", + "add sp, sp, #272\n", + "eret\n", + "3:\n", "wfi\n", - "b 1b\n" + "b 3b\n" ); } #[no_mangle] pub unsafe extern "C" fn sync_from_lower_el1_rust(frame: *mut u64) -> u64 { - let esr: u64; - let elr: u64; - asm!("mrs {}, esr_el2", out(reg) esr, options(nostack)); - asm!("mrs {}, elr_el2", out(reg) elr, options(nostack)); - let ec = (esr >> 26) & 0x3F; + if let Some(vcpu_id) = VCPU_MANAGER.0.current_vcpu_id() { + handle_guest_request(vcpu_id, frame) + } else { + handle_host_request(frame) + } +} - // EC = 0x16 (HVC64) - if ec == 0x16 { - let func_id = *frame.add(0); +unsafe fn handle_guest_request(vcpu_id: usize, frame: *mut u64) -> u64 { + let vcpu = VCPU_MANAGER.0.get_vcpu(vcpu_id).unwrap(); + save_frame_to_context(frame, vcpu); + let ok = handle_vm_exit(vcpu); - match func_id { - 0x00 => { - let el = hyper::get_current_el(); - core::ptr::write_volatile(frame.add(0), 0u64); - } - _ => { - panic!("[EL2] Unknown Host HVC:{} ", func_id); + if !ok && is_guest_shutdown() { + core::arch::asm!("msr daifset, #15"); + clear_guest_shutdown(); + hyper::shutdown_guest(); + VCPU_MANAGER.0.clear_current_vcpu(); + restore_host_to_frame(frame); + 2 + } else { + restore_context_to_frame(vcpu, frame); + vgic::flush(vcpu_id); + 1 + } +} + +unsafe fn handle_host_request(frame: *mut u64) -> u64 { + let func_id = *frame.add(0); + + match func_id { + 0x02 => { + let target_id = *frame.add(1) as usize; + if let Some(vcpu) = VCPU_MANAGER.0.get_vcpu(target_id) { + save_host_context(frame); + hyper::configure_hcr_el2_for_guest(); + vcpu.prepare_run(); + VCPU_MANAGER.0.set_current_vcpu(target_id); + restore_context_to_frame(vcpu, frame); + 1 + } else { + *frame.add(0) = 1; + 1 } } - return 1; // Resume + _ => { + *frame.add(0) = 0; + 1 + } + } +} + +unsafe fn save_host_context(frame: *mut u64) { + VCPU_MANAGER.0.host_elr = *frame.add(31); + VCPU_MANAGER.0.host_spsr = *frame.add(32); + VCPU_MANAGER.0.host_sp = *frame.add(33); + semihosting::println!("host_elr: {:x}, host_spsr: {:x}, host_sp: {:x}", VCPU_MANAGER.0.host_elr, VCPU_MANAGER.0.host_spsr, VCPU_MANAGER.0.host_sp); + for i in 0..31 { + VCPU_MANAGER.0.host_regs[i] = *frame.add(i); + } + + let (vbar, sctlr, ttbr0, ttbr1, tcr, mair): (u64, u64, u64, u64, u64, u64); + core::arch::asm!( + "mrs {vbar}, vbar_el1", + "mrs {sctlr}, sctlr_el1", + "mrs {ttbr0}, ttbr0_el1", + "mrs {ttbr1}, ttbr1_el1", + "mrs {tcr}, tcr_el1", + "mrs {mair}, mair_el1", + vbar = out(reg) vbar, + sctlr = out(reg) sctlr, + ttbr0 = out(reg) ttbr0, + ttbr1 = out(reg) ttbr1, + tcr = out(reg) tcr, + mair = out(reg) mair, + options(nostack, nomem) + ); + VCPU_MANAGER.0.host_vbar = vbar; + VCPU_MANAGER.0.host_sctlr = sctlr; + VCPU_MANAGER.0.host_ttbr0 = ttbr0; + VCPU_MANAGER.0.host_ttbr1 = ttbr1; + VCPU_MANAGER.0.host_tcr = tcr; + VCPU_MANAGER.0.host_mair = mair; +} + +unsafe fn restore_host_to_frame(frame: *mut u64) { + *frame.add(31) = VCPU_MANAGER.0.host_elr; + *frame.add(32) = VCPU_MANAGER.0.host_spsr; + *frame.add(33) = VCPU_MANAGER.0.host_sp; + semihosting::println!("host_elr: {:x}, host_spsr: {:x}, host_sp: {:x}", VCPU_MANAGER.0.host_elr, VCPU_MANAGER.0.host_spsr, VCPU_MANAGER.0.host_sp); + // Restore Host GPRs (x0-x30) + for i in 0..31 { + *frame.add(i) = VCPU_MANAGER.0.host_regs[i]; } + + let vbar = VCPU_MANAGER.0.host_vbar; + let sctlr = VCPU_MANAGER.0.host_sctlr; + let ttbr0 = VCPU_MANAGER.0.host_ttbr0; + let ttbr1 = VCPU_MANAGER.0.host_ttbr1; + let tcr = VCPU_MANAGER.0.host_tcr; + let mair = VCPU_MANAGER.0.host_mair; + + core::arch::asm!( + "msr vbar_el1, {vbar}", + "msr sctlr_el1, {sctlr}", + "msr ttbr0_el1, {ttbr0}", + "msr ttbr1_el1, {ttbr1}", + "msr tcr_el1, {tcr}", + "msr mair_el1, {mair}", + "isb", + "tlbi alle1", + "dsb sy", + "isb", + vbar = in(reg) vbar, + sctlr = in(reg) sctlr, + ttbr0 = in(reg) ttbr0, + ttbr1 = in(reg) ttbr1, + tcr = in(reg) tcr, + mair = in(reg) mair, + ); +} - // EC = 0x07 (Access to SIMD/FP) - if ec == 0x07 { - asm!("msr cptr_el2, xzr"); - return 1; +unsafe fn save_frame_to_context(frame: *mut u64, vcpu: &mut Vcpu) { + let mut ctx = vcpu.context_mut(); + let raw_x4 = *frame.add(4); + for i in 0..31 { + ctx.regs[i] = *frame.add(i); } + if ctx.regs[4] != raw_x4 { + semihosting::println!("[ALARM] Memory corruption! ctx.regs[4] expected {:x}, got {:x}", raw_x4, ctx.regs[4]); + } + ctx.elr_el2 = *frame.add(31); + ctx.spsr = *frame.add(32); + ctx.sp = *frame.add(33); + let (sctlr, ttbr0, ttbr1, tcr, mair, vbar): (u64, u64, u64, u64, u64, u64); + core::arch::asm!( + "mrs {sctlr}, sctlr_el1", + "mrs {ttbr0}, ttbr0_el1", + "mrs {ttbr1}, ttbr1_el1", + "mrs {tcr}, tcr_el1", + "mrs {mair}, mair_el1", + "mrs {vbar}, vbar_el1", + sctlr = out(reg) sctlr, + ttbr0 = out(reg) ttbr0, + ttbr1 = out(reg) ttbr1, + tcr = out(reg) tcr, + mair = out(reg) mair, + vbar = out(reg) vbar, + options(nostack, nomem) + ); + + ctx.sctlr_el1 = sctlr; + ctx.ttbr0_el1 = ttbr0; + ctx.ttbr1_el1 = ttbr1; + ctx.tcr_el1 = tcr; + ctx.mair_el1 = mair; + ctx.vbar_el1 = vbar; +} + +unsafe fn restore_context_to_frame(vcpu: &mut Vcpu, frame: *mut u64) { + let ctx = vcpu.context(); + for i in 0..31 { *frame.add(i) = ctx.regs[i]; } + *frame.add(31) = ctx.elr_el2; + *frame.add(32) = ctx.spsr; + *frame.add(33) = ctx.sp; - 0 + // while booting linux, mmu should closed. + core::arch::asm!( + "msr vbar_el1, {vbar}", + "msr ttbr0_el1, {ttbr0}", + "msr ttbr1_el1, {ttbr1}", + "msr tcr_el1, {tcr}", + "msr mair_el1, {mair}", + "msr sctlr_el1, {sctlr}", + "isb", + "tlbi alle1", + "dsb sy", + "isb", + vbar = in(reg) ctx.vbar_el1, + ttbr0 = in(reg) ctx.ttbr0_el1, + ttbr1 = in(reg) ctx.ttbr1_el1, + tcr = in(reg) ctx.tcr_el1, + mair = in(reg) ctx.mair_el1, + sctlr = in(reg) ctx.sctlr_el1, + options(nostack) + ); + + let target_vcpu_id = vcpu.id(); + vgic::flush(target_vcpu_id); } // Temporary placeholder diff --git a/kernel/src/arch/aarch64/virt/vgic.rs b/kernel/src/arch/aarch64/virt/vgic.rs new file mode 100644 index 00000000..f81a7c77 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vgic.rs @@ -0,0 +1,561 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use core::arch::asm; +use super::VCPU_MANAGER; +use crate::sync::SpinLock; +use spin::Once; + +const MAX_LR: usize = 4; +const MAX_PENDING: usize = 64; +const MAX_VCPUS: usize = 4; +const MAX_IRQS_WORDS: usize = 32; + +// Qemu default base address +const VGICD_BASE: u64 = 0x0800_0000; +const VGICD_SIZE: u64 = 0x0001_0000; +const VGICR_BASE: u64 = 0x080A_0000; +const VGICR_SIZE: u64 = 0x00F6_0000; +const GICR_VCPU_SIZE: u64 = 0x20000; + +#[derive(Debug, Copy, Clone)] +pub struct VgicDistributor { + pub ctlr: u32, + pub isenabler: [u32; MAX_IRQS_WORDS], + pub ispendr: [u32; MAX_IRQS_WORDS], + pub isactiver: [u32; MAX_IRQS_WORDS], + pub ipriorityr: [u32; MAX_IRQS_WORDS * 8], +} + +#[derive(Debug, Copy, Clone)] +pub struct VgicRedistributor { + pub isenabler0: u32, + pub ispendr0: u32, + pub isactiver0: u32, + pub ipriorityr0: [u32; 8], + + // Virq injection queue. + pub pending_irqs: [u32; MAX_PENDING], + pub pending_head: usize, + pub pending_tail: usize, + pub pending_count: usize, + // Recording how many lrs are used like KVM doing. + pub used_lrs: usize, +} + +impl VgicDistributor { + pub const fn new() -> Self { + Self { + ctlr: 0, + isenabler: [0; MAX_IRQS_WORDS], + ispendr: [0; MAX_IRQS_WORDS], + isactiver: [0; MAX_IRQS_WORDS], + ipriorityr: [0; MAX_IRQS_WORDS * 8], + } + } +} + +impl VgicRedistributor { + pub const fn new() -> Self { + Self { + isenabler0: 0, + ispendr0: 0, + isactiver0: 0, + ipriorityr0: [0; 8], + pending_irqs: [0; MAX_PENDING], + pending_head: 0, + pending_tail: 0, + pending_count: 0, + used_lrs: 0, + } + } + + pub fn push_queue(&mut self, intid: u32) { + if self.pending_count >= MAX_PENDING { return; } + + // In case of existing same Intid. + let mut curr = self.pending_head; + for _ in 0..self.pending_count { + if self.pending_irqs[curr] == intid { + return; + } + curr = (curr + 1) % MAX_PENDING; + } + + self.pending_irqs[self.pending_tail] = intid; + self.pending_tail = (self.pending_tail + 1) % MAX_PENDING; + self.pending_count += 1; + } +} + +pub struct Vgic { + pub dist: SpinLock, + pub redists: [SpinLock; MAX_VCPUS], +} + +impl Vgic { + pub fn new() -> Self { + Self { + dist: SpinLock::new(VgicDistributor::new()), + redists: [ + SpinLock::new(VgicRedistributor::new()), + SpinLock::new(VgicRedistributor::new()), + SpinLock::new(VgicRedistributor::new()), + SpinLock::new(VgicRedistributor::new()), + ], + } + } +} + +pub static VGIC: Once = Once::new(); +pub fn init() { + VGIC.call_once(Vgic::new); +} +// Set mmio to access distributor and redistributor. +struct MmioAccess { + addr: u64, + is_write: bool, + size: usize, + reg_index: usize, +} + +#[inline] +fn get_vgic() -> &'static Vgic { + #[cfg(test)] + VGIC.call_once(Vgic::new); + + VGIC.get().expect("[vGIC] Error: VGIC is not initialized!") +} + +impl MmioAccess { + fn parse(esr: u64, far: u64) -> Option { + if (esr & (1 << 24)) == 0 { + return None; + } + + Some(Self { + addr: far, + is_write: (esr & (1 << 6)) != 0, + size: 1 << ((esr >> 22) & 0b11), + reg_index: ((esr >> 16) & 0b11111) as usize, + }) + } +} + +pub fn handle_data_abort(vcpu_id: usize, esr: u64, far: u64, regs: &mut [u64; 31]) -> bool { + let access = match MmioAccess::parse(esr, far) { + Some(acc) => acc, + None => return false, + }; + + if access.addr >= VGICD_BASE && access.addr < VGICD_BASE + VGICD_SIZE { + let offset = access.addr - VGICD_BASE; + emulate_access(vcpu_id, &access, offset, regs, true); + true + } else if access.addr >= VGICR_BASE && access.addr < VGICR_BASE + VGICR_SIZE { + let relative_addr = access.addr - VGICR_BASE; + let target_vcpu_id = (relative_addr / GICR_VCPU_SIZE) as usize; + let offset = relative_addr % GICR_VCPU_SIZE; + emulate_access(target_vcpu_id, &access, offset, regs, false); + true + } else { + false + } +} + +fn emulate_access(target_vcpu: usize, access: &MmioAccess, offset: u64, regs: &mut [u64; 31], is_dist: bool) { + let is_zero_reg = access.reg_index == 31; + + if access.is_write { + let val = if is_zero_reg { + 0 + } else { + (regs[access.reg_index] & 0xFFFFFFFF) as u32 + }; + + if is_dist { + handle_gicd_write(offset, val); + } else { + handle_gicr_write(target_vcpu, offset, val); + } + } else { + let val = if is_dist { + handle_gicd_read(offset) + } else { + handle_gicr_read(target_vcpu, offset) + }; + + if !is_zero_reg { + regs[access.reg_index] = val as u64; + } + } +} + +fn handle_gicd_write(offset: u64, val: u32) { + let mut dist = get_vgic().dist.lock(); + match offset { + 0x0000 => dist.ctlr = val, + 0x0100..=0x017C => dist.isenabler[((offset - 0x0100) / 4) as usize] |= val, + 0x0180..=0x01FC => dist.isenabler[((offset - 0x0180) / 4) as usize] &= !val, + 0x0400..=0x07F8 => dist.ipriorityr[((offset - 0x0400) / 4) as usize] = val, + _ => {} + } +} + +fn handle_gicr_write(vcpu_id: usize, offset: u64, val: u32) { + if vcpu_id >= MAX_VCPUS { return; } + let mut redist = get_vgic().redists[vcpu_id].lock(); + match offset { + 0x10100 => redist.isenabler0 |= val, + 0x10180 => redist.isenabler0 &= !val, + 0x10200 => redist.ispendr0 |= val, + 0x10280 => redist.ispendr0 &= !val, + 0x10300 => redist.isactiver0 |= val, + 0x10380 => redist.isactiver0 &= !val, + 0x10400..=0x1041C => redist.ipriorityr0[((offset - 0x10400) / 4) as usize] = val, + _ => {} + } +} + +fn handle_gicd_read(offset: u64) -> u32 { + let dist = get_vgic().dist.lock(); + match offset { + 0x0000 => dist.ctlr, + 0x0004 => 0x00000006, + // 0x43B represent Arm Ltd. + 0x0008 => 0x43B00000, + 0x0100..= 0x017C => dist.isenabler[((offset - 0x0100) / 4) as usize], + 0x0180..= 0x01FC => dist.isenabler[((offset - 0x0180) / 4) as usize], + 0x0400..= 0x07F8 => dist.ipriorityr[(((offset - 0x0400) / 4) as usize)], + 0xFFE8 => 0x00000030, + _ => 0, + } +} + +fn handle_gicr_read(vcpu_id: usize, offset: u64) -> u32 { + if vcpu_id >= MAX_VCPUS { return 0; } + let redist = get_vgic().redists[vcpu_id].lock(); + match offset { + 0x0008 => { + let mut val = (vcpu_id as u32) << 8; + let active_vcpus = unsafe { + VCPU_MANAGER.0.vcpu_count() + }; + if vcpu_id == active_vcpus - 1 { + val |= 1 << 4; // Last Redistributor + } + val + } + 0x000C => { + //Nowaday, we only have one core and basic mapping, so the high 32 bits are 0. + 0 + } + 0xFFE8 => 0x00000030, + 0x10100 => redist.isenabler0, + 0x10180 => redist.isenabler0, + 0x10200 => redist.ispendr0, + 0x10300 => redist.isactiver0, + 0x10400..=0x1041C => redist.ipriorityr0[((offset - 0x10400) / 4) as usize], + _ => 0, + } +} + +// Per-CPU Initialization (called by vCPU on first run) +// To Do: Distribut every vcpu a Redistributor. +pub fn cpu_init(vcpu_id: usize) { + unsafe { + // 1. Enable System Register access for EL2 (ICC_SRE_EL2) + let mut sre: u64; + asm!("mrs {}, ICC_SRE_EL2", out(reg) sre); + if (sre & 0x9) != 0x9 { + sre |= 0x9; + asm!("msr ICC_SRE_EL2, {}", in(reg) sre); + asm!("isb"); + } + + // 2. Enable vGIC + let hcr: u64 = 1; + asm!("msr ICH_HCR_EL2, {}", in(reg) hcr); + + // 3. Configure VMCR (Group 0/1 Enable) + let vmcr: u64 = 0x3; + asm!("msr ICH_VMCR_EL2, {}", in(reg) vmcr); + + // Clear all LRs + for i in 0..MAX_LR { + write_lr(i, 0); + } + } +} + +pub fn inject(vcpu_id: usize, intid: u32) { + unsafe { + if vcpu_id >= MAX_VCPUS || intid >= 1024 { return; } + let mut is_enabled; + if intid < 32 { + let mut redist = get_vgic().redists[vcpu_id].lock(); + redist.ispendr0 |= 1 << intid; + if (redist.isenabler0 & (1 << intid)) != 0 { + redist.push_queue(intid); + } + + } else { + // SPI + let mut dist = get_vgic().dist.lock(); + let idx = (intid / 32) as usize; + let mask = 1 << (intid % 32); + dist.ispendr[idx] |= mask; + is_enabled = (dist.isenabler[idx] & mask) != 0; + + // Temporarily set for int 33 to get shell. + if intid == 33{ + is_enabled = true; + } + drop(dist); + + if is_enabled { + let mut redist = get_vgic().redists[vcpu_id].lock(); + redist.push_queue(intid); + } + } + } +} + +pub fn flush(vcpu_id: usize) { + if vcpu_id >= MAX_VCPUS { + return; + } + + let mut redist = get_vgic().redists[vcpu_id].lock(); + + unsafe { + let mut current_lr = 0; + while redist.pending_count > 0 && current_lr < MAX_LR { + let intid = redist.pending_irqs[redist.pending_head]; + + redist.pending_head = (redist.pending_head + 1) % MAX_PENDING; + redist.pending_count -= 1; + + let is_active = is_irq_active_locked(&redist, intid); + let state_bits: u64 = if is_active { 0b11 } else { 0b01 }; + // Temporarily set priority(0xA0 << 48) + // To DO: Dynamically set hw bit(61) according to irq routing. + let lr_val: u64 = (state_bits << 62) | (1 << 61) | (1 << 60) | (0xA0 << 48) | ((intid as u64) << 32) | (intid as u64); + write_lr(current_lr, lr_val); + + current_lr += 1; + } + + redist.used_lrs = current_lr; + } +} + +pub fn sync(vcpu_id: usize) { + if vcpu_id >= MAX_VCPUS { return; } + + let mut redist = get_vgic().redists[vcpu_id].lock(); + + unsafe { + for i in 0..redist.used_lrs { + let lr_val = read_lr(i); + let state = (lr_val >> 62) & 0b11; + let intid = (lr_val & 0xFFFFFFFF) as u32; + + if state == 0 { + clear_irq_state_locked(&mut redist, intid); + } else { + sync_irq_state_locked(&mut redist, intid, state); + redist.push_queue(intid); + } + + write_lr(i, 0); + } + + redist.used_lrs = 0; + } +} + +pub fn inject_irq(vcpu_id: usize, intid: u32) { + if vcpu_id >= MAX_VCPUS { + return; + } + unsafe { + inject(vcpu_id, intid); + } +} + +pub fn inject_fiq(_intid: u32) { + // ... +} + +#[cfg(test)] +pub static mut MOCK_LR: [u64; MAX_LR] = [0; MAX_LR]; + +unsafe fn read_lr(index: usize) -> u64 { + #[cfg(test)] + { + if index < MAX_LR { + MOCK_LR[index] + }else { + 0 + } + } + + #[cfg(not(test))] + { + let val: u64; + match index { + 0 => asm!("mrs {}, ICH_LR0_EL2", out(reg) val), + 1 => asm!("mrs {}, ICH_LR1_EL2", out(reg) val), + 2 => asm!("mrs {}, ICH_LR2_EL2", out(reg) val), + 3 => asm!("mrs {}, ICH_LR3_EL2", out(reg) val), + _ => val = 0, + } + val + } +} + +unsafe fn write_lr(index: usize, val: u64) { + #[cfg(test)] + { + if index < MAX_LR { + MOCK_LR[index] = val; + } + } + #[cfg(not(test))] + { + match index { + 0 => asm!("msr ICH_LR0_EL2, {}", in(reg) val), + 1 => asm!("msr ICH_LR1_EL2, {}", in(reg) val), + 2 => asm!("msr ICH_LR2_EL2, {}", in(reg) val), + 3 => asm!("msr ICH_LR3_EL2, {}", in(reg) val), + _ => (), + } + } +} + +fn clear_irq_state_locked(redist: &mut VgicRedistributor, intid: u32) { + if intid < 32 { + redist.ispendr0 &= !(1 << intid); + redist.isactiver0 &= !(1 << intid); + } else { + // SPI should have lock. + let mut dist = get_vgic().dist.lock(); + let idx = (intid / 32) as usize; + let mask = 1 << (intid % 32); + dist.ispendr[idx] &= !mask; + dist.isactiver[idx] &= !mask; + } +} + +fn sync_irq_state_locked(redist: &mut VgicRedistributor, intid: u32, state: u64) { + let is_pending = (state & 0b01) != 0; + let is_active = (state & 0b10) != 0; + + if intid < 32 { + if is_pending { redist.ispendr0 |= 1 << intid; } else { redist.ispendr0 &= !(1 << intid); } + if is_active { redist.isactiver0 |= 1 << intid; } else { redist.isactiver0 &= !(1 << intid); } + } else { + let mut dist = get_vgic().dist.lock(); + let idx = (intid / 32) as usize; + let mask = 1 << (intid % 32); + if is_pending { dist.ispendr[idx] |= mask; } else { dist.ispendr[idx] &= !mask; } + if is_active { dist.isactiver[idx] |= mask; } else { dist.isactiver[idx] &= !mask; } + } +} + +fn is_irq_active_locked(redist: &VgicRedistributor, intid: u32) -> bool { + if intid < 32 { + (redist.isactiver0 & (1 << intid)) != 0 + } else { + let dist = get_vgic().dist.lock(); + (dist.isactiver[(intid / 32) as usize] & (1 << (intid % 32))) != 0 + } +} + +#[cfg(test)] +mod tests { + use super::*; + use blueos_test_macro::test; + + fn setup_test_env() { + unsafe { + super::MOCK_LR.fill(0); + } + } + + #[test] + fn test_vgic_queue_deduplication() { + setup_test_env(); + let mut redist = VgicRedistributor::new(); + + // verify normal queuing. + redist.push_queue(27); + redist.push_queue(30); + assert_eq!(redist.pending_count, 2); + assert_eq!(redist.pending_irqs[0], 27); + assert_eq!(redist.pending_irqs[1], 30); + + // verify deadlock prevention optimization: ghost reuse deduplication. + redist.push_queue(27); + assert_eq!(redist.pending_count, 2, "Duplicate interrupt should be ignored!"); + } + + #[test] + fn test_vgic_flush_and_sync_lifecycle() { + setup_test_env(); + init(); + let vcpu_id = 0; + + // Simulating enabling 27 interrupt + { + let mut redist = get_vgic().redists[vcpu_id].lock(); + redist.isenabler0 |= 1 << 27; + } + + inject(vcpu_id, 27); + { + let redist = get_vgic().redists[vcpu_id].lock(); + assert_eq!(redist.pending_count, 1); + } + + flush(vcpu_id); + + unsafe { + let lr_val = super::MOCK_LR[0]; + let state = (lr_val >> 62) & 0b11; + let intid = (lr_val & 0xFFFFFFFF) as u32; + assert_eq!(state, 0b01, "Interrupt should be Pending in LR"); + assert_eq!(intid, 27, "IntID in LR should be 27"); + } + + { + let redist = get_vgic().redists[vcpu_id].lock(); + assert_eq!(redist.pending_count, 0, "Queue should be empty after flush"); + assert_eq!(redist.used_lrs, 1, "Should record 1 used LR"); + } + + // Simulating hardware EOI. + unsafe { + super::MOCK_LR[0] = 0; + } + sync(vcpu_id); + + { + let redist = get_vgic().redists[vcpu_id].lock(); + assert_eq!(redist.used_lrs, 0, "Used LRs should be reset"); + assert_eq!((redist.isactiver0 & (1 << 27)), 0, "Active state should be cleared"); + } + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/vtimer.rs b/kernel/src/arch/aarch64/virt/vtimer.rs new file mode 100644 index 00000000..a7d99266 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vtimer.rs @@ -0,0 +1,96 @@ +// Copyright (c) 2026 vivo Mobile Communication Co., Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use crate::arch::aarch64::{ + current_cpu_id, + irq::{self, IrqHandler, IrqNumber, Priority} +}; +use super::vgic; +use alloc::boxed::Box; +use core::arch::asm; + +pub struct VirtualTimerHandler; + +impl IrqHandler for VirtualTimerHandler { + fn handle(&mut self) { + unsafe { + // 1. Shield the virtual timer interrupt. + let mut ctl = read_cntv_ctl(); + ctl |= 1 << 1; + write_cntv_ctl(ctl); + // 2. Inject the interrupt into the vGIC queue. + let vcpu_id = current_cpu_id(); + vgic::inject_irq(vcpu_id, 27); + } + } +} + +pub fn init_global_vtimer() { + let timer_handler = Box::new(VirtualTimerHandler); + irq::register_handler(IrqNumber::new(27), timer_handler) + .expect("[vTimer] Failed to register IRQ 27 handler"); + +} + +/// Call it before booting guest. +pub fn init_vcpu_timer() { + irq::enable_irq_with_priority( + IrqNumber::new(27), + current_cpu_id(), + Priority::Normal + ); +} + +#[cfg(not(test))] +#[inline] +fn read_cntv_ctl() -> u64 { + let ctl: u64; + unsafe { asm!("mrs {}, CNTV_CTL_EL0", out(reg) ctl); } + ctl +} + +#[cfg(not(test))] +#[inline] +fn write_cntv_ctl(ctl: u64) { + unsafe { asm!("msr CNTV_CTL_EL0, {}", in(reg) ctl); } +} + +#[cfg(test)] +static mut MOCK_CNTV_CTL: u64 = 0; + +#[cfg(test)] +fn read_cntv_ctl() -> u64 { unsafe { MOCK_CNTV_CTL } } + +#[cfg(test)] +fn write_cntv_ctl(ctl: u64) { unsafe { MOCK_CNTV_CTL = ctl; } } + +#[cfg(test)] +mod tests { + use super::*; + use blueos_test_macro::test; + + #[test] + fn test_vtimer_handler_masking() { + unsafe { + MOCK_CNTV_CTL = 0; + } + + let mut handler = VirtualTimerHandler; + handler.handle(); + // Verify whether we have shield physical interrupt. (Bit 1 = IMASK) + unsafe { + assert_eq!(MOCK_CNTV_CTL & (1 << 1), (1 << 1), "vTimer must set IMASK (bit 1) to prevent IRQ storms"); + } + } +} \ No newline at end of file diff --git a/kernel/src/boards/qemu_virt64_aarch64/config.rs b/kernel/src/boards/qemu_virt64_aarch64/config.rs index b2fecbff..7ce92b9a 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/config.rs +++ b/kernel/src/boards/qemu_virt64_aarch64/config.rs @@ -19,7 +19,7 @@ pub const APBP_CLOCK: u32 = 0x16e3600; pub const PL011_UART0_BASE: u64 = 0x900_0000; pub const PL011_UART0_IRQNUM: IrqNumber = IrqNumber::new(33); pub const GENERIC_TIMER_IRQNUM: IrqNumber = IrqNumber::new(30); -pub const HEAP_SIZE: u64 = 16 * 1024 * 1024; +pub const HEAP_SIZE: u64 = 128 * 1024 * 1024; pub const PSCI_BASE: u32 = 0x84000000; pub const GICD: usize = 0x8000000; pub const GICR: usize = 0x80a0000; diff --git a/kernel/src/boards/qemu_virt64_aarch64/init.rs b/kernel/src/boards/qemu_virt64_aarch64/init.rs index 05c20899..34c1dc01 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/init.rs +++ b/kernel/src/boards/qemu_virt64_aarch64/init.rs @@ -19,6 +19,7 @@ use crate::{ irq, irq::{IrqTrigger, Priority}, registers::cntfrq_el0::CNTFRQ_EL0, + virt::virt_boot_linux, }, error::Error, irq::IrqTrace, @@ -78,6 +79,7 @@ pub(crate) fn init() { ), ); let _ = irq::register_handler(config::GENERIC_TIMER_IRQNUM, Box::new(TimerIrq {})); + virt_boot_linux(); } crate::define_peripheral! { diff --git a/kernel/src/boards/qemu_virt64_aarch64/link.x b/kernel/src/boards/qemu_virt64_aarch64/link.x index b38957a4..ab2261c9 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/link.x +++ b/kernel/src/boards/qemu_virt64_aarch64/link.x @@ -5,7 +5,7 @@ STACK_SIZE = 128 * 1024; MEMORY { - DRAM : ORIGIN = 0x40280000, LENGTH = 32M + DRAM : ORIGIN = 0x40280000, LENGTH = 256M } PHDRS @@ -80,7 +80,7 @@ SECTIONS . = ALIGN(4096); __heap_start = .; - . += 0x800000; + . += 128M; __heap_end = .; _end = .; } From 5b8c3a842016ba9307a69d306b0a3215778be775 Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 8 May 2026 10:37:51 +0800 Subject: [PATCH 2/5] Finish basic framework to boot linux --- kernel/src/arch/aarch64/registers/hcr_el2.rs | 160 +++++++++--------- kernel/src/arch/aarch64/virt/exit.rs | 122 ++++--------- kernel/src/arch/aarch64/virt/guest.rs | 12 +- kernel/src/arch/aarch64/virt/hyper.rs | 28 +-- kernel/src/arch/aarch64/virt/mmu_s2.rs | 2 - kernel/src/arch/aarch64/virt/mod.rs | 14 +- kernel/src/arch/aarch64/virt/vcpu.rs | 1 + kernel/src/arch/aarch64/virt/vector.rs | 27 ++- .../src/boards/qemu_virt64_aarch64/config.rs | 2 +- kernel/src/boards/qemu_virt64_aarch64/init.rs | 2 - kernel/src/boards/qemu_virt64_aarch64/link.x | 2 +- 11 files changed, 149 insertions(+), 223 deletions(-) diff --git a/kernel/src/arch/aarch64/registers/hcr_el2.rs b/kernel/src/arch/aarch64/registers/hcr_el2.rs index 7cdfecea..76b6e35e 100644 --- a/kernel/src/arch/aarch64/registers/hcr_el2.rs +++ b/kernel/src/arch/aarch64/registers/hcr_el2.rs @@ -31,14 +31,14 @@ register_bitfields! {u64, Set = 1 ], - /// FWB, bit [2] - Force Write-back - FWB OFFSET(2) NUMBITS(1) [ - Normal = 0, - ForceWB = 1 + /// PTW, bit [2] - Page Table Walk + PTW OFFSET(2) NUMBITS(1) [ + NoTrap = 0, + Trap = 1 ], - /// AMO, bit [3] - Asynchronous Memory Abort Override - AMO OFFSET(3) NUMBITS(1) [ + /// FMO, bit [3] - Asynchronous Memory Abort Override + FMO OFFSET(3) NUMBITS(1) [ EL1Handled = 0, EL2Handled = 1 ], @@ -49,16 +49,48 @@ register_bitfields! {u64, EL2Handled = 1 ], - /// FMO, bit [5] - FIQ Mask Override - FMO OFFSET(5) NUMBITS(1) [ + /// AMO, bit [5] - Asynchronous Abort Routing + AMO OFFSET(5) NUMBITS(1) [ EL1Handled = 0, EL2Handled = 1 ], - /// TGE, bit [6] - Trap General Exceptions - TGE OFFSET(6) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 + /// VF, bit [6] - Vitual FIQ + VF OFFSET(6) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// VI, bit [7] - Virtual IRQ + VI OFFSET(7) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// VSE, bit [8] - Virtual System Error + VSE OFFSET(8) NUMBITS(1) [ + Disable = 0, + Enable = 1 + ], + + /// FB, bit [9] - Force Broadcast + FB OFFSET(9) NUMBITS(1) [ + Normal = 0, + ForceBroadcast = 1 + ], + + /// BSU, bits [11:10] - Barrier Shareability Upgrade + BSU OFFSET(10) NUMBITS(2) [ + NoEffect = 0b00, + InnerShareable = 0b01, + OuterShareable = 0b10, + FullShareable = 0b11 + ], + + /// DC, bit [12] - Default Cacheable + DC OFFSET(12) NUMBITS(1) [ + Disable = 0, + Enable = 1 ], /// TWI, bit [13] - Trap WFI @@ -75,52 +107,58 @@ register_bitfields! {u64, Trap = 1 ], - /// DCVA, bit [15] - Data Cache Zero By VA - DCVA OFFSET(15) NUMBITS(1) [ + /// TSC, bit [19] - TRAP SMC instruction + TSC OFFSET(19) NUMBITS(1) [ NoTrap = 0, Trap = 1 ], - /// AT, bit [16] - Address Translation - AT OFFSET(16) NUMBITS(1) [ + /// TACR, bit [21] - Trap ACTLR accesses + TACR OFFSET(21) NUMBITS(1) [ NoTrap = 0, Trap = 1 ], - /// ST, bit [17] - Store Team Register - ST OFFSET(17) NUMBITS(1) [ + /// TSW, bit [22] - Trap Data Cache instructions by Set/Way + TSW OFFSET(22) NUMBITS(1) [ NoTrap = 0, Trap = 1 ], - /// VSE, bit [18] - Virtual System Error - VSE OFFSET(18) NUMBITS(1) [ - Disable = 0, - Enable = 1 + /// TPCP, bit [23] - Trap Cache Maintenance instructions + TPCP OFFSET(23) NUMBITS(1) [ + NoTrap = 0, + Trap = 1 ], - /// VI, bit [19] - Virtual IRQ - VI OFFSET(19) NUMBITS(1) [ - Disable = 0, - Enable = 1 + /// TVM, bit [26] - Trap Virtual Memory Controls + TVM OFFSET(26) NUMBITS(1) [ + NoTrap = 0, + Trap = 1 ], - /// VF, bit [20] - Virtual FIQ - VF OFFSET(20) NUMBITS(1) [ - Disable = 0, - Enable = 1 + /// TGE, bit [27] - Trap General Exceptions + TGE OFFSET(27) NUMBITS(1) [ + GuestMode = 0, + HostMode = 1 ], - /// FMO, bit [21] - Cache Maintenance Override - FMO_CM OFFSET(21) NUMBITS(1) [ - Normal = 0, - Override = 1 + /// TDZ, bit [28] - Trap DC ZVA instruction + TDZ OFFSET(28) NUMBITS(1) [ + NoTrap = 0, + Trap = 1 ], - /// AMO_BIT, bit [22] - AMO for instruction - AMO_BIT OFFSET(22) NUMBITS(1) [ - Normal = 0, - Override = 1 + /// HCD, bit [29] - HVC Instruction Disable + HCD OFFSET(29) NUMBITS(1) [ + EnableHVC = 0, + DisableHVC = 1 + ], + + /// TRVM, bit [30] - Trap Reads of Virtual Memory Controls + TRVM OFFSET(30) NUMBITS(1) [ + NoTrap = 0, + Trap = 1 ], /// RW, bit [31] - Register width control @@ -131,52 +169,22 @@ register_bitfields! {u64, EL1AArch64 = 1 ], - /// PTW, bit [23] - Page Table Walk - PTW OFFSET(23) NUMBITS(1) [ + /// CD, bit [32] - Disable Stage 2 Data Cache + CD OFFSET(32) NUMBITS(1) [ Enable = 0, Disable = 1 ], - /// HCD, bit [24] - Hypervisor Call Disable - HCD OFFSET(24) NUMBITS(1) [ + /// ID, bit [33] - Disable Stage 2 Instruction Cache + ID OFFSET(33) NUMBITS(1) [ Enable = 0, Disable = 1 ], - /// TDZ, bit [25] - TRAP DC ZVA - TDZ OFFSET(25) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 - ], - - /// TSC, bit [31] - TRAP SC - TSC OFFSET(31) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 - ], - - /// TACR, bit [33] - TRAP ACTLR - TACR OFFSET(33) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 - ], - - /// TIDCP, bit [36] - TRAP IMPLEMENTATION DEFINED - TIDCP OFFSET(36) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 - ], - - /// TOCU, bit [38] - TRAP OSLAR - TOCU OFFSET(38) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 - ], - - /// TID4, bit [40] - TRAP ID bits - TID4 OFFSET(40) NUMBITS(1) [ - NoTrap = 0, - Trap = 1 + /// E2H, bit [34] - EL2 Host + E2H OFFSET(34) NUMBITS(1) [ + Disable = 0, + Enable = 1 ] ] } diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index dee4f53c..c8470fde 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -13,7 +13,7 @@ // limitations under the License. use core::arch::asm; -use super::{vcpu::Vcpu, vgic, hyper}; +use super::{vcpu::Vcpu, vgic, hyper, guest}; use semihosting::println; static mut GUEST_SHUTDOWN: bool = false; @@ -24,7 +24,6 @@ pub const PSCI_SYSTEM_RESET: u32 = 0x8400_0009; pub const PSCI_FEATURES: u32 = 0x8400_000A; pub const HVC_VMM_GET_INFO: u64 = 0x11; pub const HVC_VMM_INJECT_IRQ: u64 = 0x13; -pub const HVC_GUEST_SHUTDOWN: u64 = 0x20; #[derive(Debug, Clone, Copy, PartialEq)] pub enum VmExitReason { @@ -50,7 +49,7 @@ pub fn parse_exit_reason(esr: u64) -> VmExitReason { let ec = (esr >> 26) & 0x3F; match ec { - 0x16 => VmExitReason::Hvc, + 0x16 | 0x17 => VmExitReason::Hvc, 0x15 => VmExitReason::Svc, 0x24 => VmExitReason::DataAbortLowerEL, 0x20 => VmExitReason::InstructionAbortLowerEL, @@ -75,13 +74,6 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { return_addr: elr + 4, }; - semihosting::println!("[EXIT] VM Exit Happened!"); - semihosting::println!("[EXIT] Reason: {:?}", reason); - semihosting::println!("[EXIT] ESR: {:#x}", esr); - semihosting::println!("[EXIT] EC: {:#x}", (esr >> 26) & 0x3F); - semihosting::println!("[EXIT] FAR: {:#x}", elr); - semihosting::println!("[EXIT] PSTATE: {:#x}", pstate); - match reason { VmExitReason::Hvc => { handle_hvc(vcpu, &exit_info) @@ -96,35 +88,9 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { let is_write = (iss & (1 << 6)) != 0; let faulting_pc = vcpu.context().elr_el2; - unsafe { - semihosting::println!("====================================="); - semihosting::println!("[EXIT] PoC Guest triggered Data Abort!"); - semihosting::println!("[EXIT] 1. Faulting PC (ELR_EL2) : {}", faulting_pc); - semihosting::println!("[EXIT] 2. Target Addr (FAR_EL2) : {}", { - let far: u64; - core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); - far - }); - if is_write { - semihosting::println!("[EXIT] 3. Access Type : WRITE"); - } else { - semihosting::println!("[EXIT] 3. Access Type : READ"); - } - - semihosting::println!("[EXIT] 4. DFSC Code : {}", dfsc as u64); - let hpfar_el2: u64; - unsafe { core::arch::asm!("mrs {}, hpfar_el2", out(reg) hpfar_el2); } - - // Caclate Linux want to access physical address (IPA) - let fault_ipa = (hpfar_el2 & 0x0000_00FF_FFFF_FFF0) << 8; - println!("Target Physical Addr (IPA): {:#x}", fault_ipa); - semihosting::println!("====================================="); - } - if (dfsc & 0x3C) == 0x04 || (dfsc & 0x3C) == 0x08 || (dfsc & 0x3C) == 0x0C { // Translation fault (level 0/1/2/3) - Stage-2 未映射 unsafe { - semihosting::println!("[EXIT] Stage-2 Translation Fault - skipping instruction"); let far: u64; core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); let hpfar_el2: u64; @@ -139,21 +105,18 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { &mut vcpu.context_mut().regs ); - if handled { - semihosting::println!("[EXIT] MMIO Handled by vGIC"); - vcpu.context_mut().elr_el2 += 4; - vgic::flush(vcpu.id()); - return true; - } else { - semihosting::println!("[EXIT] Unhandled Stage-2 Address!"); - } + if handled { + vcpu.context_mut().elr_el2 += 4; + vgic::flush(vcpu.id()); + return true; + } else { + semihosting::println!("[EXIT] Unhandled Stage-2 Address!"); + } } } - semihosting::println!("[EXIT] Unrecoverable Data Abort, terminating Guest"); false } VmExitReason::InstructionAbortLowerEL => { - semihosting::println!("[EXIT] Instruction Abort from Guest!"); let iss = esr & 0x1FFFFFF; let ifsc = iss & 0x3F; @@ -163,9 +126,22 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { false } VmExitReason::TrappedWfiWfe => { - semihosting::println!("[EXIT] Trapped WFI/WFE instruction"); + let iss = exit_info.esr & 0x1FFFFFF; + let is_wfe = (iss & 1) != 0; vcpu.context_mut().elr_el2 += 4; - true + + if is_wfe { + true + } else { + let irq_masked = (vcpu.context().spsr & (1 << 7)) != 0; + + if irq_masked { + false + } else { + unsafe { core::arch::asm!("wfi"); } + true + } + } } VmExitReason::Unknown(ec) => { semihosting::println!("[EXIT] Unknown Exit Reason: EC = {:#x}", ec); @@ -177,76 +153,52 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { // hvc from guest. fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { let saved_x0 = vcpu.context().regs[0]; - semihosting::println!("[EXIT] Handle HVC Call"); let vcpu_id = vcpu.id(); let context = vcpu.context_mut(); let hvc_num = info.esr & 0xFFFF; - semihosting::println!("[EXIT] HVC#{}", hvc_num); - let mut need_advance_pc = true; + let mut is_psci_call = false; // Easy HVC Services. let result = match hvc_num { 0x00 => { let psci_func_id = context.regs[0] as u32; - semihosting::println!("[DEBUG] Returning to Linux: VBAR={:#x}, TTBR1={:#x}, SCTLR={:#x}", context.vbar_el1, context.ttbr1_el1, context.sctlr_el1); + is_psci_call = true; + match psci_func_id { PSCI_VERSION => { - semihosting::println!("[EXIT] HVC#0: Linux requested PSCI_VERSION"); - context.regs[0] = 0x00010001; - context.regs[1] = 0; - context.regs[2] = 0; - context.regs[3] = 0; - - if context.regs[4] == 0 { - context.regs[4] = context.sp - 16; - } - true + let version = 0x0000_0002; + context.regs[0] = version; } PSCI_SYSTEM_OFF | PSCI_SYSTEM_RESET => { unsafe { - semihosting::println!("[EXIT] HVC#0: Linux requested PSCI_SYSTEM_OFF. Shutting down..."); GUEST_SHUTDOWN = true; } - // Shutdown needn't "pc + 4". need_advance_pc = false; - false + return false; } PSCI_FEATURES => { let feature_id = context.regs[1] as u32; - // semihosting::println!("[EXIT] HVC#0: Linux requested PSCI_FEATURES for {:#x}", feature_id); - if feature_id == PSCI_SYSTEM_OFF || feature_id == PSCI_SYSTEM_RESET { context.regs[0] = 0; } else { context.regs[0] = 0xFFFF_FFFF; } - true } _ => { semihosting::println!("[EXIT] HVC#0: Ignored PSCI call: {:#x}", psci_func_id); - // We don't suuport this function. context.regs[0] = 0xFFFF_FFFF; - true } } + + true } - HVC_VMM_GET_INFO=> { + HVC_VMM_GET_INFO => { context.regs[0] = 0x48495001; true } - HVC_GUEST_SHUTDOWN => { - unsafe { - semihosting::println!("[EXIT] HVC#20 Guest Shutdown..."); - need_advance_pc = false; - GUEST_SHUTDOWN = true; - } - - false - - } _ => { - semihosting::println!("[EXIT] Unknown HVC Number"); + semihosting::println!("[EXIT] Unknown HVC Number"); true } }; @@ -258,28 +210,24 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { result } +// To DO: Finish this function. fn handle_svc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { let context = vcpu.context_mut(); let svc_num = context.regs[0]; - semihosting::println!("[EXIT] SVC Number: {}", svc_num); match svc_num { 0 => { - semihosting::println!("[EXIT] SVC#0: Hello from Guest via SVC!"); context.regs[0] = 0; } 1 => { - semihosting::println!("[EXIT] SVC#1: Get Guest ID"); context.regs[0] = 1; } _ => { - semihosting::println!("[EXIT] Unknown SVC Number"); context.regs[0] = 0xFFFFFFFF; } } context.elr_el2 = info.return_addr as u64; - true } diff --git a/kernel/src/arch/aarch64/virt/guest.rs b/kernel/src/arch/aarch64/virt/guest.rs index dde4735d..41b700b6 100644 --- a/kernel/src/arch/aarch64/virt/guest.rs +++ b/kernel/src/arch/aarch64/virt/guest.rs @@ -14,16 +14,6 @@ use core::arch::asm; -pub const GUEST_CODE_LOAD_ADDR: usize = 0x4100_0000; -pub const GUEST_STACK_SIZE: usize = 32 * 1024; -pub const GUEST_STACK_TOP: usize = 0x4110_0000 - 16; -const GUEST_STACK_TOP_LO: u16 = (GUEST_STACK_TOP & 0xFFFF) as u16; -const GUEST_STACK_TOP_HI: u16 = ((GUEST_STACK_TOP >> 16) & 0xFFFF) as u16; pub const LINUX_KERNEL_LOAD_ADDR: usize = 0x4400_0000; pub const LINUX_DTB_ADDR: usize = 0x4E00_0000; - -/// Get guest stack top address. -#[inline] -pub fn guest_stack_top() -> usize { - GUEST_STACK_TOP -} \ No newline at end of file +pub const LINUX_RAM_SIZE: usize = 0x1000_0000; diff --git a/kernel/src/arch/aarch64/virt/hyper.rs b/kernel/src/arch/aarch64/virt/hyper.rs index 0c3fb3a3..470bdd3d 100644 --- a/kernel/src/arch/aarch64/virt/hyper.rs +++ b/kernel/src/arch/aarch64/virt/hyper.rs @@ -90,15 +90,15 @@ pub fn read_spsr_el2() -> u64 { #[inline] pub fn configure_hcr_el2_for_guest() { - // Identity map 128MB of RAM starting from 0x4400_0000 so the Guest can run in-place - super::mmu_s2::init_stage2(0x4400_0000, 0x1000_0000); + // Identity map 192MB of RAM starting from 0x4400_0000 so the Guest can run in-place + super::mmu_s2::init_stage2(0x4400_0000, 0x0c00_0000); HCR_EL2.write( HCR_EL2::VM::Enable + HCR_EL2::RW::EL1AArch64 + HCR_EL2::IMO::EL2Handled + HCR_EL2::FMO::EL2Handled + HCR_EL2::AMO::EL2Handled - +HCR_EL2::TSC::Trap + + HCR_EL2::TSC::Trap ); unsafe { core::arch::asm!("isb"); @@ -186,26 +186,4 @@ pub fn hyp_init() { core::arch::asm!("dsb sy", options(nostack)); core::arch::asm!("isb sy", options(nostack)); } -} - -// For GuestOS -#[inline] -pub fn enter_guest(entry: usize, dtb_addr: usize, pstate: u64) { - unsafe { - core::arch::asm!( - "msr elr_el2, {entry}", - "msr spsr_el2, {pstate}", - "mov x0, {dtb}", - "mov x1, xzr", - "mov x2, xzr", - "mov x3, xzr", - "dsb sy", - "isb sy", - "eret", - entry = in(reg) entry as u64, - pstate = in(reg) pstate, - dtb = in(reg) dtb_addr as u64, - options(noreturn) - ); - } } \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/mmu_s2.rs b/kernel/src/arch/aarch64/virt/mmu_s2.rs index de8d723a..fd192780 100644 --- a/kernel/src/arch/aarch64/virt/mmu_s2.rs +++ b/kernel/src/arch/aarch64/virt/mmu_s2.rs @@ -114,8 +114,6 @@ pub fn init_stage2(ipa_base: usize, size: usize) { // map_range(0x0800_0000, 0x0800_0000, 0x0001_0000, true); // map_range(0x080A_0000, 0x080A_0000, 0x00F6_0000, true); - semihosting::println!("[S2MMU] init_stage2 done."); - unsafe { core::arch::asm!("dsb sy", options(nostack, nomem)); let start = &S2_L1 as *const _ as usize; diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index 58a246b0..c42d693a 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -27,6 +27,7 @@ pub use vcpu::{Vcpu, VcpuManager, VcpuState}; pub use vgic::init; pub use crate::arch::aarch64::psci::hvc_call; use semihosting::println; +use blueos_hal::PlatPeri; // PL011 UART addresses for QEMU Virt const UART0_DR: *mut u32 = 0x0900_0000 as *mut u32; @@ -56,9 +57,6 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - return 0; } - if intid != 27 { - semihosting::println!("[EL2] IRQ trap! INTID: {}", intid); - } if intid == 33 { unsafe { let lr_val: u64 = (1 << 62) | (1 << 61) | (1 << 60) | (0xA0 << 48) | (33 << 32) | 33; @@ -132,6 +130,7 @@ pub fn virt_boot_linux() { vgic::init(); vtimer::init_global_vtimer(); + // Initiate vCpu for Linux kernel, set the entry point and parameters. unsafe { let vcpu = VCPU_MANAGER.0.create_vcpu(0, guest::LINUX_KERNEL_LOAD_ADDR, 0).unwrap(); let mut ctx = vcpu.context_mut(); @@ -142,4 +141,13 @@ pub fn virt_boot_linux() { vtimer::init_vcpu_timer(); let result = hvc_call(2, 0, 0); + + /// Nowadays, Linux kernel will call PSCI CPU_OFF to shutdown the vCPU after it finishes its work, where we can't print any message. + /// So we print the shutdown message here before Linux kernel calls PSCI CPU_OFF. + /// To Do: Solve this problem in next step. + if result == 0 { + let uart = crate::boards::get_device!(console_uart); + uart.enable(); + semihosting::println!("Linux shutdown!!!"); + } } \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs index 708c3ee1..bc1a176a 100644 --- a/kernel/src/arch/aarch64/virt/vcpu.rs +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -235,6 +235,7 @@ impl Vcpu { } } +// Temporarily set 4 vCPUs，though we use one. #[repr(align(16))] pub struct VcpuManager { vcpus: [Option; 4], diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index 8afc61db..a26293fc 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -145,9 +145,9 @@ pub unsafe extern "C" fn sync_from_lower_el1() { "add sp, sp, #272\n", "eret\n", "2:\n", - "ldr x1, [sp, #248]\n", // Host ELR - "ldr x2, [sp, #256]\n", // Host SPSR - "ldr x3, [sp, #264]\n", // Host SP_EL1 + "ldr x1, [sp, #248]\n", + "ldr x2, [sp, #256]\n", + "ldr x3, [sp, #264]\n", "msr elr_el2, x1\n", "msr spsr_el2, x2\n", "msr sp_el1, x3\n", @@ -230,10 +230,9 @@ unsafe fn handle_host_request(frame: *mut u64) -> u64 { } unsafe fn save_host_context(frame: *mut u64) { - VCPU_MANAGER.0.host_elr = *frame.add(31); + VCPU_MANAGER.0.host_elr = *frame.add(31) + 4; VCPU_MANAGER.0.host_spsr = *frame.add(32); VCPU_MANAGER.0.host_sp = *frame.add(33); - semihosting::println!("host_elr: {:x}, host_spsr: {:x}, host_sp: {:x}", VCPU_MANAGER.0.host_elr, VCPU_MANAGER.0.host_spsr, VCPU_MANAGER.0.host_sp); for i in 0..31 { VCPU_MANAGER.0.host_regs[i] = *frame.add(i); } @@ -266,12 +265,14 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { *frame.add(31) = VCPU_MANAGER.0.host_elr; *frame.add(32) = VCPU_MANAGER.0.host_spsr; *frame.add(33) = VCPU_MANAGER.0.host_sp; - semihosting::println!("host_elr: {:x}, host_spsr: {:x}, host_sp: {:x}", VCPU_MANAGER.0.host_elr, VCPU_MANAGER.0.host_spsr, VCPU_MANAGER.0.host_sp); // Restore Host GPRs (x0-x30) for i in 0..31 { *frame.add(i) = VCPU_MANAGER.0.host_regs[i]; } + // Pass success code (0) back to host's x0 + *frame.add(0) = 0; + let vbar = VCPU_MANAGER.0.host_vbar; let sctlr = VCPU_MANAGER.0.host_sctlr; let ttbr0 = VCPU_MANAGER.0.host_ttbr0; @@ -301,13 +302,9 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { unsafe fn save_frame_to_context(frame: *mut u64, vcpu: &mut Vcpu) { let mut ctx = vcpu.context_mut(); - let raw_x4 = *frame.add(4); for i in 0..31 { ctx.regs[i] = *frame.add(i); } - if ctx.regs[4] != raw_x4 { - semihosting::println!("[ALARM] Memory corruption! ctx.regs[4] expected {:x}, got {:x}", raw_x4, ctx.regs[4]); - } ctx.elr_el2 = *frame.add(31); ctx.spsr = *frame.add(32); ctx.sp = *frame.add(33); @@ -352,9 +349,6 @@ unsafe fn restore_context_to_frame(vcpu: &mut Vcpu, frame: *mut u64) { "msr mair_el1, {mair}", "msr sctlr_el1, {sctlr}", "isb", - "tlbi alle1", - "dsb sy", - "isb", vbar = in(reg) ctx.vbar_el1, ttbr0 = in(reg) ctx.ttbr0_el1, ttbr1 = in(reg) ctx.ttbr1_el1, @@ -396,16 +390,19 @@ pub unsafe extern "C" fn irq_from_lower_el1() { "str x30, [sp, #240]\n", "mrs x1, elr_el2\n", "mrs x2, spsr_el2\n", + "mrs x3, sp_el1\n", "str x1, [sp, #248]\n", "str x2, [sp, #256]\n", + "str x3, [sp, #264]\n", "mov x0, sp\n", - "mov x19, sp\n", "bl hyper_trap_irq\n", - "mov sp, x19\n", "ldr x1, [sp, #248]\n", "ldr x2, [sp, #256]\n", + "ldr x3, [sp, #264]\n", "msr elr_el2, x1\n", "msr spsr_el2, x2\n", + "msr sp_el1, x3\n", + "isb\n", "ldp x0, x1, [sp, #0]\n", "ldp x2, x3, [sp, #16]\n", "ldp x4, x5, [sp, #32]\n", diff --git a/kernel/src/boards/qemu_virt64_aarch64/config.rs b/kernel/src/boards/qemu_virt64_aarch64/config.rs index 7ce92b9a..b2fecbff 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/config.rs +++ b/kernel/src/boards/qemu_virt64_aarch64/config.rs @@ -19,7 +19,7 @@ pub const APBP_CLOCK: u32 = 0x16e3600; pub const PL011_UART0_BASE: u64 = 0x900_0000; pub const PL011_UART0_IRQNUM: IrqNumber = IrqNumber::new(33); pub const GENERIC_TIMER_IRQNUM: IrqNumber = IrqNumber::new(30); -pub const HEAP_SIZE: u64 = 128 * 1024 * 1024; +pub const HEAP_SIZE: u64 = 16 * 1024 * 1024; pub const PSCI_BASE: u32 = 0x84000000; pub const GICD: usize = 0x8000000; pub const GICR: usize = 0x80a0000; diff --git a/kernel/src/boards/qemu_virt64_aarch64/init.rs b/kernel/src/boards/qemu_virt64_aarch64/init.rs index 34c1dc01..05c20899 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/init.rs +++ b/kernel/src/boards/qemu_virt64_aarch64/init.rs @@ -19,7 +19,6 @@ use crate::{ irq, irq::{IrqTrigger, Priority}, registers::cntfrq_el0::CNTFRQ_EL0, - virt::virt_boot_linux, }, error::Error, irq::IrqTrace, @@ -79,7 +78,6 @@ pub(crate) fn init() { ), ); let _ = irq::register_handler(config::GENERIC_TIMER_IRQNUM, Box::new(TimerIrq {})); - virt_boot_linux(); } crate::define_peripheral! { diff --git a/kernel/src/boards/qemu_virt64_aarch64/link.x b/kernel/src/boards/qemu_virt64_aarch64/link.x index ab2261c9..5c4283f1 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/link.x +++ b/kernel/src/boards/qemu_virt64_aarch64/link.x @@ -80,7 +80,7 @@ SECTIONS . = ALIGN(4096); __heap_start = .; - . += 128M; + . += 0x800000; __heap_end = .; _end = .; } From 409913f36c1cbc1ae951ece2f95bed97b5429f4a Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 8 May 2026 11:22:38 +0800 Subject: [PATCH 3/5] Change for rabase --- kernel/src/arch/aarch64/registers/hcr_el2.rs | 2 +- kernel/src/arch/aarch64/registers/mair_el2.rs | 2 +- .../src/arch/aarch64/registers/sctlr_el2.rs | 2 +- kernel/src/arch/aarch64/registers/spsr_el2.rs | 2 +- kernel/src/arch/aarch64/registers/tcr_el2.rs | 2 +- .../src/arch/aarch64/registers/ttbr0_el2.rs | 2 +- kernel/src/arch/aarch64/registers/vtcr_el2.rs | 2 +- .../src/arch/aarch64/registers/vttbr_el2.rs | 2 +- kernel/src/arch/aarch64/virt/exit.rs | 115 ++++++----- kernel/src/arch/aarch64/virt/hyper.rs | 26 +-- kernel/src/arch/aarch64/virt/mmu_el2.rs | 36 ++-- kernel/src/arch/aarch64/virt/mmu_s2.rs | 68 ++++--- kernel/src/arch/aarch64/virt/mod.rs | 38 ++-- kernel/src/arch/aarch64/virt/vcpu.rs | 135 ++++++------- kernel/src/arch/aarch64/virt/vector.rs | 48 +++-- kernel/src/arch/aarch64/virt/vgic.rs | 183 +++++++++++------- kernel/src/arch/aarch64/virt/vtimer.rs | 48 +++-- 17 files changed, 392 insertions(+), 321 deletions(-) diff --git a/kernel/src/arch/aarch64/registers/hcr_el2.rs b/kernel/src/arch/aarch64/registers/hcr_el2.rs index 76b6e35e..d8f8dbc2 100644 --- a/kernel/src/arch/aarch64/registers/hcr_el2.rs +++ b/kernel/src/arch/aarch64/registers/hcr_el2.rs @@ -119,7 +119,7 @@ register_bitfields! {u64, Trap = 1 ], - /// TSW, bit [22] - Trap Data Cache instructions by Set/Way + /// TSW, bit [22] - Trap Data Cache instructions by Set/Way TSW OFFSET(22) NUMBITS(1) [ NoTrap = 0, Trap = 1 diff --git a/kernel/src/arch/aarch64/registers/mair_el2.rs b/kernel/src/arch/aarch64/registers/mair_el2.rs index 773746ff..5485676c 100644 --- a/kernel/src/arch/aarch64/registers/mair_el2.rs +++ b/kernel/src/arch/aarch64/registers/mair_el2.rs @@ -73,4 +73,4 @@ impl Writeable for MairEl2 { } } -pub const MAIR_EL2: MairEl2 = MairEl2 {}; \ No newline at end of file +pub const MAIR_EL2: MairEl2 = MairEl2 {}; diff --git a/kernel/src/arch/aarch64/registers/sctlr_el2.rs b/kernel/src/arch/aarch64/registers/sctlr_el2.rs index af34574c..f6d0d62e 100644 --- a/kernel/src/arch/aarch64/registers/sctlr_el2.rs +++ b/kernel/src/arch/aarch64/registers/sctlr_el2.rs @@ -291,4 +291,4 @@ impl Writeable for SctlrEl2 { } } -pub const SCTLR_EL2: SctlrEl2 = SctlrEl2 {}; \ No newline at end of file +pub const SCTLR_EL2: SctlrEl2 = SctlrEl2 {}; diff --git a/kernel/src/arch/aarch64/registers/spsr_el2.rs b/kernel/src/arch/aarch64/registers/spsr_el2.rs index a8a3c4f2..52a3848a 100644 --- a/kernel/src/arch/aarch64/registers/spsr_el2.rs +++ b/kernel/src/arch/aarch64/registers/spsr_el2.rs @@ -110,4 +110,4 @@ impl Writeable for SpsrEl2 { } } -pub const SPSR_EL2: SpsrEl2 = SpsrEl2 {}; \ No newline at end of file +pub const SPSR_EL2: SpsrEl2 = SpsrEl2 {}; diff --git a/kernel/src/arch/aarch64/registers/tcr_el2.rs b/kernel/src/arch/aarch64/registers/tcr_el2.rs index e45cb943..a7b120a1 100644 --- a/kernel/src/arch/aarch64/registers/tcr_el2.rs +++ b/kernel/src/arch/aarch64/registers/tcr_el2.rs @@ -101,4 +101,4 @@ impl Writeable for TcrEl2 { } } -pub const TCR_EL2: TcrEl2 = TcrEl2 {}; \ No newline at end of file +pub const TCR_EL2: TcrEl2 = TcrEl2 {}; diff --git a/kernel/src/arch/aarch64/registers/ttbr0_el2.rs b/kernel/src/arch/aarch64/registers/ttbr0_el2.rs index 0dbf5d6f..b6e715ce 100644 --- a/kernel/src/arch/aarch64/registers/ttbr0_el2.rs +++ b/kernel/src/arch/aarch64/registers/ttbr0_el2.rs @@ -64,4 +64,4 @@ impl Writeable for Ttbr0El2 { } } -pub const TTBR0_EL2: Ttbr0El2 = Ttbr0El2 {}; \ No newline at end of file +pub const TTBR0_EL2: Ttbr0El2 = Ttbr0El2 {}; diff --git a/kernel/src/arch/aarch64/registers/vtcr_el2.rs b/kernel/src/arch/aarch64/registers/vtcr_el2.rs index 022ffffc..c46e8d2f 100644 --- a/kernel/src/arch/aarch64/registers/vtcr_el2.rs +++ b/kernel/src/arch/aarch64/registers/vtcr_el2.rs @@ -104,4 +104,4 @@ impl Writeable for VtcrEl2 { } } -pub const VTCR_EL2: VtcrEl2 = VtcrEl2 {}; \ No newline at end of file +pub const VTCR_EL2: VtcrEl2 = VtcrEl2 {}; diff --git a/kernel/src/arch/aarch64/registers/vttbr_el2.rs b/kernel/src/arch/aarch64/registers/vttbr_el2.rs index 70031ea8..5c4fc08a 100644 --- a/kernel/src/arch/aarch64/registers/vttbr_el2.rs +++ b/kernel/src/arch/aarch64/registers/vttbr_el2.rs @@ -71,4 +71,4 @@ impl Writeable for VttbrEl2 { } } -pub const VTTBR_EL2: VttbrEl2 = VttbrEl2 {}; \ No newline at end of file +pub const VTTBR_EL2: VttbrEl2 = VttbrEl2 {}; diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index c8470fde..a4f6fadb 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +use super::{guest, hyper, vcpu::Vcpu, vgic}; use core::arch::asm; -use super::{vcpu::Vcpu, vgic, hyper, guest}; use semihosting::println; static mut GUEST_SHUTDOWN: bool = false; -pub const PSCI_VERSION: u32 = 0x8400_0000; -pub const PSCI_SYSTEM_OFF: u32 = 0x8400_0008; -pub const PSCI_SYSTEM_RESET: u32 = 0x8400_0009; -pub const PSCI_FEATURES: u32 = 0x8400_000A; -pub const HVC_VMM_GET_INFO: u64 = 0x11; -pub const HVC_VMM_INJECT_IRQ: u64 = 0x13; +pub const PSCI_VERSION: u32 = 0x8400_0000; +pub const PSCI_SYSTEM_OFF: u32 = 0x8400_0008; +pub const PSCI_SYSTEM_RESET: u32 = 0x8400_0009; +pub const PSCI_FEATURES: u32 = 0x8400_000A; +pub const HVC_VMM_GET_INFO: u64 = 0x11; +pub const HVC_VMM_INJECT_IRQ: u64 = 0x13; #[derive(Debug, Clone, Copy, PartialEq)] pub enum VmExitReason { @@ -47,10 +47,10 @@ pub struct VmExitInfo { #[inline] pub fn parse_exit_reason(esr: u64) -> VmExitReason { let ec = (esr >> 26) & 0x3F; - + match ec { 0x16 | 0x17 => VmExitReason::Hvc, - 0x15 => VmExitReason::Svc, + 0x15 => VmExitReason::Svc, 0x24 => VmExitReason::DataAbortLowerEL, 0x20 => VmExitReason::InstructionAbortLowerEL, 0x01 => VmExitReason::TrappedWfiWfe, @@ -71,26 +71,22 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { esr, far: elr, pstate, - return_addr: elr + 4, + return_addr: elr + 4, }; - + match reason { - VmExitReason::Hvc => { - handle_hvc(vcpu, &exit_info) - } - VmExitReason::Svc => { - handle_svc(vcpu, &exit_info) - } + VmExitReason::Hvc => handle_hvc(vcpu, &exit_info), + VmExitReason::Svc => handle_svc(vcpu, &exit_info), VmExitReason::DataAbortLowerEL => { semihosting::println!("[EXIT] Data Abort from Guest (Stage-2 Fault)"); let iss = esr & 0x1FFFFFF; let dfsc = iss & 0x3F; let is_write = (iss & (1 << 6)) != 0; - let faulting_pc = vcpu.context().elr_el2; + let faulting_pc = vcpu.context().elr_el2; if (dfsc & 0x3C) == 0x04 || (dfsc & 0x3C) == 0x08 || (dfsc & 0x3C) == 0x0C { // Translation fault (level 0/1/2/3) - Stage-2 未映射 - unsafe { + unsafe { let far: u64; core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); let hpfar_el2: u64; @@ -99,12 +95,12 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { let fault_ipa_base = (hpfar_el2 & 0x0000_00FF_FFFF_FFF0) << 8; let exact_ipa = fault_ipa_base | (far & 0xFFF); let handled = vgic::handle_data_abort( - vcpu.id(), - esr, - exact_ipa, - &mut vcpu.context_mut().regs - ); - + vcpu.id(), + esr, + exact_ipa, + &mut vcpu.context_mut().regs, + ); + if handled { vcpu.context_mut().elr_el2 += 4; vgic::flush(vcpu.id()); @@ -119,11 +115,11 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { VmExitReason::InstructionAbortLowerEL => { let iss = esr & 0x1FFFFFF; let ifsc = iss & 0x3F; - + if (ifsc & 0x3C) == 0x14 { - semihosting::println!("[EXIT] Stage-2 Translation Fault (Instruction)!"); + semihosting::println!("[EXIT] Stage-2 Translation Fault (Instruction)!"); } - false + false } VmExitReason::TrappedWfiWfe => { let iss = exit_info.esr & 0x1FFFFFF; @@ -131,14 +127,16 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { vcpu.context_mut().elr_el2 += 4; if is_wfe { - true + true } else { let irq_masked = (vcpu.context().spsr & (1 << 7)) != 0; - + if irq_masked { false } else { - unsafe { core::arch::asm!("wfi"); } + unsafe { + core::arch::asm!("wfi"); + } true } } @@ -158,20 +156,20 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { let hvc_num = info.esr & 0xFFFF; let mut need_advance_pc = true; let mut is_psci_call = false; - + // Easy HVC Services. let result = match hvc_num { 0x00 => { let psci_func_id = context.regs[0] as u32; is_psci_call = true; - + match psci_func_id { PSCI_VERSION => { let version = 0x0000_0002; context.regs[0] = version; } PSCI_SYSTEM_OFF | PSCI_SYSTEM_RESET => { - unsafe { + unsafe { GUEST_SHUTDOWN = true; } need_advance_pc = false; @@ -202,9 +200,9 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { true } }; - + if result && need_advance_pc { - context.elr_el2 += 4; + context.elr_el2 += 4; } result @@ -226,7 +224,7 @@ fn handle_svc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { context.regs[0] = 0xFFFFFFFF; } } - + context.elr_el2 = info.return_addr as u64; true } @@ -236,7 +234,9 @@ pub fn is_guest_shutdown() -> bool { } pub fn clear_guest_shutdown() { - unsafe { GUEST_SHUTDOWN = false; } + unsafe { + GUEST_SHUTDOWN = false; + } } #[inline] @@ -275,8 +275,14 @@ mod tests { fn test_parse_exit_reason_exhaustive() { assert_eq!(parse_exit_reason(0x16 << 26), VmExitReason::Hvc); assert_eq!(parse_exit_reason(0x15 << 26), VmExitReason::Svc); - assert_eq!(parse_exit_reason(0x24 << 26), VmExitReason::DataAbortLowerEL); - assert_eq!(parse_exit_reason(0x20 << 26), VmExitReason::InstructionAbortLowerEL); + assert_eq!( + parse_exit_reason(0x24 << 26), + VmExitReason::DataAbortLowerEL + ); + assert_eq!( + parse_exit_reason(0x20 << 26), + VmExitReason::InstructionAbortLowerEL + ); assert_eq!(parse_exit_reason(0x01 << 26), VmExitReason::TrappedWfiWfe); } @@ -284,18 +290,28 @@ mod tests { fn test_handle_hvc_pc_increment_and_psci() { let mut vcpu = Vcpu::new(0, 0x4000_0000, 0x4100_0000); let initial_pc = 0x4000_1000; - + // Scenario 1: HVC #0x11 (Get Information), should resume execution and PC + 4 vcpu.context_mut().elr_el2 = initial_pc; let info_normal = VmExitInfo { reason: VmExitReason::Hvc, esr: (0x16 << 26) | 0x11, - far: 0, pstate: 0, return_addr: initial_pc as usize + 4, + far: 0, + pstate: 0, + return_addr: initial_pc as usize + 4, }; let should_resume = handle_hvc(&mut vcpu, &info_normal); assert!(should_resume); - assert_eq!(vcpu.context().regs[0], 0x48495001, "Should set magic return value"); - assert_eq!(vcpu.context().elr_el2, initial_pc + 4, "PC MUST be incremented to avoid infinite loop!"); + assert_eq!( + vcpu.context().regs[0], + 0x48495001, + "Should set magic return value" + ); + assert_eq!( + vcpu.context().elr_el2, + initial_pc + 4, + "PC MUST be incremented to avoid infinite loop!" + ); // Scenario 2: PSCI SYSTEM OFF (HVC #0, X0 = 0x84000008), should shut down and refuse recovery. vcpu.context_mut().elr_el2 = initial_pc; @@ -304,12 +320,17 @@ mod tests { let info_shutdown = VmExitInfo { reason: VmExitReason::Hvc, esr: (0x16 << 26), - far: 0, pstate: 0, return_addr: initial_pc as usize + 4, + far: 0, + pstate: 0, + return_addr: initial_pc as usize + 4, }; clear_guest_shutdown(); let should_resume_shutdown = handle_hvc(&mut vcpu, &info_shutdown); - assert!(!should_resume_shutdown, "Should refuse to resume on PSCI Shutdown"); + assert!( + !should_resume_shutdown, + "Should refuse to resume on PSCI Shutdown" + ); assert!(is_guest_shutdown(), "Global shutdown flag must be set"); assert_eq!(vcpu.context().elr_el2, initial_pc); } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/hyper.rs b/kernel/src/arch/aarch64/virt/hyper.rs index 470bdd3d..fd05a823 100644 --- a/kernel/src/arch/aarch64/virt/hyper.rs +++ b/kernel/src/arch/aarch64/virt/hyper.rs @@ -13,11 +13,8 @@ // limitations under the License. use crate::arch::aarch64::{ - registers::hcr_el2::HCR_EL2, - registers::sctlr_el2::SCTLR_EL2, - registers::spsr_el2::SPSR_EL2, - virt::vector, - virt::mmu_el2 + registers::{hcr_el2::HCR_EL2, sctlr_el2::SCTLR_EL2, spsr_el2::SPSR_EL2}, + virt::{mmu_el2, vector}, }; use tock_registers::interfaces::{Readable, Writeable}; @@ -91,14 +88,14 @@ pub fn read_spsr_el2() -> u64 { #[inline] pub fn configure_hcr_el2_for_guest() { // Identity map 192MB of RAM starting from 0x4400_0000 so the Guest can run in-place - super::mmu_s2::init_stage2(0x4400_0000, 0x0c00_0000); + super::mmu_s2::init_stage2(0x4400_0000, 0x0c00_0000); HCR_EL2.write( HCR_EL2::VM::Enable + HCR_EL2::RW::EL1AArch64 + HCR_EL2::IMO::EL2Handled + HCR_EL2::FMO::EL2Handled + HCR_EL2::AMO::EL2Handled - + HCR_EL2::TSC::Trap + + HCR_EL2::TSC::Trap, ); unsafe { core::arch::asm!("isb"); @@ -118,11 +115,7 @@ fn configure_vector_table(vector_base: usize) { #[inline] fn configure_sctlr_el2() { - SCTLR_EL2.write( - SCTLR_EL2::M::Enable - + SCTLR_EL2::C::Cacheable - + SCTLR_EL2::I::Cacheable - ); + SCTLR_EL2.write(SCTLR_EL2::M::Enable + SCTLR_EL2::C::Cacheable + SCTLR_EL2::I::Cacheable); } #[inline] @@ -134,7 +127,7 @@ fn configure_timer_el2() { unsafe { core::arch::asm!("msr CNTHCTL_EL2, {}", in(reg) cnthctl); } - + // CNTVOFF_EL2: virtual timer offset register let cntvoff: u64 = 0; unsafe { @@ -144,10 +137,7 @@ fn configure_timer_el2() { #[inline] pub fn shutdown_guest() { - HCR_EL2.write( - HCR_EL2::RW::EL1AArch64 - + HCR_EL2::SWIO::Set - ); + HCR_EL2.write(HCR_EL2::RW::EL1AArch64 + HCR_EL2::SWIO::Set); unsafe { // Disable vGIC CPU interface to restore normal physical IRQ handling let mut ich_hcr: u64; @@ -186,4 +176,4 @@ pub fn hyp_init() { core::arch::asm!("dsb sy", options(nostack)); core::arch::asm!("isb sy", options(nostack)); } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/mmu_el2.rs b/kernel/src/arch/aarch64/virt/mmu_el2.rs index 593c620d..ae3cfe1a 100644 --- a/kernel/src/arch/aarch64/virt/mmu_el2.rs +++ b/kernel/src/arch/aarch64/virt/mmu_el2.rs @@ -12,14 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. - -use tock_registers::{interfaces::*, register_bitfields, registers::InMemoryRegister}; use crate::arch::aarch64::registers::{ - mair_el2::MAIR_EL2, - tcr_el2::TCR_EL2, - sctlr_el2::SCTLR_EL2, - ttbr0_el2::TTBR0_EL2, + mair_el2::MAIR_EL2, sctlr_el2::SCTLR_EL2, tcr_el2::TCR_EL2, ttbr0_el2::TTBR0_EL2, }; +use tock_registers::{interfaces::*, register_bitfields, registers::InMemoryRegister}; register_bitfields! {u64, pub PAGE_DESCRIPTOR_EL2 [ @@ -43,7 +39,9 @@ register_bitfields! {u64, pub struct PageEntry(u64); impl PageEntry { - const fn new() -> Self { Self(0) } + const fn new() -> Self { + Self(0) + } fn set(&mut self, output_addr: u64, device: bool) { let entry = InMemoryRegister::::new(0); @@ -51,11 +49,9 @@ impl PageEntry { + PAGE_DESCRIPTOR_EL2::AF::True + PAGE_DESCRIPTOR_EL2::TYPE::Block; if device { - val += PAGE_DESCRIPTOR_EL2::ATTRINDX.val(0) - + PAGE_DESCRIPTOR_EL2::XN::True; + val += PAGE_DESCRIPTOR_EL2::ATTRINDX.val(0) + PAGE_DESCRIPTOR_EL2::XN::True; } else { - val += PAGE_DESCRIPTOR_EL2::ATTRINDX.val(1) - + PAGE_DESCRIPTOR_EL2::SH::InnerShareable; + val += PAGE_DESCRIPTOR_EL2::ATTRINDX.val(1) + PAGE_DESCRIPTOR_EL2::SH::InnerShareable; } entry.write(val); self.0 = entry.get() | (output_addr & 0xFFFF_FFFF_C000_0000); @@ -76,8 +72,8 @@ impl El2PageTable { pub fn enable_el2_mmu() { unsafe { - EL2_PAGE_TABLE.0[0].set(0x0, true); // 0~1G: Device - EL2_PAGE_TABLE.0[1].set(0x4000_0000, false); // 1~2G: Normal + EL2_PAGE_TABLE.0[0].set(0x0, true); // 0~1G: Device + EL2_PAGE_TABLE.0[1].set(0x4000_0000, false); // 1~2G: Normal } // MAIR_EL2: Attr0=Device nGnRE, Attr1=Normal WB @@ -101,17 +97,15 @@ pub fn enable_el2_mmu() { + TCR_EL2::PS::Bits_32, ); - unsafe { + unsafe { core::arch::asm!("dsb sy", options(nostack, nomem)); core::arch::asm!("tlbi alle2", options(nostack, nomem)); core::arch::asm!("dsb sy", options(nostack, nomem)); core::arch::asm!("isb sy", options(nostack, nomem)); } - SCTLR_EL2.modify( - SCTLR_EL2::M::Enable - + SCTLR_EL2::C::Cacheable - + SCTLR_EL2::I::Cacheable, - ); - unsafe { core::arch::asm!("isb sy", options(nostack, nomem)); } -} \ No newline at end of file + SCTLR_EL2.modify(SCTLR_EL2::M::Enable + SCTLR_EL2::C::Cacheable + SCTLR_EL2::I::Cacheable); + unsafe { + core::arch::asm!("isb sy", options(nostack, nomem)); + } +} diff --git a/kernel/src/arch/aarch64/virt/mmu_s2.rs b/kernel/src/arch/aarch64/virt/mmu_s2.rs index fd192780..673aaf7a 100644 --- a/kernel/src/arch/aarch64/virt/mmu_s2.rs +++ b/kernel/src/arch/aarch64/virt/mmu_s2.rs @@ -12,15 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - -use crate::arch::aarch64::{ - registers::{ - vtcr_el2::VTCR_EL2, - vttbr_el2::VTTBR_EL2, - }, -}; -use tock_registers::interfaces::*; +use crate::arch::aarch64::registers::{vtcr_el2::VTCR_EL2, vttbr_el2::VTTBR_EL2}; use semihosting::println; +use tock_registers::interfaces::*; // Structure of Page Table. #[repr(align(4096))] @@ -33,20 +27,24 @@ static mut POOL_INDEX: usize = 0; static mut S2_L1: S2PageTable = S2PageTable([0; 512]); // Stage-2 Descriptor -const S2_DESC_TABLE: u64 = 3; -const S2_DESC_PAGE: u64 = 3; -const S2_ATTR_NORMAL: u64 = 0xF << 2; -const S2_ATTR_DEVICE: u64 = 1 << 2; -const S2_ATTR_S2AP_RW: u64 = 3 << 6; +const S2_DESC_TABLE: u64 = 3; +const S2_DESC_PAGE: u64 = 3; +const S2_ATTR_NORMAL: u64 = 0xF << 2; +const S2_ATTR_DEVICE: u64 = 1 << 2; +const S2_ATTR_S2AP_RW: u64 = 3 << 6; const S2_ATTR_SH_INNER: u64 = 3 << 8; -const S2_ATTR_AF: u64 = 1 << 10; +const S2_ATTR_AF: u64 = 1 << 10; fn alloc_page_table() -> Option<&'static mut S2PageTable> { unsafe { - if POOL_INDEX >= POOL_SIZE { return None; } + if POOL_INDEX >= POOL_SIZE { + return None; + } let table = &mut PAGE_TABLE_POOL[POOL_INDEX]; POOL_INDEX += 1; - for e in table.0.iter_mut() { *e = 0; } + for e in table.0.iter_mut() { + *e = 0; + } let start = table as *const _ as usize; for addr in (start..start + core::mem::size_of::()).step_by(64) { core::arch::asm!("dc civac, {}", in(reg) addr); @@ -86,8 +84,14 @@ fn map_page(ipa: usize, pa: usize, device: bool) { } }; - let attr = S2_ATTR_S2AP_RW | S2_ATTR_AF | S2_ATTR_SH_INNER - | if device { S2_ATTR_DEVICE } else { S2_ATTR_NORMAL }; + let attr = S2_ATTR_S2AP_RW + | S2_ATTR_AF + | S2_ATTR_SH_INNER + | if device { + S2_ATTR_DEVICE + } else { + S2_ATTR_NORMAL + }; l3_table.0[l3_idx] = (pa as u64 & 0xFFFFFFFFF000) | attr | S2_DESC_PAGE; let addr = &l3_table.0[l3_idx] as *const _ as usize; @@ -105,7 +109,9 @@ pub fn map_range(ipa: usize, pa: usize, size: usize, device: bool) { } pub fn init_stage2(ipa_base: usize, size: usize) { - unsafe { POOL_INDEX = 0; } + unsafe { + POOL_INDEX = 0; + } map_range(ipa_base, ipa_base, size, false); // for guest visting uart @@ -121,7 +127,7 @@ pub fn init_stage2(ipa_base: usize, size: usize) { core::arch::asm!("dc civac, {}", in(reg) addr); } let pool_start = &PAGE_TABLE_POOL as *const _ as usize; - let pool_end = pool_start + core::mem::size_of_val(&PAGE_TABLE_POOL); + let pool_end = pool_start + core::mem::size_of_val(&PAGE_TABLE_POOL); for addr in (pool_start..pool_end).step_by(32) { core::arch::asm!("dc civac, {}", in(reg) addr); } @@ -156,11 +162,13 @@ mod tests { #[test] fn test_page_table_allocation() { - unsafe { POOL_INDEX = 0; } - + unsafe { + POOL_INDEX = 0; + } + let table1 = alloc_page_table(); assert!(table1.is_some(), "Should allocate first table"); - + unsafe { let idx = POOL_INDEX; assert_eq!(idx, 1); @@ -171,7 +179,7 @@ mod tests { fn test_stage2_attributes_generation() { // Normal Memory should be 0xF << 2 (Inner & Outer WB Cacheable) // Device Memory should be 0x1 << 2 (Device-nGnRE) - + let attr_normal = S2_ATTR_S2AP_RW | S2_ATTR_AF | S2_ATTR_SH_INNER | S2_ATTR_NORMAL; let attr_device = S2_ATTR_S2AP_RW | S2_ATTR_AF | S2_ATTR_SH_INNER | S2_ATTR_DEVICE; @@ -179,7 +187,13 @@ mod tests { let mem_attr_normal = (attr_normal >> 2) & 0xF; let mem_attr_device = (attr_device >> 2) & 0xF; - assert_eq!(mem_attr_normal, 0xF, "Normal memory attribute must be 0b1111 to prevent Alignment Faults"); - assert_eq!(mem_attr_device, 0x1, "Device memory attribute must be 0b0001"); + assert_eq!( + mem_attr_normal, 0xF, + "Normal memory attribute must be 0b1111 to prevent Alignment Faults" + ); + assert_eq!( + mem_attr_device, 0x1, + "Device memory attribute must be 0b0001" + ); } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index c42d693a..e6951b38 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -14,20 +14,20 @@ pub mod exit; pub mod guest; +pub mod hyper; pub mod mmu_el2; pub mod mmu_s2; -pub mod hyper; pub mod vcpu; pub mod vector; pub mod vgic; pub mod vtimer; -pub use exit::{VmExitReason, VmExitInfo}; +pub use crate::arch::aarch64::psci::hvc_call; +use blueos_hal::PlatPeri; +pub use exit::{VmExitInfo, VmExitReason}; pub use hyper::{get_current_el, hyp_init}; +use semihosting::println; pub use vcpu::{Vcpu, VcpuManager, VcpuState}; pub use vgic::init; -pub use crate::arch::aarch64::psci::hvc_call; -use semihosting::println; -use blueos_hal::PlatPeri; // PL011 UART addresses for QEMU Virt const UART0_DR: *mut u32 = 0x0900_0000 as *mut u32; @@ -36,10 +36,10 @@ const UART0_FR: *mut u32 = 0x0900_0018 as *mut u32; // Temporary placeholder #[no_mangle] pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) -> usize { - unsafe{ + unsafe { let mut ctlr: u64; core::arch::asm!("mrs {}, ICC_CTLR_EL1", out(reg) ctlr); - if (ctlr & (1 << 1)) == 0{ + if (ctlr & (1 << 1)) == 0 { // set EOImode. ctlr |= 1 << 1; core::arch::asm!("msr ICC_CTLR_EL1, {}", in(reg) ctlr); @@ -60,7 +60,7 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - if intid == 33 { unsafe { let lr_val: u64 = (1 << 62) | (1 << 61) | (1 << 60) | (0xA0 << 48) | (33 << 32) | 33; - + // Temporarily occupy physical register for uart print in Linux shell core::arch::asm!("msr ICH_LR1_EL2, {}", in(reg) lr_val); let mut hcr: u64; @@ -77,19 +77,19 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - } if let Some(vcpu_id) = get_current_vcpu_id() { - vgic::inject_irq(vcpu_id, 27); + vgic::inject_irq(vcpu_id, 27); } - unsafe{ + unsafe { core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); } } else { semihosting::println!("[EL2] Unhandled Guest IRQ: {}", intid); - // For uninterruptible/unknown interrupts, - // we must manually downgrade and deactivate them; + // For uninterruptible/unknown interrupts, + // we must manually downgrade and deactivate them; // otherwise, the interrupt line will be permanently blocked. - unsafe { - core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + unsafe { + core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); } } @@ -113,8 +113,7 @@ pub extern "C" fn hyper_trap_fiq(_context: &mut crate::arch::aarch64::Context) - #[repr(align(16))] pub struct VcpuManagerWrapper(pub vcpu::VcpuManager); -pub static mut VCPU_MANAGER: VcpuManagerWrapper = - VcpuManagerWrapper(vcpu::VcpuManager::new()); +pub static mut VCPU_MANAGER: VcpuManagerWrapper = VcpuManagerWrapper(vcpu::VcpuManager::new()); #[inline] pub fn get_current_vcpu_id() -> Option { @@ -132,7 +131,10 @@ pub fn virt_boot_linux() { // Initiate vCpu for Linux kernel, set the entry point and parameters. unsafe { - let vcpu = VCPU_MANAGER.0.create_vcpu(0, guest::LINUX_KERNEL_LOAD_ADDR, 0).unwrap(); + let vcpu = VCPU_MANAGER + .0 + .create_vcpu(0, guest::LINUX_KERNEL_LOAD_ADDR, 0) + .unwrap(); let mut ctx = vcpu.context_mut(); ctx.regs[0] = guest::LINUX_DTB_ADDR as u64; ctx.spsr = 0x3C5; @@ -150,4 +152,4 @@ pub fn virt_boot_linux() { uart.enable(); semihosting::println!("Linux shutdown!!!"); } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs index bc1a176a..548c4b5c 100644 --- a/kernel/src/arch/aarch64/virt/vcpu.rs +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -use core::arch::asm; use super::{ - hyper::{read_hcr_el2, write_hcr_el2}, vgic + hyper::{read_hcr_el2, write_hcr_el2}, + vgic, }; +use core::arch::asm; /// HCR_EL2_VI: Enable virtual IRQ. const HCR_EL2_VI: u64 = 1 << 7; @@ -42,18 +43,18 @@ pub struct VcpuStateStruct { pub pstate: u64, pub spsr: u64, pub vbar_el1: u64, - pub sctlr_el1: u64, - pub ttbr0_el1: u64, - pub ttbr1_el1: u64, - pub tcr_el1: u64, - pub mair_el1: u64, - pub elr_el1: u64, - pub spsr_el1: u64, - pub sp_el0: u64, - pub tpidr_el0: u64, - pub tpidr_el1: u64, - pub esr_el1: u64, - pub far_el1: u64, + pub sctlr_el1: u64, + pub ttbr0_el1: u64, + pub ttbr1_el1: u64, + pub tcr_el1: u64, + pub mair_el1: u64, + pub elr_el1: u64, + pub spsr_el1: u64, + pub sp_el0: u64, + pub tpidr_el0: u64, + pub tpidr_el1: u64, + pub esr_el1: u64, + pub far_el1: u64, } impl VcpuStateStruct { @@ -61,12 +62,12 @@ impl VcpuStateStruct { pub fn new() -> Self { Self::default() } - + #[inline] pub fn is_valid(&self) -> bool { self.elr_el2 != 0 && self.sp != 0 } - + #[inline] pub fn reset(&mut self) { self.regs = [0; 31]; @@ -76,22 +77,22 @@ impl VcpuStateStruct { self.spsr = 0; self.vbar_el1 = 0; } - + #[inline] pub fn elr(&self) -> u64 { self.elr_el2 } - + #[inline] pub fn set_elr(&mut self, elr: u64) { self.elr_el2 = elr; } - + #[inline] pub fn spsr(&self) -> u64 { self.spsr } - + #[inline] pub fn set_spsr(&mut self, spsr: u64) { self.spsr = spsr; @@ -117,7 +118,7 @@ impl Vcpu { context.spsr = 0x3C5; // Default PSTATE: EL1h, DAIF masked context.vbar_el1 = (entry as u64 + 0x1000) & !0x7FF; // context.sp = stack_top as u64; - + Self { id, state: VcpuState::Stopped, @@ -128,32 +129,32 @@ impl Vcpu { pending_fiq: false, } } - + #[inline] pub fn id(&self) -> usize { self.id } - + #[inline] pub fn entry_point(&self) -> usize { self.entry } - + #[inline] pub fn stack_top(&self) -> usize { self.stack_top } - + #[inline] pub fn state(&self) -> VcpuState { self.state } - + #[inline] pub fn context(&self) -> &VcpuStateStruct { &self.context } - + #[inline] pub fn context_mut(&mut self) -> &mut VcpuStateStruct { &mut self.context @@ -178,27 +179,27 @@ impl Vcpu { pub fn set_pending_fiq(&mut self, val: bool) { self.pending_fiq = val; } - + #[inline] pub fn elr(&self) -> u64 { self.context.elr_el2 } - + #[inline] pub fn set_entry(&mut self, entry: usize) { self.entry = entry; } - + #[inline] pub fn set_stack_top(&mut self, stack_top: usize) { self.stack_top = stack_top; } - + #[inline] pub fn set_state(&mut self, state: VcpuState) { self.state = state; } - + pub fn prepare_run(&mut self) { if self.state == VcpuState::Stopped { #[cfg(not(test))] @@ -206,13 +207,13 @@ impl Vcpu { } self.state = VcpuState::Running; } - + pub fn inject_irq(&mut self) { self.pending_irq = true; let hcr = read_hcr_el2(); write_hcr_el2(hcr | HCR_EL2_VI); - unsafe{ + unsafe { core::arch::asm!("isb", options(nomem, nostack)); } } @@ -222,16 +223,16 @@ impl Vcpu { let hcr = read_hcr_el2(); write_hcr_el2(hcr | HCR_EL2_VF); - unsafe{ + unsafe { core::arch::asm!("isb", options(nomem, nostack)); } } #[inline] pub fn can_run(&self) -> bool { - self.state == VcpuState::Stopped || - self.state == VcpuState::Paused || - self.state == VcpuState::Exited + self.state == VcpuState::Stopped + || self.state == VcpuState::Paused + || self.state == VcpuState::Exited } } @@ -273,12 +274,12 @@ impl VcpuManager { host_sctlr: 0, } } - + pub fn create_vcpu( - &mut self, - id: usize, - entry: usize, - stack_top: usize + &mut self, + id: usize, + entry: usize, + stack_top: usize, ) -> Result<&mut Vcpu, &'static str> { if id >= 4 { return Err("vCPU ID out of range (max 3)"); @@ -288,27 +289,27 @@ impl VcpuManager { if self.count >= 4 { return Err("Reached max vCPU count"); } - + if self.vcpus[id].is_some() { return Err("vCPU ID already used"); } - + let vcpu = Vcpu::new(id, entry, stack_top); self.vcpus[id] = Some(vcpu); self.count += 1; - + Ok(self.vcpus[id].as_mut().unwrap()) } - + pub fn clear_current_vcpu(&mut self) { self.current_vcpu = None; } - + #[inline] pub fn get_vcpu(&mut self, id: usize) -> Option<&mut Vcpu> { self.vcpus[id].as_mut() } - + #[inline] pub fn current_vcpu_id(&self) -> Option { self.current_vcpu @@ -318,17 +319,17 @@ impl VcpuManager { pub fn set_current_vcpu(&mut self, id: usize) { self.current_vcpu = Some(id); } - + #[inline] pub fn vcpu_count(&self) -> usize { self.count } - + #[inline] pub fn has_running(&self) -> bool { self.current_vcpu.is_some() } - + pub fn iter(&mut self) -> impl Iterator { self.vcpus .iter_mut() @@ -349,16 +350,11 @@ pub enum VcpuError { impl core::fmt::Display for VcpuError { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { match self { - VcpuError::IdOutOfRange => - write!(f, "vCPU ID out of range (max 3)"), - VcpuError::MaxLimitReached => - write!(f, "Reached max vCPU count"), - VcpuError::IdAlreadyUsed => - write!(f, "vCPU ID already used"), - VcpuError::NotFound => - write!(f, "vCPU not found"), - VcpuError::InvalidState => - write!(f, "vCPU state invalid for this operation"), + VcpuError::IdOutOfRange => write!(f, "vCPU ID out of range (max 3)"), + VcpuError::MaxLimitReached => write!(f, "Reached max vCPU count"), + VcpuError::IdAlreadyUsed => write!(f, "vCPU ID already used"), + VcpuError::NotFound => write!(f, "vCPU not found"), + VcpuError::InvalidState => write!(f, "vCPU state invalid for this operation"), } } } @@ -391,7 +387,9 @@ mod tests { assert_eq!(manager.vcpu_count(), 0); assert!(!manager.has_running()); - let vcpu0 = manager.create_vcpu(0, 0x4000_0000, 0x4100_0000).expect("Failed to create vCPU 0"); + let vcpu0 = manager + .create_vcpu(0, 0x4000_0000, 0x4100_0000) + .expect("Failed to create vCPU 0"); assert_eq!(vcpu0.id(), 0); assert_eq!(vcpu0.entry_point(), 0x4000_0000); assert_eq!(vcpu0.state(), VcpuState::Stopped); @@ -412,12 +410,15 @@ mod tests { fn test_vcpu_context_switch_preparation() { let mut manager = VcpuManager::new(); manager.create_vcpu(0, 0x4000_0000, 0x4100_0000).unwrap(); - + let vcpu = manager.get_vcpu(0).unwrap(); assert!(vcpu.can_run()); - + vcpu.prepare_run(); assert_eq!(vcpu.state(), VcpuState::Running); - assert!(!vcpu.can_run(), "Running vCPU should not be marked as can_run to prevent re-entry"); + assert!( + !vcpu.can_run(), + "Running vCPU should not be marked as can_run to prevent re-entry" + ); } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index a26293fc..bf4e99e0 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -13,14 +13,10 @@ // limitations under the License. use super::{ - VCPU_MANAGER, - guest, - hyper, + exit::{clear_guest_shutdown, handle_vm_exit, is_guest_shutdown}, + guest, hyper, vcpu::Vcpu, - vgic, - exit::{ - handle_vm_exit, is_guest_shutdown, clear_guest_shutdown - } + vgic, VCPU_MANAGER, }; use core::arch::asm; @@ -178,7 +174,7 @@ pub unsafe extern "C" fn sync_from_lower_el1() { #[no_mangle] pub unsafe extern "C" fn sync_from_lower_el1_rust(frame: *mut u64) -> u64 { - if let Some(vcpu_id) = VCPU_MANAGER.0.current_vcpu_id() { + if let Some(vcpu_id) = VCPU_MANAGER.0.current_vcpu_id() { handle_guest_request(vcpu_id, frame) } else { handle_host_request(frame) @@ -196,35 +192,35 @@ unsafe fn handle_guest_request(vcpu_id: usize, frame: *mut u64) -> u64 { hyper::shutdown_guest(); VCPU_MANAGER.0.clear_current_vcpu(); restore_host_to_frame(frame); - 2 + 2 } else { restore_context_to_frame(vcpu, frame); vgic::flush(vcpu_id); - 1 + 1 } } unsafe fn handle_host_request(frame: *mut u64) -> u64 { let func_id = *frame.add(0); - + match func_id { 0x02 => { let target_id = *frame.add(1) as usize; if let Some(vcpu) = VCPU_MANAGER.0.get_vcpu(target_id) { - save_host_context(frame); + save_host_context(frame); hyper::configure_hcr_el2_for_guest(); vcpu.prepare_run(); VCPU_MANAGER.0.set_current_vcpu(target_id); restore_context_to_frame(vcpu, frame); - 1 + 1 } else { *frame.add(0) = 1; 1 } } _ => { - *frame.add(0) = 0; - 1 + *frame.add(0) = 0; + 1 } } } @@ -233,7 +229,7 @@ unsafe fn save_host_context(frame: *mut u64) { VCPU_MANAGER.0.host_elr = *frame.add(31) + 4; VCPU_MANAGER.0.host_spsr = *frame.add(32); VCPU_MANAGER.0.host_sp = *frame.add(33); - for i in 0..31 { + for i in 0..31 { VCPU_MANAGER.0.host_regs[i] = *frame.add(i); } @@ -269,7 +265,7 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { for i in 0..31 { *frame.add(i) = VCPU_MANAGER.0.host_regs[i]; } - + // Pass success code (0) back to host's x0 *frame.add(0) = 0; @@ -289,7 +285,7 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { "msr mair_el1, {mair}", "isb", "tlbi alle1", - "dsb sy", + "dsb sy", "isb", vbar = in(reg) vbar, sctlr = in(reg) sctlr, @@ -328,14 +324,16 @@ unsafe fn save_frame_to_context(frame: *mut u64, vcpu: &mut Vcpu) { ctx.sctlr_el1 = sctlr; ctx.ttbr0_el1 = ttbr0; ctx.ttbr1_el1 = ttbr1; - ctx.tcr_el1 = tcr; - ctx.mair_el1 = mair; - ctx.vbar_el1 = vbar; + ctx.tcr_el1 = tcr; + ctx.mair_el1 = mair; + ctx.vbar_el1 = vbar; } unsafe fn restore_context_to_frame(vcpu: &mut Vcpu, frame: *mut u64) { let ctx = vcpu.context(); - for i in 0..31 { *frame.add(i) = ctx.regs[i]; } + for i in 0..31 { + *frame.add(i) = ctx.regs[i]; + } *frame.add(31) = ctx.elr_el2; *frame.add(32) = ctx.spsr; *frame.add(33) = ctx.sp; @@ -343,11 +341,11 @@ unsafe fn restore_context_to_frame(vcpu: &mut Vcpu, frame: *mut u64) { // while booting linux, mmu should closed. core::arch::asm!( "msr vbar_el1, {vbar}", - "msr ttbr0_el1, {ttbr0}", + "msr ttbr0_el1, {ttbr0}", "msr ttbr1_el1, {ttbr1}", "msr tcr_el1, {tcr}", "msr mair_el1, {mair}", - "msr sctlr_el1, {sctlr}", + "msr sctlr_el1, {sctlr}", "isb", vbar = in(reg) ctx.vbar_el1, ttbr0 = in(reg) ctx.ttbr0_el1, @@ -357,7 +355,7 @@ unsafe fn restore_context_to_frame(vcpu: &mut Vcpu, frame: *mut u64) { sctlr = in(reg) ctx.sctlr_el1, options(nostack) ); - + let target_vcpu_id = vcpu.id(); vgic::flush(target_vcpu_id); } diff --git a/kernel/src/arch/aarch64/virt/vgic.rs b/kernel/src/arch/aarch64/virt/vgic.rs index f81a7c77..9332b138 100644 --- a/kernel/src/arch/aarch64/virt/vgic.rs +++ b/kernel/src/arch/aarch64/virt/vgic.rs @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -use core::arch::asm; use super::VCPU_MANAGER; use crate::sync::SpinLock; +use core::arch::asm; use spin::Once; const MAX_LR: usize = 4; @@ -44,7 +44,7 @@ pub struct VgicRedistributor { pub ispendr0: u32, pub isactiver0: u32, pub ipriorityr0: [u32; 8], - + // Virq injection queue. pub pending_irqs: [u32; MAX_PENDING], pub pending_head: usize, @@ -82,12 +82,14 @@ impl VgicRedistributor { } pub fn push_queue(&mut self, intid: u32) { - if self.pending_count >= MAX_PENDING { return; } - + if self.pending_count >= MAX_PENDING { + return; + } + // In case of existing same Intid. let mut curr = self.pending_head; for _ in 0..self.pending_count { - if self.pending_irqs[curr] == intid { + if self.pending_irqs[curr] == intid { return; } curr = (curr + 1) % MAX_PENDING; @@ -174,30 +176,36 @@ pub fn handle_data_abort(vcpu_id: usize, esr: u64, far: u64, regs: &mut [u64; 31 } } -fn emulate_access(target_vcpu: usize, access: &MmioAccess, offset: u64, regs: &mut [u64; 31], is_dist: bool) { - let is_zero_reg = access.reg_index == 31; - +fn emulate_access( + target_vcpu: usize, + access: &MmioAccess, + offset: u64, + regs: &mut [u64; 31], + is_dist: bool, +) { + let is_zero_reg = access.reg_index == 31; + if access.is_write { let val = if is_zero_reg { - 0 - } else { - (regs[access.reg_index] & 0xFFFFFFFF) as u32 + 0 + } else { + (regs[access.reg_index] & 0xFFFFFFFF) as u32 }; - if is_dist { - handle_gicd_write(offset, val); - } else { - handle_gicr_write(target_vcpu, offset, val); + if is_dist { + handle_gicd_write(offset, val); + } else { + handle_gicr_write(target_vcpu, offset, val); } } else { - let val = if is_dist { - handle_gicd_read(offset) - } else { - handle_gicr_read(target_vcpu, offset) + let val = if is_dist { + handle_gicd_read(offset) + } else { + handle_gicr_read(target_vcpu, offset) }; - if !is_zero_reg { - regs[access.reg_index] = val as u64; + if !is_zero_reg { + regs[access.reg_index] = val as u64; } } } @@ -214,7 +222,9 @@ fn handle_gicd_write(offset: u64, val: u32) { } fn handle_gicr_write(vcpu_id: usize, offset: u64, val: u32) { - if vcpu_id >= MAX_VCPUS { return; } + if vcpu_id >= MAX_VCPUS { + return; + } let mut redist = get_vgic().redists[vcpu_id].lock(); match offset { 0x10100 => redist.isenabler0 |= val, @@ -235,23 +245,23 @@ fn handle_gicd_read(offset: u64) -> u32 { 0x0004 => 0x00000006, // 0x43B represent Arm Ltd. 0x0008 => 0x43B00000, - 0x0100..= 0x017C => dist.isenabler[((offset - 0x0100) / 4) as usize], - 0x0180..= 0x01FC => dist.isenabler[((offset - 0x0180) / 4) as usize], - 0x0400..= 0x07F8 => dist.ipriorityr[(((offset - 0x0400) / 4) as usize)], - 0xFFE8 => 0x00000030, + 0x0100..=0x017C => dist.isenabler[((offset - 0x0100) / 4) as usize], + 0x0180..=0x01FC => dist.isenabler[((offset - 0x0180) / 4) as usize], + 0x0400..=0x07F8 => dist.ipriorityr[(((offset - 0x0400) / 4) as usize)], + 0xFFE8 => 0x00000030, _ => 0, } } fn handle_gicr_read(vcpu_id: usize, offset: u64) -> u32 { - if vcpu_id >= MAX_VCPUS { return 0; } + if vcpu_id >= MAX_VCPUS { + return 0; + } let redist = get_vgic().redists[vcpu_id].lock(); match offset { 0x0008 => { let mut val = (vcpu_id as u32) << 8; - let active_vcpus = unsafe { - VCPU_MANAGER.0.vcpu_count() - }; + let active_vcpus = unsafe { VCPU_MANAGER.0.vcpu_count() }; if vcpu_id == active_vcpus - 1 { val |= 1 << 4; // Last Redistributor } @@ -278,30 +288,32 @@ pub fn cpu_init(vcpu_id: usize) { // 1. Enable System Register access for EL2 (ICC_SRE_EL2) let mut sre: u64; asm!("mrs {}, ICC_SRE_EL2", out(reg) sre); - if (sre & 0x9) != 0x9 { - sre |= 0x9; - asm!("msr ICC_SRE_EL2, {}", in(reg) sre); - asm!("isb"); + if (sre & 0x9) != 0x9 { + sre |= 0x9; + asm!("msr ICC_SRE_EL2, {}", in(reg) sre); + asm!("isb"); } // 2. Enable vGIC - let hcr: u64 = 1; + let hcr: u64 = 1; asm!("msr ICH_HCR_EL2, {}", in(reg) hcr); // 3. Configure VMCR (Group 0/1 Enable) let vmcr: u64 = 0x3; asm!("msr ICH_VMCR_EL2, {}", in(reg) vmcr); - + // Clear all LRs for i in 0..MAX_LR { - write_lr(i, 0); + write_lr(i, 0); } } } pub fn inject(vcpu_id: usize, intid: u32) { unsafe { - if vcpu_id >= MAX_VCPUS || intid >= 1024 { return; } + if vcpu_id >= MAX_VCPUS || intid >= 1024 { + return; + } let mut is_enabled; if intid < 32 { let mut redist = get_vgic().redists[vcpu_id].lock(); @@ -309,7 +321,6 @@ pub fn inject(vcpu_id: usize, intid: u32) { if (redist.isenabler0 & (1 << intid)) != 0 { redist.push_queue(intid); } - } else { // SPI let mut dist = get_vgic().dist.lock(); @@ -317,12 +328,12 @@ pub fn inject(vcpu_id: usize, intid: u32) { let mask = 1 << (intid % 32); dist.ispendr[idx] |= mask; is_enabled = (dist.isenabler[idx] & mask) != 0; - + // Temporarily set for int 33 to get shell. - if intid == 33{ + if intid == 33 { is_enabled = true; - } - drop(dist); + } + drop(dist); if is_enabled { let mut redist = get_vgic().redists[vcpu_id].lock(); @@ -333,39 +344,46 @@ pub fn inject(vcpu_id: usize, intid: u32) { } pub fn flush(vcpu_id: usize) { - if vcpu_id >= MAX_VCPUS { - return; + if vcpu_id >= MAX_VCPUS { + return; } - + let mut redist = get_vgic().redists[vcpu_id].lock(); - + unsafe { let mut current_lr = 0; while redist.pending_count > 0 && current_lr < MAX_LR { let intid = redist.pending_irqs[redist.pending_head]; - + redist.pending_head = (redist.pending_head + 1) % MAX_PENDING; redist.pending_count -= 1; - + let is_active = is_irq_active_locked(&redist, intid); let state_bits: u64 = if is_active { 0b11 } else { 0b01 }; // Temporarily set priority(0xA0 << 48) // To DO: Dynamically set hw bit(61) according to irq routing. - let lr_val: u64 = (state_bits << 62) | (1 << 61) | (1 << 60) | (0xA0 << 48) | ((intid as u64) << 32) | (intid as u64); + let lr_val: u64 = (state_bits << 62) + | (1 << 61) + | (1 << 60) + | (0xA0 << 48) + | ((intid as u64) << 32) + | (intid as u64); write_lr(current_lr, lr_val); - + current_lr += 1; } - + redist.used_lrs = current_lr; } } pub fn sync(vcpu_id: usize) { - if vcpu_id >= MAX_VCPUS { return; } - + if vcpu_id >= MAX_VCPUS { + return; + } + let mut redist = get_vgic().redists[vcpu_id].lock(); - + unsafe { for i in 0..redist.used_lrs { let lr_val = read_lr(i); @@ -373,12 +391,12 @@ pub fn sync(vcpu_id: usize) { let intid = (lr_val & 0xFFFFFFFF) as u32; if state == 0 { - clear_irq_state_locked(&mut redist, intid); + clear_irq_state_locked(&mut redist, intid); } else { sync_irq_state_locked(&mut redist, intid, state); redist.push_queue(intid); } - + write_lr(i, 0); } @@ -387,7 +405,7 @@ pub fn sync(vcpu_id: usize) { } pub fn inject_irq(vcpu_id: usize, intid: u32) { - if vcpu_id >= MAX_VCPUS { + if vcpu_id >= MAX_VCPUS { return; } unsafe { @@ -407,7 +425,7 @@ unsafe fn read_lr(index: usize) -> u64 { { if index < MAX_LR { MOCK_LR[index] - }else { + } else { 0 } } @@ -464,14 +482,30 @@ fn sync_irq_state_locked(redist: &mut VgicRedistributor, intid: u32, state: u64) let is_active = (state & 0b10) != 0; if intid < 32 { - if is_pending { redist.ispendr0 |= 1 << intid; } else { redist.ispendr0 &= !(1 << intid); } - if is_active { redist.isactiver0 |= 1 << intid; } else { redist.isactiver0 &= !(1 << intid); } + if is_pending { + redist.ispendr0 |= 1 << intid; + } else { + redist.ispendr0 &= !(1 << intid); + } + if is_active { + redist.isactiver0 |= 1 << intid; + } else { + redist.isactiver0 &= !(1 << intid); + } } else { let mut dist = get_vgic().dist.lock(); let idx = (intid / 32) as usize; let mask = 1 << (intid % 32); - if is_pending { dist.ispendr[idx] |= mask; } else { dist.ispendr[idx] &= !mask; } - if is_active { dist.isactiver[idx] |= mask; } else { dist.isactiver[idx] &= !mask; } + if is_pending { + dist.ispendr[idx] |= mask; + } else { + dist.ispendr[idx] &= !mask; + } + if is_active { + dist.isactiver[idx] |= mask; + } else { + dist.isactiver[idx] &= !mask; + } } } @@ -491,7 +525,7 @@ mod tests { fn setup_test_env() { unsafe { - super::MOCK_LR.fill(0); + super::MOCK_LR.fill(0); } } @@ -499,7 +533,7 @@ mod tests { fn test_vgic_queue_deduplication() { setup_test_env(); let mut redist = VgicRedistributor::new(); - + // verify normal queuing. redist.push_queue(27); redist.push_queue(30); @@ -509,15 +543,18 @@ mod tests { // verify deadlock prevention optimization: ghost reuse deduplication. redist.push_queue(27); - assert_eq!(redist.pending_count, 2, "Duplicate interrupt should be ignored!"); + assert_eq!( + redist.pending_count, 2, + "Duplicate interrupt should be ignored!" + ); } #[test] fn test_vgic_flush_and_sync_lifecycle() { setup_test_env(); - init(); + init(); let vcpu_id = 0; - + // Simulating enabling 27 interrupt { let mut redist = get_vgic().redists[vcpu_id].lock(); @@ -547,7 +584,7 @@ mod tests { } // Simulating hardware EOI. - unsafe { + unsafe { super::MOCK_LR[0] = 0; } sync(vcpu_id); @@ -555,7 +592,11 @@ mod tests { { let redist = get_vgic().redists[vcpu_id].lock(); assert_eq!(redist.used_lrs, 0, "Used LRs should be reset"); - assert_eq!((redist.isactiver0 & (1 << 27)), 0, "Active state should be cleared"); + assert_eq!( + (redist.isactiver0 & (1 << 27)), + 0, + "Active state should be cleared" + ); } } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/vtimer.rs b/kernel/src/arch/aarch64/virt/vtimer.rs index a7d99266..6e0f47fc 100644 --- a/kernel/src/arch/aarch64/virt/vtimer.rs +++ b/kernel/src/arch/aarch64/virt/vtimer.rs @@ -12,18 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. +use super::vgic; use crate::arch::aarch64::{ current_cpu_id, - irq::{self, IrqHandler, IrqNumber, Priority} + irq::{self, IrqNumber, Priority}, }; -use super::vgic; use alloc::boxed::Box; +use blueos_hal::isr::IsrDesc; use core::arch::asm; pub struct VirtualTimerHandler; -impl IrqHandler for VirtualTimerHandler { - fn handle(&mut self) { +impl IsrDesc for VirtualTimerHandler { + fn service_isr(&self) { unsafe { // 1. Shield the virtual timer interrupt. let mut ctl = read_cntv_ctl(); @@ -40,40 +41,45 @@ pub fn init_global_vtimer() { let timer_handler = Box::new(VirtualTimerHandler); irq::register_handler(IrqNumber::new(27), timer_handler) .expect("[vTimer] Failed to register IRQ 27 handler"); - } /// Call it before booting guest. pub fn init_vcpu_timer() { - irq::enable_irq_with_priority( - IrqNumber::new(27), - current_cpu_id(), - Priority::Normal - ); + irq::enable_irq_with_priority(IrqNumber::new(27), current_cpu_id(), Priority::Normal); } #[cfg(not(test))] #[inline] fn read_cntv_ctl() -> u64 { let ctl: u64; - unsafe { asm!("mrs {}, CNTV_CTL_EL0", out(reg) ctl); } + unsafe { + asm!("mrs {}, CNTV_CTL_EL0", out(reg) ctl); + } ctl } #[cfg(not(test))] #[inline] fn write_cntv_ctl(ctl: u64) { - unsafe { asm!("msr CNTV_CTL_EL0, {}", in(reg) ctl); } + unsafe { + asm!("msr CNTV_CTL_EL0, {}", in(reg) ctl); + } } #[cfg(test)] static mut MOCK_CNTV_CTL: u64 = 0; #[cfg(test)] -fn read_cntv_ctl() -> u64 { unsafe { MOCK_CNTV_CTL } } +fn read_cntv_ctl() -> u64 { + unsafe { MOCK_CNTV_CTL } +} #[cfg(test)] -fn write_cntv_ctl(ctl: u64) { unsafe { MOCK_CNTV_CTL = ctl; } } +fn write_cntv_ctl(ctl: u64) { + unsafe { + MOCK_CNTV_CTL = ctl; + } +} #[cfg(test)] mod tests { @@ -82,15 +88,19 @@ mod tests { #[test] fn test_vtimer_handler_masking() { - unsafe { + unsafe { MOCK_CNTV_CTL = 0; - } + } let mut handler = VirtualTimerHandler; - handler.handle(); + handler.service_isr(); // Verify whether we have shield physical interrupt. (Bit 1 = IMASK) unsafe { - assert_eq!(MOCK_CNTV_CTL & (1 << 1), (1 << 1), "vTimer must set IMASK (bit 1) to prevent IRQ storms"); + assert_eq!( + MOCK_CNTV_CTL & (1 << 1), + (1 << 1), + "vTimer must set IMASK (bit 1) to prevent IRQ storms" + ); } } -} \ No newline at end of file +} From c707ae9045de8bfaacc5a49bbcc5b2d7eccb1cd9 Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 8 May 2026 14:14:35 +0800 Subject: [PATCH 4/5] Change semihosting to conditional compilation --- kernel/src/arch/aarch64/virt/exit.rs | 6 +++++- kernel/src/arch/aarch64/virt/mmu_s2.rs | 1 - kernel/src/arch/aarch64/virt/mod.rs | 6 +++++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index a4f6fadb..4dbb864e 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -14,6 +14,7 @@ use super::{guest, hyper, vcpu::Vcpu, vgic}; use core::arch::asm; +#[cfg(test)] use semihosting::println; static mut GUEST_SHUTDOWN: bool = false; @@ -78,7 +79,6 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { VmExitReason::Hvc => handle_hvc(vcpu, &exit_info), VmExitReason::Svc => handle_svc(vcpu, &exit_info), VmExitReason::DataAbortLowerEL => { - semihosting::println!("[EXIT] Data Abort from Guest (Stage-2 Fault)"); let iss = esr & 0x1FFFFFF; let dfsc = iss & 0x3F; let is_write = (iss & (1 << 6)) != 0; @@ -106,6 +106,7 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { vgic::flush(vcpu.id()); return true; } else { + #[cfg(test)] semihosting::println!("[EXIT] Unhandled Stage-2 Address!"); } } @@ -142,6 +143,7 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { } } VmExitReason::Unknown(ec) => { + #[cfg(test)] semihosting::println!("[EXIT] Unknown Exit Reason: EC = {:#x}", ec); false } @@ -184,6 +186,7 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { } } _ => { + #[cfg(test)] semihosting::println!("[EXIT] HVC#0: Ignored PSCI call: {:#x}", psci_func_id); context.regs[0] = 0xFFFF_FFFF; } @@ -196,6 +199,7 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { true } _ => { + #[cfg(test)] semihosting::println!("[EXIT] Unknown HVC Number"); true } diff --git a/kernel/src/arch/aarch64/virt/mmu_s2.rs b/kernel/src/arch/aarch64/virt/mmu_s2.rs index 673aaf7a..c9aab92c 100644 --- a/kernel/src/arch/aarch64/virt/mmu_s2.rs +++ b/kernel/src/arch/aarch64/virt/mmu_s2.rs @@ -13,7 +13,6 @@ // limitations under the License. use crate::arch::aarch64::registers::{vtcr_el2::VTCR_EL2, vttbr_el2::VTTBR_EL2}; -use semihosting::println; use tock_registers::interfaces::*; // Structure of Page Table. diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index e6951b38..a20249ef 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -25,10 +25,12 @@ pub use crate::arch::aarch64::psci::hvc_call; use blueos_hal::PlatPeri; pub use exit::{VmExitInfo, VmExitReason}; pub use hyper::{get_current_el, hyp_init}; -use semihosting::println; pub use vcpu::{Vcpu, VcpuManager, VcpuState}; pub use vgic::init; +#[cfg(test)] +use semihosting::println; + // PL011 UART addresses for QEMU Virt const UART0_DR: *mut u32 = 0x0900_0000 as *mut u32; const UART0_FR: *mut u32 = 0x0900_0018 as *mut u32; @@ -84,6 +86,7 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); } } else { + #[cfg(test)] semihosting::println!("[EL2] Unhandled Guest IRQ: {}", intid); // For uninterruptible/unknown interrupts, // we must manually downgrade and deactivate them; @@ -150,6 +153,7 @@ pub fn virt_boot_linux() { if result == 0 { let uart = crate::boards::get_device!(console_uart); uart.enable(); + #[cfg(test)] semihosting::println!("Linux shutdown!!!"); } } From 9b7f60acbc748bdb01668e005648ffdd3736724f Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 8 May 2026 14:28:45 +0800 Subject: [PATCH 5/5] Change semihosting to conditional compilation --- kernel/src/arch/aarch64/virt/exit.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index 4dbb864e..49381abb 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -116,10 +116,6 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { VmExitReason::InstructionAbortLowerEL => { let iss = esr & 0x1FFFFFF; let ifsc = iss & 0x3F; - - if (ifsc & 0x3C) == 0x14 { - semihosting::println!("[EXIT] Stage-2 Translation Fault (Instruction)!"); - } false } VmExitReason::TrappedWfiWfe => {