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 01/10] 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 02/10] 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 03/10] 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 04/10] 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 05/10] 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 => { From 5f7d5ab50e19c23311791717250db36c94bb414c Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 22 May 2026 14:40:13 +0800 Subject: [PATCH 06/10] finish basic virtio framework --- kernel/src/arch/aarch64/virt/exit.rs | 194 ++++++++----- kernel/src/arch/aarch64/virt/hyper.rs | 19 ++ kernel/src/arch/aarch64/virt/mmu_s2.rs | 6 - kernel/src/arch/aarch64/virt/mod.rs | 85 +++--- kernel/src/arch/aarch64/virt/vcpu.rs | 16 ++ kernel/src/arch/aarch64/virt/vector.rs | 13 +- kernel/src/arch/aarch64/virt/vgic.rs | 261 ++++++++++++++---- .../src/arch/aarch64/virt/virtio/console.rs | 105 +++++++ kernel/src/arch/aarch64/virt/virtio/mmio.rs | 99 +++++++ kernel/src/arch/aarch64/virt/virtio/mod.rs | 73 +++++ kernel/src/arch/aarch64/virt/vtimer.rs | 1 + kernel/src/arch/aarch64/virt/vuart.rs | 105 +++++++ kernel/src/boards/qemu_virt64_aarch64/init.rs | 2 + 13 files changed, 803 insertions(+), 176 deletions(-) create mode 100644 kernel/src/arch/aarch64/virt/virtio/console.rs create mode 100644 kernel/src/arch/aarch64/virt/virtio/mmio.rs create mode 100644 kernel/src/arch/aarch64/virt/virtio/mod.rs create mode 100644 kernel/src/arch/aarch64/virt/vuart.rs diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index 49381abb..0a77dd71 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -use super::{guest, hyper, vcpu::Vcpu, vgic}; +use super::{guest, hyper, vcpu::Vcpu, vgic, virtio, vuart}; use core::arch::asm; -#[cfg(test)] -use semihosting::println; +// #[cfg(test)] +use crate::{kearly_println, kprintln}; static mut GUEST_SHUTDOWN: bool = false; pub const PSCI_VERSION: u32 = 0x8400_0000; +pub const PSCI_MIGRATE_INFO_TYPE: u32 = 0x8400_0006; pub const PSCI_SYSTEM_OFF: u32 = 0x8400_0008; pub const PSCI_SYSTEM_RESET: u32 = 0x8400_0009; pub const PSCI_FEATURES: u32 = 0x8400_000A; @@ -75,79 +76,48 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { return_addr: elr + 4, }; - match reason { + let result = match reason { VmExitReason::Hvc => handle_hvc(vcpu, &exit_info), VmExitReason::Svc => handle_svc(vcpu, &exit_info), - VmExitReason::DataAbortLowerEL => { - let iss = esr & 0x1FFFFFF; - let dfsc = iss & 0x3F; - let is_write = (iss & (1 << 6)) != 0; - 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 { - 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 { - vcpu.context_mut().elr_el2 += 4; - vgic::flush(vcpu.id()); - return true; - } else { - #[cfg(test)] - semihosting::println!("[EXIT] Unhandled Stage-2 Address!"); - } - } - } - false - } + VmExitReason::DataAbortLowerEL => handle_data_abort(vcpu, &exit_info), VmExitReason::InstructionAbortLowerEL => { let iss = esr & 0x1FFFFFF; let ifsc = iss & 0x3F; false } - VmExitReason::TrappedWfiWfe => { - let iss = exit_info.esr & 0x1FFFFFF; - let is_wfe = (iss & 1) != 0; - vcpu.context_mut().elr_el2 += 4; + VmExitReason::TrappedWfiWfe => trap_wfi_wfe(vcpu, &exit_info), + VmExitReason::Unknown(ec) => { + false + } + }; - if is_wfe { - true - } else { - let irq_masked = (vcpu.context().spsr & (1 << 7)) != 0; + if is_guest_shutdown() { + kearly_println!("\n[VMM] Guest requested shutdown via PSCI. Exiting."); + return false; + } - if irq_masked { - false - } else { - unsafe { - core::arch::asm!("wfi"); - } - true - } - } + if !result { + kearly_println!( + " Reason: {:?}, ESR: {:#018x}, ELR(PC): {:#018x}", + reason, esr, elr + ); + if let VmExitReason::DataAbortLowerEL = reason { + let far: u64; + unsafe { core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); } + kearly_println!(" Faulting IPA / FAR: {:#018x}", far); } - VmExitReason::Unknown(ec) => { - #[cfg(test)] - semihosting::println!("[EXIT] Unknown Exit Reason: EC = {:#x}", ec); - false + + loop { + unsafe { core::arch::asm!("wfe"); } } } + + result } // hvc from guest. fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { + kearly_println!("[EXIT] HVC call received"); let saved_x0 = vcpu.context().regs[0]; let vcpu_id = vcpu.id(); let context = vcpu.context_mut(); @@ -166,7 +136,11 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { let version = 0x0000_0002; context.regs[0] = version; } + PSCI_MIGRATE_INFO_TYPE => { + context.regs[0] =2; + } PSCI_SYSTEM_OFF | PSCI_SYSTEM_RESET => { + kearly_println!("\n[VMM] Recieve Linux PSCI shutdown request."); unsafe { GUEST_SHUTDOWN = true; } @@ -182,8 +156,7 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { } } _ => { - #[cfg(test)] - semihosting::println!("[EXIT] HVC#0: Ignored PSCI call: {:#x}", psci_func_id); + kearly_println!("[EXIT] HVC#0: Ignored PSCI call: {:#x}", psci_func_id); context.regs[0] = 0xFFFF_FFFF; } } @@ -196,7 +169,7 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { } _ => { #[cfg(test)] - semihosting::println!("[EXIT] Unknown HVC Number"); + kearly_println!("[EXIT] Unknown HVC Number"); true } }; @@ -229,6 +202,101 @@ fn handle_svc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { true } +fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { + let esr = exit_info.esr; + let iss = esr & 0x1FFFFFF; + let dfsc = iss & 0x3F; + let is_write = (iss & (1 << 6)) != 0; + let faulting_pc = vcpu.context().elr(); + + if (dfsc & 0x3C) == 0x04 || (dfsc & 0x3C) == 0x08 || (dfsc & 0x3C) == 0x0C { + unsafe { + 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 and vUart. + let fault_ipa_base = (hpfar_el2 & 0x0000_00FF_FFFF_FFF0) << 8; + let exact_ipa = fault_ipa_base | (far & 0xFFF); + // Get target register index. + let srt = ((iss >> 16) & 0x1F) as usize; + // For vUart. + if (0x08F0_0000..0x08F0_1000).contains(&exact_ipa) { + let offset = exact_ipa - 0x08F0_0000; + + if is_write { + let value = vcpu.context().get_reg(srt); + vuart::handle_write(offset, value); + } else { + let value = vuart::handle_read(offset); + vcpu.context_mut().set_reg(srt, value); + } + + vcpu.context_mut().set_elr(faulting_pc + 4); + vgic::flush(vcpu.id()); + return true; + } + + if (0x0a00_0000..0x0a00_0200).contains(&exact_ipa) { + // semihosting::println!("[EXIT] virtio handle"); + let offset = exact_ipa - 0x0a00_0000; + + if is_write { + let value = vcpu.context().get_reg(srt) as u32; + virtio::VIRTIO_CONSOLE.handle_write(offset, value); + } else { + let value = virtio::VIRTIO_CONSOLE.handle_read(offset); + vcpu.context_mut().set_reg(srt, value as u64); + } + + vcpu.context_mut().set_elr(faulting_pc + 4); + vgic::flush(vcpu.id()); + return true; + } + + // For vGic. + let handled = vgic::handle_data_abort( + vcpu.id(), + esr, + exact_ipa, + &mut vcpu.context_mut().regs, + ); + + if handled { + vcpu.context_mut().set_elr(faulting_pc + 4); + vgic::flush(vcpu.id()); + return true; + } else { + kearly_println!("[EXIT] Unhandled Stage-2 Address!"); + } + } + } + false +} + +pub fn trap_wfi_wfe(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { + let iss = exit_info.esr & 0x1FFFFFF; + let is_wfe = (iss & 1) != 0; + let target_elr = vcpu.context_mut().elr() + 4; + vcpu.context_mut().set_elr(target_elr); + + 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 + } + } +} + + pub fn is_guest_shutdown() -> bool { unsafe { GUEST_SHUTDOWN } } diff --git a/kernel/src/arch/aarch64/virt/hyper.rs b/kernel/src/arch/aarch64/virt/hyper.rs index fd05a823..20c3e6c3 100644 --- a/kernel/src/arch/aarch64/virt/hyper.rs +++ b/kernel/src/arch/aarch64/virt/hyper.rs @@ -149,6 +149,25 @@ pub fn shutdown_guest() { tmp = out(reg) ich_hcr, options(nostack) ); + + // Restore EOImode to 0 for BlueOS. + // BlueOS's native GIC driver assumes EOImode=0 (EOIR drops priority AND deactivates). + // If we leave it as 1, BlueOS will never deactivate interrupts, causing an interrupt storm/hang! + let mut ctlr: u64; + core::arch::asm!( + "mrs {tmp2}, ICC_CTLR_EL1", + "bic {tmp2}, {tmp2}, #2", + "msr ICC_CTLR_EL1, {tmp2}", + "isb", + tmp2 = out(reg) ctlr, + options(nostack) + ); + + // Force deactivate physical IRQ 27 (Virtual Timer) in case the guest left it Active. + // If left Active, it will mask all equal/lower priority interrupts, completely blocking + // BlueOS's physical timer (IRQ 30) and causing the Host OS scheduler to hang! + let irq_27: u64 = 27; + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) irq_27, options(nostack)); } } diff --git a/kernel/src/arch/aarch64/virt/mmu_s2.rs b/kernel/src/arch/aarch64/virt/mmu_s2.rs index c9aab92c..6e516543 100644 --- a/kernel/src/arch/aarch64/virt/mmu_s2.rs +++ b/kernel/src/arch/aarch64/virt/mmu_s2.rs @@ -113,12 +113,6 @@ pub fn init_stage2(ipa_base: usize, size: usize) { } 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); - 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 a20249ef..a7419e2c 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -21,21 +21,21 @@ pub mod vcpu; pub mod vector; pub mod vgic; pub mod vtimer; +pub mod vuart; +pub(crate) mod virtio; pub use crate::arch::aarch64::psci::hvc_call; -use blueos_hal::PlatPeri; +use blueos_hal::{PlatPeri, isr::IsrDesc}; pub use exit::{VmExitInfo, VmExitReason}; pub use hyper::{get_current_el, hyp_init}; pub use vcpu::{Vcpu, VcpuManager, VcpuState}; pub use vgic::init; -#[cfg(test)] -use semihosting::println; +use crate::{kearly_println, kprintln}; // 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 { @@ -47,6 +47,10 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - core::arch::asm!("msr ICC_CTLR_EL1, {}", in(reg) ctlr); } } + + let vcpu_id = get_current_vcpu_id().expect("Failed to get current vcpu id"); + vgic::sync(vcpu_id); + let iar: u64; unsafe { core::arch::asm!("mrs {}, ICC_IAR1_EL1", out(reg) iar); @@ -56,55 +60,38 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - if intid == 1023 { // 1023 is Spurious Interrupt of GIC，skip + vgic::flush(vcpu_id); return 0; } 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); - } + vuart::handle_physical_uart_interrupt(); + vgic::flush(vcpu_id); unsafe { core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + core::arch::asm!("msr ICC_DIR_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; - // otherwise, the interrupt line will be permanently blocked. + return 0; + } else if intid == 27 { + vgic::inject_irq(vcpu_id, 27); + vgic::flush(vcpu_id); unsafe { + // For hardware routed timer, we only do EOI (Priority Drop). + // The guest will automatically do DIR (Deactivate) when it EOIs. core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); - core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); } + return 0; + } else { + kearly_println!("[EL2] Unhandled Guest IRQ: {}", intid); } unsafe { core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); } - if let Some(vcpu_id) = get_current_vcpu_id() { - vgic::flush(vcpu_id); - } - + vgic::flush(vcpu_id); 0 } @@ -131,6 +118,20 @@ pub fn virt_boot_linux() { // It will be placed here next. vgic::init(); vtimer::init_global_vtimer(); + // vuart::init_uart_irq(); + + unsafe { + let current_el = hyper::get_current_el(); + if current_el == 2 { + let mut ich_hcr: u64; + core::arch::asm!("mrs {}, ich_hcr_el2", out(reg) ich_hcr); + if (ich_hcr & 1) == 0 { + ich_hcr |= 1; + core::arch::asm!("msr ich_hcr_el2, {}", in(reg) ich_hcr); + core::arch::asm!("isb"); + } + } + } // Initiate vCpu for Linux kernel, set the entry point and parameters. unsafe { @@ -145,15 +146,11 @@ pub fn virt_boot_linux() { } vtimer::init_vcpu_timer(); + vuart::enable_physical_uart_interrupts(); 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. + vuart::cleanup_physical_uart_interrupts(); + if result == 0 { - let uart = crate::boards::get_device!(console_uart); - uart.enable(); - #[cfg(test)] - semihosting::println!("Linux shutdown!!!"); + kearly_println!("Linux shutdown!!!"); } } diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs index 548c4b5c..0cb152c8 100644 --- a/kernel/src/arch/aarch64/virt/vcpu.rs +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -97,6 +97,22 @@ impl VcpuStateStruct { pub fn set_spsr(&mut self, spsr: u64) { self.spsr = spsr; } + + #[inline] + pub fn set_reg(&mut self, index: usize, value: u64) { + if index < 31 { + self.regs[index] = value; + } + } + + #[inline] + pub fn get_reg(&self, index: usize) -> u64 { + if index < 31 { + self.regs[index] + } else { + 0 + } + } } #[repr(align(16))] diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index bf4e99e0..971359ed 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -277,12 +277,13 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { let mair = VCPU_MANAGER.0.host_mair; core::arch::asm!( - "msr vbar_el1, {vbar}", - "msr sctlr_el1, {sctlr}", + "msr mair_el1, {mair}", + "msr tcr_el1, {tcr}", "msr ttbr0_el1, {ttbr0}", "msr ttbr1_el1, {ttbr1}", - "msr tcr_el1, {tcr}", - "msr mair_el1, {mair}", + "isb", + "msr sctlr_el1, {sctlr}", + "msr vbar_el1, {vbar}", "isb", "tlbi alle1", "dsb sy", @@ -356,8 +357,8 @@ unsafe fn restore_context_to_frame(vcpu: &mut Vcpu, frame: *mut u64) { options(nostack) ); - let target_vcpu_id = vcpu.id(); - vgic::flush(target_vcpu_id); + // 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 index 9332b138..11a6b81c 100644 --- a/kernel/src/arch/aarch64/virt/vgic.rs +++ b/kernel/src/arch/aarch64/virt/vgic.rs @@ -16,11 +16,12 @@ use super::VCPU_MANAGER; use crate::sync::SpinLock; use core::arch::asm; use spin::Once; +use crate::{kprintln, kearly_println}; const MAX_LR: usize = 4; const MAX_PENDING: usize = 64; const MAX_VCPUS: usize = 4; -const MAX_IRQS_WORDS: usize = 32; +pub const MAX_IRQS_WORDS: usize = 32; // Qemu default base address const VGICD_BASE: u64 = 0x0800_0000; @@ -52,6 +53,7 @@ pub struct VgicRedistributor { pub pending_count: usize, // Recording how many lrs are used like KVM doing. pub used_lrs: usize, + pub lr_intids: [u32; MAX_LR], } impl VgicDistributor { @@ -78,6 +80,7 @@ impl VgicRedistributor { pending_tail: 0, pending_count: 0, used_lrs: 0, + lr_intids: [0; MAX_LR], } } @@ -133,7 +136,7 @@ struct MmioAccess { } #[inline] -fn get_vgic() -> &'static Vgic { +pub fn get_vgic() -> &'static Vgic { #[cfg(test)] VGIC.call_once(Vgic::new); @@ -211,13 +214,39 @@ fn emulate_access( } 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, - _ => {} + let mut pending_intids = [0u32; 32]; + let mut pending_count = 0; + // Set a scope, so that we can release lock automatically. + { + let mut dist = get_vgic().dist.lock(); + match offset { + 0x0000 => dist.ctlr = val, + 0x0100..=0x017C => { + let index = ((offset - 0x0100) / 4) as usize; + let newly_enabled = val & !dist.isenabler[index]; + dist.isenabler[index] |= val; + let pending_to_push = dist.ispendr[index] & newly_enabled; + if pending_to_push != 0 { + for i in 0..32 { + if (pending_to_push & (1 << i)) != 0 { + pending_intids[pending_count] = (index * 32 + i) as u32; + pending_count += 1; + } + } + } + } + 0x0180..=0x01FC => dist.isenabler[((offset - 0x0180) / 4) as usize] &= !val, + 0x0400..=0x07F8 => dist.ipriorityr[((offset - 0x0400) / 4) as usize] = val, + _ => {} + } + } + + if pending_count > 0 { + // To Do: When GICv3 is improved in the future, GICD_IROUTER should be read here to determine the target vCPU. + let mut redist = get_vgic().redists[0].lock(); + for &intid in &pending_intids[..pending_count] { + redist.push_queue(intid); + } } } @@ -227,7 +256,19 @@ fn handle_gicr_write(vcpu_id: usize, offset: u64, val: u32) { } let mut redist = get_vgic().redists[vcpu_id].lock(); match offset { - 0x10100 => redist.isenabler0 |= val, + // 0x10100 => redist.isenabler0 |= val, + 0x10100 => { + let newly_enabled = val & !redist.isenabler0; + redist.isenabler0 |= val; + let pending_to_push = redist.ispendr0 & newly_enabled; + if pending_to_push != 0 { + for i in 0..32 { + if (pending_to_push & (1 << i)) != 0 { + redist.push_queue(i); + } + } + } + } 0x10180 => redist.isenabler0 &= !val, 0x10200 => redist.ispendr0 |= val, 0x10280 => redist.ispendr0 &= !val, @@ -324,55 +365,134 @@ pub fn inject(vcpu_id: usize, intid: u32) { } else { // SPI let mut dist = get_vgic().dist.lock(); - let idx = (intid / 32) as usize; + let index = (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; - } + dist.ispendr[index] |= mask; + is_enabled = (dist.isenabler[index] & mask) != 0; drop(dist); if is_enabled { let mut redist = get_vgic().redists[vcpu_id].lock(); redist.push_queue(intid); - } + } } } } +// ICH_LR_EL2 - List Register, used to inject virtual interrupts to guest +// +// Bit layout: +// [63:62] State - Virtual interrupt state +// 00 = Invalid (slot unused) +// 01 = Pending +// 10 = Active +// 11 = Pending and Active +// [61] HW - Hardware interrupt +// 0 = Virtual interrupt (software-generated) +// 1 = Physical interrupt (maps to a physical IRQ, requires pINTID) +// [60] Group - Interrupt group +// 0 = Group 0 (FIQ) +// 1 = Group 1 (IRQ) +// [59] Reserved +// [55:48] Priority - Virtual interrupt priority (8-bit, lower = higher priority) +// e.g. 0xA0 = lower priority +// [47:45] Reserved +// [44:32] pINTID - Physical INTID (only valid when HW=1) +// Maps virtual interrupt to a physical interrupt for EOI deactivation +// [31:0] vINTID - Virtual INTID to be injected into the guest (bits [23:0] effective) pub fn flush(vcpu_id: usize) { if vcpu_id >= MAX_VCPUS { return; } + let mut dist = get_vgic().dist.lock(); 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]; + let mut active_word = redist.isactiver0; + while active_word != 0 && current_lr < MAX_LR { + let bit = active_word.trailing_zeros(); + let intid = bit as u32; + active_word &= !(1 << bit); + + // let is_active = is_irq_active_locked(&redist, intid); + let is_pending = (redist.ispendr0 & (1 << intid)) != 0; + let is_enabled = (redist.isenabler0 & (1 << intid)) != 0; + let mut state_bits: u64 = 0b10; + if is_pending && is_enabled { + state_bits |= 0b01; + redist.ispendr0 &= !(1 << intid); + } - redist.pending_head = (redist.pending_head + 1) % MAX_PENDING; - redist.pending_count -= 1; + let is_hw = intid == 27; + let hw_bit = if is_hw { 1u64 << 61 } else { 0 }; + let pintid_bits = if is_hw { (intid as u64) << 32 } else { 0 }; - 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); + // To DO: Dynamically set hw bit(61) according to irq routing, and hw bit open while setting passthrough (finish smmu). + let lr_val: u64 = (state_bits << 62) | hw_bit | (1 << 60) | (0xA0 << 48) | pintid_bits | (intid as u64); write_lr(current_lr, lr_val); - + redist.lr_intids[current_lr] = intid; current_lr += 1; } + // Scan SPIs. + for i in 1..MAX_IRQS_WORDS { + if current_lr >= MAX_LR { break; } + let mut spi_active_word = dist.isactiver[i]; + + while spi_active_word != 0 && current_lr < MAX_LR { + let bit = spi_active_word.trailing_zeros(); + let intid = (i * 32) as u32 + bit as u32; + spi_active_word &= !(1 << bit); + let is_pending = is_irq_pending_locked(&redist, &dist, intid); + let is_enabled = is_irq_enabled_locked(&redist, &dist, intid); + let mut state_bits: u64 = 0b10; + + if is_pending && is_enabled { + state_bits |= 0b01; + dist.ispendr[i] &= !(1 << bit); + } + + let lr_val: u64 = (state_bits << 62) | (1 << 60) | (0xA0 << 48) | (intid as u64); + write_lr(current_lr, lr_val); + redist.lr_intids[current_lr] = intid; + current_lr += 1; + } + } + + // Solve Pending interrupt in queue. + let mut processed_count = 0; + let original_count = redist.pending_count; + + while processed_count < original_count && 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; + processed_count += 1; + if is_irq_active_locked(&redist, &dist, intid) { + continue; + } + + let is_pending = is_irq_pending_locked(&redist, &dist, intid); + let is_enabled = is_irq_enabled_locked(&redist, &dist, intid); + + if is_pending && is_enabled { + let state_bits: u64 = 0b01; + clear_irq_pending_locked(&mut redist, &mut dist, intid); + + let is_hw = intid == 27; + let hw_bit = if is_hw { 1u64 << 61 } else { 0 }; + let pintid_bits = if is_hw { (intid as u64) << 32 } else { 0 }; + + let lr_val: u64 = (state_bits << 62) | hw_bit | (1 << 60) | (0xA0 << 48) | pintid_bits | (intid as u64); + write_lr(current_lr, lr_val); + redist.lr_intids[current_lr] = intid; + current_lr += 1; + } + } + redist.used_lrs = current_lr; } } @@ -382,19 +502,22 @@ pub fn sync(vcpu_id: usize) { return; } + let mut dist = get_vgic().dist.lock(); 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; - + let intid = redist.lr_intids[i]; if state == 0 { - clear_irq_state_locked(&mut redist, intid); + clear_irq_state_locked(&mut redist, &mut dist, intid); } else { - sync_irq_state_locked(&mut redist, intid, state); - redist.push_queue(intid); + sync_irq_state_locked(&mut redist, &mut dist, intid, state); + // redist.push_queue(intid); + if (state & 0b01) != 0 { + redist.push_queue(intid); + } } write_lr(i, 0); @@ -463,61 +586,85 @@ unsafe fn write_lr(index: usize, val: u64) { } } -fn clear_irq_state_locked(redist: &mut VgicRedistributor, intid: u32) { +fn clear_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistributor, intid: u32) { if intid < 32 { - redist.ispendr0 &= !(1 << intid); + // 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 index = (intid / 32) as usize; let mask = 1 << (intid % 32); - dist.ispendr[idx] &= !mask; - dist.isactiver[idx] &= !mask; + // dist.ispendr[index] &= !mask; + dist.isactiver[index] &= !mask; } } -fn sync_irq_state_locked(redist: &mut VgicRedistributor, intid: u32, state: u64) { +fn sync_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistributor, 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); } + // } 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 index = (intid / 32) as usize; let mask = 1 << (intid % 32); if is_pending { - dist.ispendr[idx] |= mask; - } else { - dist.ispendr[idx] &= !mask; - } + dist.ispendr[index] |= mask; + } + // else { + // dist.ispendr[index] &= !mask; + // } if is_active { - dist.isactiver[idx] |= mask; + dist.isactiver[index] |= mask; } else { - dist.isactiver[idx] &= !mask; + dist.isactiver[index] &= !mask; } } } -fn is_irq_active_locked(redist: &VgicRedistributor, intid: u32) -> bool { +fn is_irq_active_locked(redist: &VgicRedistributor, dist: &VgicDistributor, 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 } } +fn is_irq_pending_locked(redist: &VgicRedistributor, dist: &VgicDistributor, intid: u32) -> bool { + if intid < 32 { + (redist.ispendr0 & (1 << intid)) != 0 + } else { + (dist.ispendr[(intid / 32) as usize] & (1 << (intid % 32))) != 0 + } +} + +fn is_irq_enabled_locked(redist: &VgicRedistributor, dist: &VgicDistributor, intid: u32) -> bool { + if intid < 32 { + (redist.isenabler0 & (1 << intid)) != 0 + } else { + (dist.isenabler[(intid / 32) as usize] & (1 << (intid % 32))) != 0 + } +} + +fn clear_irq_pending_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistributor, intid: u32) { + if intid < 32 { + redist.ispendr0 &= !(1 << intid); + } else { + let index = (intid / 32) as usize; + let mask = 1 << (intid % 32); + dist.ispendr[index] &= !mask; + } +} #[cfg(test)] mod tests { use super::*; diff --git a/kernel/src/arch/aarch64/virt/virtio/console.rs b/kernel/src/arch/aarch64/virt/virtio/console.rs new file mode 100644 index 00000000..9b9f1f9f --- /dev/null +++ b/kernel/src/arch/aarch64/virt/virtio/console.rs @@ -0,0 +1,105 @@ +// 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::sync::atomic::{compiler_fence, Ordering}; +use super::{VirtioDevice, VirtQueue, VirtqDesc, VirtqAvail, VirtqUsed}; +use crate::{kprintln, kearly_println}; + +pub struct VirtioConsole; + +impl VirtioConsole { + pub const fn new() -> Self { Self } + + pub fn handle_rx(&mut self, byte: u8, queues: &mut [VirtQueue]) -> bool { + // RX Queue is index 0. + let q_idx = 0; + let q = &mut queues[q_idx]; + + if !q.ready || q.desc_addr == 0 { return false; } + + unsafe { + let avail = q.avail_addr as *const VirtqAvail; + let used = q.used_addr as *mut VirtqUsed; + let desc_table = q.desc_addr as *const VirtqDesc; + if q.last_idx == (*avail).idx { return false; } + + let ring_index = (q.last_idx % q.num as u16) as usize; + let head = (*avail).ring[ring_index] as usize; + let desc = &(*desc_table.add(head)); + let payload_ptr = desc.addr as *mut u8; + *payload_ptr = byte; + let u_idx = ((*used).idx % q.num as u16) as usize; + (*used).ring[u_idx].id = head as u32; + (*used).ring[u_idx].len = 1; + core::arch::asm!("dmb ishst", options(nostack)); + (*used).idx = (*used).idx.wrapping_add(1); + q.last_idx = q.last_idx.wrapping_add(1); + core::arch::asm!("dsb ishst", options(nostack)); + } + true + } +} + +impl VirtioDevice for VirtioConsole { + fn device_id(&self) -> u32 { 3 } + fn vendor_id(&self) -> u32 { 0x424C5545 } + fn handle_status(&mut self, _status: u32) {} + + fn handle_kick(&mut self, q_idx: usize, queues: &mut [VirtQueue]) -> bool { + // TX Queue is index 1. + if q_idx != 1 { + return false; + } + + let q = &mut queues[q_idx]; + if !q.ready || q.desc_addr == 0 { + return false; + } + let mut processed = false; + + unsafe { + let desc_table = q.desc_addr as *const VirtqDesc; + let avail_ring = q.avail_addr as *const VirtqAvail; + let used_ring = q.used_addr as *mut VirtqUsed; + + while q.last_idx != (*avail_ring).idx { + processed = true; + let ring_index = (q.last_idx % q.num as u16) as usize; + let head = (*avail_ring).ring[ring_index] as usize; + let desc = &(*desc_table.add(head)); + let slice = core::slice::from_raw_parts(desc.addr as *const u8, desc.len as usize); + if let Ok(s) = core::str::from_utf8(slice) { + for byte in s.as_bytes() { + let mut writer = crate::console::EarlyConsole {}; + use core::fmt::Write; + writer.write_str(core::str::from_utf8(&[*byte]).unwrap_or("")).unwrap(); + } + } + + let u_idx = ((*used_ring).idx % q.num as u16) as usize; + (*used_ring).ring[u_idx].id = head as u32; + (*used_ring).ring[u_idx].len = 0; + + // compiler_fence(Ordering::SeqCst); + unsafe { + core::arch::asm!("dmb ishst", options(nostack)); + (*used_ring).idx = (*used_ring).idx.wrapping_add(1); + q.last_idx = q.last_idx.wrapping_add(1); + core::arch::asm!("dsb ishst", options(nostack)); + } + } + } + processed + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/virtio/mmio.rs b/kernel/src/arch/aarch64/virt/virtio/mmio.rs new file mode 100644 index 00000000..7f1ddaa6 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/virtio/mmio.rs @@ -0,0 +1,99 @@ +// 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 super::{VirtioDevice, VirtQueue, console::VirtioConsole}; + +pub struct VirtioMmioTransport { + pub device: T, + pub status: u32, + pub queue_sel: u32, + pub devices_features_sel: u32, + pub interrupt_status: u32, + pub queues: [VirtQueue; 2], + pub irq_handler: fn(), +} + +impl VirtioMmioTransport { + pub const fn new(device: T, irq_handler: fn()) -> Self { + Self { + device, + status: 0, + queue_sel: 0, + devices_features_sel: 0, + interrupt_status: 0, + queues: [VirtQueue { ready: false, num: 0, desc_addr: 0, avail_addr: 0, used_addr: 0, last_idx: 0 }; 2], + irq_handler, + } + } + + pub fn handle_read(&self, offset: u64) -> u32 { + match offset { + 0x000 => 0x74726976, + 0x004 => 2, + 0x008 => self.device.device_id(), + 0x00c => self.device.vendor_id(), + 0x010 => { + if self.devices_features_sel == 1 { + 1 + } else { + 0 + } + } + 0x034 => 256, + 0x044 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].ready as u32 } else { 0 }, + 0x60 => self.interrupt_status, + 0x070 => self.status, + _ => 0, + } + } + + pub fn handle_write(&mut self, offset: u64, value: u32) { + match offset { + 0x014 => self.devices_features_sel = value, + 0x030 => self.queue_sel = value, + 0x038 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].num = value }, + 0x044 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].ready = value == 1 }, + 0x050 => { + let should_interrupt = self.device.handle_kick(value as usize, &mut self.queues); + if should_interrupt { + self.interrupt_status |= 1; + (self.irq_handler)(); + } + } + 0x064 => self.interrupt_status &= !value, + 0x070 => { + self.status = value; + self.device.handle_status(value); + } + 0x080 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].desc_addr = (self.queues[self.queue_sel as usize].desc_addr & !0xFFFF_FFFF) | value as u64 }, + 0x084 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].desc_addr = (self.queues[self.queue_sel as usize].desc_addr & 0xFFFF_FFFF) | ((value as u64) << 32) }, + 0x090 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].avail_addr = (self.queues[self.queue_sel as usize].avail_addr & !0xFFFF_FFFF) | value as u64 }, + 0x094 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].avail_addr = (self.queues[self.queue_sel as usize].avail_addr & 0xFFFF_FFFF) | ((value as u64) << 32) }, + 0x0a0 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].used_addr = (self.queues[self.queue_sel as usize].used_addr & !0xFFFF_FFFF) | value as u64 }, + 0x0a4 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].used_addr = (self.queues[self.queue_sel as usize].used_addr & 0xFFFF_FFFF) | ((value as u64) << 32) }, + _ => {} + } + } +} + +impl VirtioMmioTransport { + pub fn inject_rx(&mut self, byte: u8) { + if self.device.handle_rx(byte, &mut self.queues) { + self.interrupt_status |= 1; + // inject_irq(0, 48); + (self.irq_handler)(); + + } + } +} \ No newline at end of file diff --git a/kernel/src/arch/aarch64/virt/virtio/mod.rs b/kernel/src/arch/aarch64/virt/virtio/mod.rs new file mode 100644 index 00000000..7dbdaced --- /dev/null +++ b/kernel/src/arch/aarch64/virt/virtio/mod.rs @@ -0,0 +1,73 @@ +// 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. + +pub mod console; +pub mod mmio; + +pub use console::VirtioConsole; +pub use mmio::VirtioMmioTransport; +use crate::arch::virt::vgic::inject_irq; + +fn console_irq_handler() { + inject_irq(0, 48); +} + +pub static mut VIRTIO_CONSOLE: VirtioMmioTransport = + VirtioMmioTransport::new(VirtioConsole::new(), console_irq_handler); + +#[repr(C)] +#[derive(Debug, Clone, Copy)] +pub struct VirtqDesc { + pub addr: u64, + pub len: u32, + pub flags: u16, + pub next: u16, +} + +#[repr(C)] +pub struct VirtqAvail { + pub flags: u16, + pub idx: u16, + pub ring: [u16; 256], +} + +#[repr(C)] +pub struct VirtqUsedElem { + pub id: u32, + pub len: u32, +} + +#[repr(C)] +pub struct VirtqUsed { + pub flags: u16, + pub idx: u16, + pub ring: [VirtqUsedElem; 256], +} + +#[derive(Default, Clone, Copy)] +pub struct VirtQueue { + pub ready: bool, + pub num: u32, + pub desc_addr: u64, + pub avail_addr: u64, + pub used_addr: u64, + pub last_idx: u16, +} + +pub trait VirtioDevice { + fn device_id(&self) -> u32; + fn vendor_id(&self) -> u32; + fn handle_status(&mut self, status: u32); + fn handle_kick(&mut self, q_idx: usize, queues: &mut [VirtQueue]) -> bool; +} \ 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 6e0f47fc..8ba0888c 100644 --- a/kernel/src/arch/aarch64/virt/vtimer.rs +++ b/kernel/src/arch/aarch64/virt/vtimer.rs @@ -33,6 +33,7 @@ impl IsrDesc for VirtualTimerHandler { // 2. Inject the interrupt into the vGIC queue. let vcpu_id = current_cpu_id(); vgic::inject_irq(vcpu_id, 27); + vgic::flush(vcpu_id); } } } diff --git a/kernel/src/arch/aarch64/virt/vuart.rs b/kernel/src/arch/aarch64/virt/vuart.rs new file mode 100644 index 00000000..e2542724 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vuart.rs @@ -0,0 +1,105 @@ +// 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 super::{get_current_vcpu_id, vgic, virtio}; +use blueos_hal::isr::IsrDesc; + +pub fn forward_byte_to_guest(byte: u8) { + if let Some(vcpu_id) = get_current_vcpu_id() { + unsafe { + virtio::VIRTIO_CONSOLE.inject_rx(byte); + } + + vgic::inject_irq(vcpu_id, 48); + } +} + +#[inline] +pub fn handle_read(offset: u64) -> u64 { + match offset { + 0xFE0 => 0x11, 0xFE4 => 0x10, 0xFE8 => 0x14, 0xFEC => 0x00, + 0xFF0 => 0x0D, 0xFF4 => 0xF0, 0xFF8 => 0x05, 0xFFC => 0xB1, + 0x018 => 0x90, + 0x024 => 0x24, + 0x030 => 0x301, + 0x038 => 0x00, + _ => 0, + } +} + +#[inline] +pub fn handle_write(offset: u64, value: u64) { + match offset { + 0x000 => { + let c = (value & 0xFF) as u8; + let mut writer = crate::console::EarlyConsole {}; + use core::fmt::Write; + writer.write_str(core::str::from_utf8(&[c]).unwrap_or("")).unwrap(); + } + 0x030..=0x044 => (), + _ => {} + } +} + +pub fn handle_physical_uart_interrupt() { + unsafe { + let uart_base = 0x0900_0000 as *mut u32; + let mut injected = false; + + while (core::ptr::read_volatile(uart_base.add(0x18 / 4)) & (1 << 4)) == 0 { + let mut byte = core::ptr::read_volatile(uart_base) as u8; + if byte == b'\r' { + byte = b'\n'; + } + + virtio::VIRTIO_CONSOLE.inject_rx(byte); + injected = true; + } + + if injected { + if let Some(vcpu_id) = get_current_vcpu_id() { + vgic::inject_irq(vcpu_id, 48); + } + } + + core::ptr::write_volatile(uart_base.add(0x44 / 4), (1 << 4) | (1 << 6)); + } +} + +pub fn enable_physical_uart_interrupts() { + unsafe { + // Enable UART RX and RT interrupts in PL011 IMSC register + // Without this, the hardware won't assert IRQ 33 when keys are pressed! + let uart_base = 0x0900_0000 as *mut u32; + let imsc = uart_base.add(0x38 / 4); + core::ptr::write_volatile(imsc, core::ptr::read_volatile(imsc) | (1 << 4) | (1 << 6)); + } +} + +pub fn cleanup_physical_uart_interrupts() { + unsafe { + let uart_base = 0x0900_0000 as *mut u32; + + // Disable RTI (bit 6) because BlueOS's native driver doesn't handle it. + // If we leave it enabled, it causes an infinite interrupt storm in EL1! + let imsc = uart_base.add(0x38 / 4); + let mut val = core::ptr::read_volatile(imsc); + val &= !(1 << 6); + core::ptr::write_volatile(imsc, val); + + // Clear any pending RTI to ensure a clean state for BlueOS + let icr = uart_base.add(0x44 / 4); + core::ptr::write_volatile(icr, 1 << 6); + } +} \ No newline at end of file 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! { From f669e6adcb4687f31609b6a41fa3bbf0206f9da6 Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 29 May 2026 15:29:32 +0800 Subject: [PATCH 07/10] Fix timer bug --- kernel/src/arch/aarch64/registers/hcr_el2.rs | 2 +- .../src/arch/aarch64/registers/sctlr_el2.rs | 7 -- kernel/src/arch/aarch64/registers/spsr_el2.rs | 25 ++-- kernel/src/arch/aarch64/virt/exit.rs | 59 +++++---- kernel/src/arch/aarch64/virt/guest.rs | 2 +- kernel/src/arch/aarch64/virt/hyper.rs | 40 ++++-- kernel/src/arch/aarch64/virt/mod.rs | 26 ++-- kernel/src/arch/aarch64/virt/vcpu.rs | 21 +++- kernel/src/arch/aarch64/virt/vector.rs | 114 +++++++++++++++-- kernel/src/arch/aarch64/virt/vgic.rs | 115 +++++++++++------- .../src/arch/aarch64/virt/virtio/console.rs | 4 +- kernel/src/arch/aarch64/virt/virtio/mmio.rs | 1 - kernel/src/arch/aarch64/virt/virtio/mod.rs | 6 +- kernel/src/arch/aarch64/virt/vtimer.rs | 13 +- kernel/src/arch/aarch64/virt/vuart.rs | 33 ++--- 15 files changed, 312 insertions(+), 156 deletions(-) diff --git a/kernel/src/arch/aarch64/registers/hcr_el2.rs b/kernel/src/arch/aarch64/registers/hcr_el2.rs index d8f8dbc2..ca342f1d 100644 --- a/kernel/src/arch/aarch64/registers/hcr_el2.rs +++ b/kernel/src/arch/aarch64/registers/hcr_el2.rs @@ -55,7 +55,7 @@ register_bitfields! {u64, EL2Handled = 1 ], - /// VF, bit [6] - Vitual FIQ + /// VF, bit [6] - Virtual FIQ VF OFFSET(6) NUMBITS(1) [ Disable = 0, Enable = 1 diff --git a/kernel/src/arch/aarch64/registers/sctlr_el2.rs b/kernel/src/arch/aarch64/registers/sctlr_el2.rs index f6d0d62e..7526b4ae 100644 --- a/kernel/src/arch/aarch64/registers/sctlr_el2.rs +++ b/kernel/src/arch/aarch64/registers/sctlr_el2.rs @@ -203,13 +203,6 @@ register_bitfields! {u64, 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) [ diff --git a/kernel/src/arch/aarch64/registers/spsr_el2.rs b/kernel/src/arch/aarch64/registers/spsr_el2.rs index 52a3848a..e7a9a927 100644 --- a/kernel/src/arch/aarch64/registers/spsr_el2.rs +++ b/kernel/src/arch/aarch64/registers/spsr_el2.rs @@ -16,6 +16,15 @@ use tock_registers::{interfaces::*, register_bitfields}; register_bitfields! {u64, pub SPSR_EL2 [ + /// Negative condition flag. + N OFFSET(31) NUMBITS(1) [], + /// Zero condition flag. + Z OFFSET(30) NUMBITS(1) [], + /// Carry condition flag. + C OFFSET(29) NUMBITS(1) [], + /// Overflow condition flag. + V OFFSET(28) NUMBITS(1) [], + /// Debug exception mask. Controls routing of debug exceptions to EL2. D OFFSET(9) NUMBITS(1) [ Allow = 0, @@ -40,13 +49,6 @@ register_bitfields! {u64, 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 @@ -55,15 +57,8 @@ register_bitfields! {u64, 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. + /// Bit 0 also encodes SP selection: 0 = SP_EL0, 1 = SP_ELx. M OFFSET(0) NUMBITS(4) [ EL0t = 0b0000, EL1t = 0b0100, diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index 0a77dd71..a3dbde2d 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -14,7 +14,6 @@ use super::{guest, hyper, vcpu::Vcpu, vgic, virtio, vuart}; use core::arch::asm; -// #[cfg(test)] use crate::{kearly_println, kprintln}; static mut GUEST_SHUTDOWN: bool = false; @@ -27,6 +26,11 @@ pub const PSCI_FEATURES: u32 = 0x8400_000A; pub const HVC_VMM_GET_INFO: u64 = 0x11; pub const HVC_VMM_INJECT_IRQ: u64 = 0x13; +const VUART_IPA_BASE: u64 = 0x08F0_0000; +const VUART_IPA_SIZE: u64 = 0x1000; +const VIRTIO_MMIO_IPA_BASE: u64 = 0x0a00_0000; +const VIRTIO_MMIO_IPA_SIZE: u64 = 0x200; + #[derive(Debug, Clone, Copy, PartialEq)] pub enum VmExitReason { Hvc, @@ -83,30 +87,29 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { VmExitReason::InstructionAbortLowerEL => { let iss = esr & 0x1FFFFFF; let ifsc = iss & 0x3F; + let far: u64; + unsafe { core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); } + kearly_println!("[EXIT] InstructionAbort: ELR={:#018x} FAR={:#018x} IFSC={:#x}", elr, far, ifsc); false } VmExitReason::TrappedWfiWfe => trap_wfi_wfe(vcpu, &exit_info), VmExitReason::Unknown(ec) => { + kearly_println!("[EXIT] Unknown exit: EC={:#x} ESR={:#018x} ELR={:#018x}", ec, esr, elr); false } }; if is_guest_shutdown() { - kearly_println!("\n[VMM] Guest requested shutdown via PSCI. Exiting."); - return false; + return false; } if !result { - kearly_println!( - " Reason: {:?}, ESR: {:#018x}, ELR(PC): {:#018x}", - reason, esr, elr - ); if let VmExitReason::DataAbortLowerEL = reason { let far: u64; unsafe { core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); } kearly_println!(" Faulting IPA / FAR: {:#018x}", far); } - + loop { unsafe { core::arch::asm!("wfe"); } } @@ -117,7 +120,6 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { // hvc from guest. fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { - kearly_println!("[EXIT] HVC call received"); let saved_x0 = vcpu.context().regs[0]; let vcpu_id = vcpu.id(); let context = vcpu.context_mut(); @@ -140,7 +142,6 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { context.regs[0] =2; } PSCI_SYSTEM_OFF | PSCI_SYSTEM_RESET => { - kearly_println!("\n[VMM] Recieve Linux PSCI shutdown request."); unsafe { GUEST_SHUTDOWN = true; } @@ -221,8 +222,8 @@ fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { // Get target register index. let srt = ((iss >> 16) & 0x1F) as usize; // For vUart. - if (0x08F0_0000..0x08F0_1000).contains(&exact_ipa) { - let offset = exact_ipa - 0x08F0_0000; + if (VUART_IPA_BASE..VUART_IPA_BASE + VUART_IPA_SIZE).contains(&exact_ipa) { + let offset = exact_ipa - VUART_IPA_BASE; if is_write { let value = vcpu.context().get_reg(srt); @@ -237,9 +238,8 @@ fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { return true; } - if (0x0a00_0000..0x0a00_0200).contains(&exact_ipa) { - // semihosting::println!("[EXIT] virtio handle"); - let offset = exact_ipa - 0x0a00_0000; + if (VIRTIO_MMIO_IPA_BASE..VIRTIO_MMIO_IPA_BASE + VIRTIO_MMIO_IPA_SIZE).contains(&exact_ipa) { + let offset = exact_ipa - VIRTIO_MMIO_IPA_BASE; if is_write { let value = vcpu.context().get_reg(srt) as u32; @@ -266,9 +266,17 @@ fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { vcpu.context_mut().set_elr(faulting_pc + 4); vgic::flush(vcpu.id()); return true; - } else { - kearly_println!("[EXIT] Unhandled Stage-2 Address!"); } + + // Unhandled Stage-2 MMIO: return 0 for reads, ignore writes, advance PC. + // This prevents the VMM from entering a wfe death loop on unemulated + // devices (virtio-blk, virtio-net, etc.), allowing Guest shutdown to proceed. + if !is_write { + vcpu.context_mut().set_reg(srt, 0); + } + vcpu.context_mut().set_elr(faulting_pc + 4); + vgic::flush(vcpu.id()); + return true; } } false @@ -280,19 +288,20 @@ pub fn trap_wfi_wfe(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { let target_elr = vcpu.context_mut().elr() + 4; vcpu.context_mut().set_elr(target_elr); + // Flush pending vGIC interrupts before returning to Guest. + // When Guest does WFI waiting for timer, Pending IRQ 27 must + // be in a List Register so the virtual interrupt can wake it up. + vgic::flush(vcpu.id()); + 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 - } + // WFI with IRQ masked is normal idle behavior (e.g. cpu_do_idle). + // Always return true — KVM never returns failure for WFI regardless + // of IRQ mask state. The PC was already advanced +4. + true } } diff --git a/kernel/src/arch/aarch64/virt/guest.rs b/kernel/src/arch/aarch64/virt/guest.rs index 41b700b6..c0e4a0d8 100644 --- a/kernel/src/arch/aarch64/virt/guest.rs +++ b/kernel/src/arch/aarch64/virt/guest.rs @@ -16,4 +16,4 @@ use core::arch::asm; pub const LINUX_KERNEL_LOAD_ADDR: usize = 0x4400_0000; pub const LINUX_DTB_ADDR: usize = 0x4E00_0000; -pub const LINUX_RAM_SIZE: usize = 0x1000_0000; +pub const LINUX_RAM_SIZE: usize = 0x0C00_0000; diff --git a/kernel/src/arch/aarch64/virt/hyper.rs b/kernel/src/arch/aarch64/virt/hyper.rs index 20c3e6c3..c0a7dfa3 100644 --- a/kernel/src/arch/aarch64/virt/hyper.rs +++ b/kernel/src/arch/aarch64/virt/hyper.rs @@ -14,7 +14,7 @@ use crate::arch::aarch64::{ registers::{hcr_el2::HCR_EL2, sctlr_el2::SCTLR_EL2, spsr_el2::SPSR_EL2}, - virt::{mmu_el2, vector}, + virt::{guest, mmu_el2, vector, vgic}, }; use tock_registers::interfaces::{Readable, Writeable}; @@ -88,7 +88,7 @@ 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(guest::LINUX_KERNEL_LOAD_ADDR, guest::LINUX_RAM_SIZE); HCR_EL2.write( HCR_EL2::VM::Enable + HCR_EL2::RW::EL1AArch64 @@ -135,11 +135,16 @@ fn configure_timer_el2() { } } +#[inline] +unsafe fn deactivate_irq(intid: u64) { + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) intid, options(nostack)); +} + #[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 + // Disable vGIC CPU interface let mut ich_hcr: u64; core::arch::asm!( "mrs {tmp}, ich_hcr_el2", @@ -150,9 +155,10 @@ pub fn shutdown_guest() { options(nostack) ); - // Restore EOImode to 0 for BlueOS. - // BlueOS's native GIC driver assumes EOImode=0 (EOIR drops priority AND deactivates). - // If we leave it as 1, BlueOS will never deactivate interrupts, causing an interrupt storm/hang! + // Clear all List Registers to invalidate pending/active virtual interrupts + vgic::clear_all_lrs(); + + // Immediately restore EOImode=0 so EOI both drops priority AND deactivates. let mut ctlr: u64; core::arch::asm!( "mrs {tmp2}, ICC_CTLR_EL1", @@ -163,11 +169,23 @@ pub fn shutdown_guest() { options(nostack) ); - // Force deactivate physical IRQ 27 (Virtual Timer) in case the guest left it Active. - // If left Active, it will mask all equal/lower priority interrupts, completely blocking - // BlueOS's physical timer (IRQ 30) and causing the Host OS scheduler to hang! - let irq_27: u64 = 27; - core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) irq_27, options(nostack)); + // Deactivate all PPIs (0-31) and UART SPI (33). + // DIR on an Inactive interrupt is a safe no-op per GICv3 spec. + // This clears any Active state left by the Guest, allowing + // running priority to drop to idle so PMR=0xFF takes full effect. + for intid in 0..32 { + deactivate_irq(intid as u64); + } + deactivate_irq(33); + + let rpr: u64; + core::arch::asm!("mrs {}, ICC_RPR_EL1", out(reg) rpr, options(nostack)); + + // Re-enable Host physical timer (Guest may have disabled CNTP_CTL_EL0). + // Enable=1, IMASK=0 so IRQ 30 fires for Host scheduler. + let cntp_ctl: u64 = 1; + core::arch::asm!("msr CNTP_CTL_EL0, {}", in(reg) cntp_ctl, options(nostack)); + core::arch::asm!("isb", options(nostack)); } } diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index a7419e2c..4257f8da 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -32,10 +32,6 @@ pub use vgic::init; use crate::{kearly_println, kprintln}; -// 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; - #[no_mangle] pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) -> usize { unsafe { @@ -45,6 +41,7 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - // set EOImode. ctlr |= 1 << 1; core::arch::asm!("msr ICC_CTLR_EL1, {}", in(reg) ctlr); + core::arch::asm!("isb"); } } @@ -74,6 +71,23 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - } return 0; } else if intid == 27 { + let is_enabled = { + let redist = vgic::get_vgic().redists[vcpu_id].lock(); + (redist.isenabler0 & (1 << 27)) != 0 + }; + + if !is_enabled { + 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); + core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); + } + return 0; + } + vgic::inject_irq(vcpu_id, 27); vgic::flush(vcpu_id); unsafe { @@ -115,10 +129,8 @@ pub fn virt_init() { } pub fn virt_boot_linux() { - // It will be placed here next. vgic::init(); vtimer::init_global_vtimer(); - // vuart::init_uart_irq(); unsafe { let current_el = hyper::get_current_el(); @@ -149,7 +161,7 @@ pub fn virt_boot_linux() { vuart::enable_physical_uart_interrupts(); let result = hvc_call(2, 0, 0); vuart::cleanup_physical_uart_interrupts(); - + if result == 0 { kearly_println!("Linux shutdown!!!"); } diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs index 0cb152c8..a2a39912 100644 --- a/kernel/src/arch/aarch64/virt/vcpu.rs +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -131,9 +131,8 @@ impl Vcpu { 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.spsr = 0x3C5; context.vbar_el1 = (entry as u64 + 0x1000) & !0x7FF; - // context.sp = stack_top as u64; Self { id, @@ -269,6 +268,15 @@ pub struct VcpuManager { pub host_tcr: u64, pub host_mair: u64, pub host_sctlr: u64, + pub host_pmr: u64, + pub host_sre: u64, + pub host_ctlr: u64, + pub host_cntp_ctl: u64, + pub host_cntp_cval: u64, + pub host_tpidr_el0: u64, + pub host_tpidr_el1: u64, + pub host_tpidrro_el0: u64, + pub host_sp_el0: u64, } impl VcpuManager { @@ -288,6 +296,15 @@ impl VcpuManager { host_tcr: 0, host_mair: 0, host_sctlr: 0, + host_pmr: 0, + host_sre: 0, + host_ctlr: 0, + host_cntp_ctl: 0, + host_cntp_cval: 0, + host_tpidr_el0: 0, + host_tpidr_el1: 0, + host_tpidrro_el0: 0, + host_sp_el0: 0, } } diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index 971359ed..236d56e3 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -187,7 +187,6 @@ unsafe fn handle_guest_request(vcpu_id: usize, frame: *mut u64) -> u64 { let ok = handle_vm_exit(vcpu); if !ok && is_guest_shutdown() { - core::arch::asm!("msr daifset, #15"); clear_guest_shutdown(); hyper::shutdown_guest(); VCPU_MANAGER.0.clear_current_vcpu(); @@ -226,14 +225,16 @@ 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) + 4; + VCPU_MANAGER.0.host_elr = *frame.add(31); VCPU_MANAGER.0.host_spsr = *frame.add(32); VCPU_MANAGER.0.host_sp = *frame.add(33); 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); + // A 13-element tuple type would exceed Clippy's complexity threshold. + let (vbar, sctlr, ttbr0, ttbr1, tcr, mair, pmr, sre, ctlr): (u64, u64, u64, u64, u64, u64, u64, u64, u64); + let (tpidr_el0, tpidr_el1, tpidrro_el0, sp_el0): (u64, u64, u64, u64); core::arch::asm!( "mrs {vbar}, vbar_el1", "mrs {sctlr}, sctlr_el1", @@ -241,12 +242,26 @@ unsafe fn save_host_context(frame: *mut u64) { "mrs {ttbr1}, ttbr1_el1", "mrs {tcr}, tcr_el1", "mrs {mair}, mair_el1", + "mrs {pmr}, ICC_PMR_EL1", + "mrs {sre}, ICC_SRE_EL1", + "mrs {ctlr}, ICC_CTLR_EL1", + "mrs {tpidr_el0}, tpidr_el0", + "mrs {tpidr_el1}, tpidr_el1", + "mrs {tpidrro_el0}, tpidrro_el0", + "mrs {sp_el0}, sp_el0", vbar = out(reg) vbar, sctlr = out(reg) sctlr, ttbr0 = out(reg) ttbr0, ttbr1 = out(reg) ttbr1, tcr = out(reg) tcr, mair = out(reg) mair, + pmr = out(reg) pmr, + sre = out(reg) sre, + ctlr = out(reg) ctlr, + tpidr_el0 = out(reg) tpidr_el0, + tpidr_el1 = out(reg) tpidr_el1, + tpidrro_el0 = out(reg) tpidrro_el0, + sp_el0 = out(reg) sp_el0, options(nostack, nomem) ); VCPU_MANAGER.0.host_vbar = vbar; @@ -255,6 +270,29 @@ unsafe fn save_host_context(frame: *mut u64) { VCPU_MANAGER.0.host_ttbr1 = ttbr1; VCPU_MANAGER.0.host_tcr = tcr; VCPU_MANAGER.0.host_mair = mair; + VCPU_MANAGER.0.host_pmr = pmr; + VCPU_MANAGER.0.host_sre = sre; + VCPU_MANAGER.0.host_ctlr = ctlr; + VCPU_MANAGER.0.host_tpidr_el0 = tpidr_el0; + VCPU_MANAGER.0.host_tpidr_el1 = tpidr_el1; + VCPU_MANAGER.0.host_tpidrro_el0 = tpidrro_el0; + VCPU_MANAGER.0.host_sp_el0 = sp_el0; + + // Save Host physical timer state (Guest can modify CNTP via direct EL1 access). + let (cntp_ctl, cntp_cval): (u64, u64); + core::arch::asm!( + "mrs {cntp_ctl}, CNTP_CTL_EL0", + "mrs {cntp_cval}, CNTP_CVAL_EL0", + cntp_ctl = out(reg) cntp_ctl, + cntp_cval = out(reg) cntp_cval, + options(nostack, nomem) + ); + VCPU_MANAGER.0.host_cntp_ctl = cntp_ctl; + VCPU_MANAGER.0.host_cntp_cval = cntp_cval; + + // Diagnostic: print key system registers before Guest runs + let cpacr: u64; + core::arch::asm!("mrs {}, cpacr_el1", out(reg) cpacr, options(nomem, nostack)); } unsafe fn restore_host_to_frame(frame: *mut u64) { @@ -275,26 +313,80 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { let ttbr1 = VCPU_MANAGER.0.host_ttbr1; let tcr = VCPU_MANAGER.0.host_tcr; let mair = VCPU_MANAGER.0.host_mair; + let sre = VCPU_MANAGER.0.host_sre; + let ctlr = VCPU_MANAGER.0.host_ctlr; + let pmr = VCPU_MANAGER.0.host_pmr; + let tpidr_el0 = VCPU_MANAGER.0.host_tpidr_el0; + let tpidr_el1 = VCPU_MANAGER.0.host_tpidr_el1; + let tpidrro_el0 = VCPU_MANAGER.0.host_tpidrro_el0; + let sp_el0 = VCPU_MANAGER.0.host_sp_el0; core::arch::asm!( + // Restore GIC: SRE must come first (enables ICC_* register access) + "msr ICC_SRE_EL1, {sre}", + "isb", + "msr ICC_CTLR_EL1, {ctlr}", + "msr ICC_PMR_EL1, {pmr}", + "isb", "msr mair_el1, {mair}", "msr tcr_el1, {tcr}", "msr ttbr0_el1, {ttbr0}", "msr ttbr1_el1, {ttbr1}", "isb", "msr sctlr_el1, {sctlr}", - "msr vbar_el1, {vbar}", + "msr vbar_el1, {vbar}", + "msr tpidr_el0, {tpidr_el0}", + "msr tpidr_el1, {tpidr_el1}", + "msr tpidrro_el0, {tpidrro_el0}", + "msr sp_el0, {sp_el0}", + "msr VTTBR_EL2, xzr", "isb", - "tlbi alle1", - "dsb sy", + "tlbi vmalle1is", + "tlbi alle2is", + "dsb ish", "isb", + sre = in(reg) sre, + ctlr = in(reg) ctlr, + pmr = in(reg) pmr, vbar = in(reg) vbar, sctlr = in(reg) sctlr, ttbr0 = in(reg) ttbr0, ttbr1 = in(reg) ttbr1, tcr = in(reg) tcr, mair = in(reg) mair, + tpidr_el0 = in(reg) tpidr_el0, + tpidr_el1 = in(reg) tpidr_el1, + tpidrro_el0 = in(reg) tpidrro_el0, + sp_el0 = in(reg) sp_el0, + ); + + let restored_pmr: u64; + unsafe { core::arch::asm!("mrs {}, ICC_PMR_EL1", out(reg) restored_pmr); } + + // Diagnostic: print key system registers after restoring Host + let cpacr_after: u64; + let sctlr_after: u64; + let ttbr0_after: u64; + let ttbr1_after: u64; + let vbar_after: u64; + unsafe { + core::arch::asm!("mrs {}, cpacr_el1", out(reg) cpacr_after); + core::arch::asm!("mrs {}, sctlr_el1", out(reg) sctlr_after); + core::arch::asm!("mrs {}, ttbr0_el1", out(reg) ttbr0_after); + core::arch::asm!("mrs {}, ttbr1_el1", out(reg) ttbr1_after); + core::arch::asm!("mrs {}, vbar_el1", out(reg) vbar_after); + } + + // Restore Host physical timer CVAL (compare value). + // CNTP_CTL is left as-is — shutdown_guest() already set it to Enable=1, IMASK=0. + let cntp_cval = VCPU_MANAGER.0.host_cntp_cval; + core::arch::asm!( + "msr CNTP_CVAL_EL0, {cntp_cval}", + "isb", + cntp_cval = in(reg) cntp_cval, + options(nostack) ); + core::arch::asm!("msr CNTV_CTL_EL0, xzr", options(nostack)); } unsafe fn save_frame_to_context(frame: *mut u64, vcpu: &mut Vcpu) { @@ -356,9 +448,6 @@ 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); } // Temporary placeholder @@ -447,12 +536,19 @@ pub unsafe extern "C" fn fiq_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", "bl hyper_trap_fiq\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/arch/aarch64/virt/vgic.rs b/kernel/src/arch/aarch64/virt/vgic.rs index 11a6b81c..b6f68228 100644 --- a/kernel/src/arch/aarch64/virt/vgic.rs +++ b/kernel/src/arch/aarch64/virt/vgic.rs @@ -16,7 +16,6 @@ use super::VCPU_MANAGER; use crate::sync::SpinLock; use core::arch::asm; use spin::Once; -use crate::{kprintln, kearly_println}; const MAX_LR: usize = 4; const MAX_PENDING: usize = 64; @@ -268,6 +267,19 @@ fn handle_gicr_write(vcpu_id: usize, offset: u64, val: u32) { } } } + // When Guest enables IRQ 27 (virtual timer), clear CNTV_CTL_EL0.IMASK + // that was set by hyper_trap_irq when the timer fired before Guest + // enabled isenabler0. Without this, the physical timer stays permanently + // silent — no timer ticks → Guest scheduler can't wake processes. + if (newly_enabled & (1 << 27)) != 0 { + unsafe { + let mut ctl: u64; + asm!("mrs {}, CNTV_CTL_EL0", out(reg) ctl); + ctl &= !(1u64 << 1); + asm!("msr CNTV_CTL_EL0, {}", in(reg) ctl); + asm!("isb", options(nostack)); + } + } } 0x10180 => redist.isenabler0 &= !val, 0x10200 => redist.ispendr0 |= val, @@ -304,7 +316,7 @@ fn handle_gicr_read(vcpu_id: usize, offset: u64) -> u32 { 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 |= 1 << 4; } val } @@ -350,6 +362,14 @@ pub fn cpu_init(vcpu_id: usize) { } } +/// Clear all ICH_LR registers to invalidate pending/active virtual interrupts. +/// Called during guest shutdown to ensure no stale vGIC state leaks to the host. +pub unsafe fn 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 { @@ -410,13 +430,14 @@ pub fn flush(vcpu_id: usize) { unsafe { let mut current_lr = 0; + + // Phase 1: All PPI Active interrupts (Active-only and Active+Pending). let mut active_word = redist.isactiver0; while active_word != 0 && current_lr < MAX_LR { let bit = active_word.trailing_zeros(); let intid = bit as u32; active_word &= !(1 << bit); - // let is_active = is_irq_active_locked(&redist, intid); let is_pending = (redist.ispendr0 & (1 << intid)) != 0; let is_enabled = (redist.isenabler0 & (1 << intid)) != 0; let mut state_bits: u64 = 0b10; @@ -429,40 +450,14 @@ pub fn flush(vcpu_id: usize) { let hw_bit = if is_hw { 1u64 << 61 } else { 0 }; let pintid_bits = if is_hw { (intid as u64) << 32 } else { 0 }; - // Temporarily set priority(0xA0 << 48) - // To DO: Dynamically set hw bit(61) according to irq routing, and hw bit open while setting passthrough (finish smmu). let lr_val: u64 = (state_bits << 62) | hw_bit | (1 << 60) | (0xA0 << 48) | pintid_bits | (intid as u64); write_lr(current_lr, lr_val); redist.lr_intids[current_lr] = intid; current_lr += 1; } - // Scan SPIs. - for i in 1..MAX_IRQS_WORDS { - if current_lr >= MAX_LR { break; } - let mut spi_active_word = dist.isactiver[i]; - - while spi_active_word != 0 && current_lr < MAX_LR { - let bit = spi_active_word.trailing_zeros(); - let intid = (i * 32) as u32 + bit as u32; - spi_active_word &= !(1 << bit); - let is_pending = is_irq_pending_locked(&redist, &dist, intid); - let is_enabled = is_irq_enabled_locked(&redist, &dist, intid); - let mut state_bits: u64 = 0b10; - - if is_pending && is_enabled { - state_bits |= 0b01; - dist.ispendr[i] &= !(1 << bit); - } - - let lr_val: u64 = (state_bits << 62) | (1 << 60) | (0xA0 << 48) | (intid as u64); - write_lr(current_lr, lr_val); - redist.lr_intids[current_lr] = intid; - current_lr += 1; - } - } - - // Solve Pending interrupt in queue. + // Phase 2: Pending interrupts from queue (state 01) — including timer IRQ 27. + // Elevated above SPI Active to ensure timer delivery before LR overflow. let mut processed_count = 0; let original_count = redist.pending_count; @@ -477,7 +472,7 @@ pub fn flush(vcpu_id: usize) { let is_pending = is_irq_pending_locked(&redist, &dist, intid); let is_enabled = is_irq_enabled_locked(&redist, &dist, intid); - + if is_pending && is_enabled { let state_bits: u64 = 0b01; clear_irq_pending_locked(&mut redist, &mut dist, intid); @@ -493,6 +488,31 @@ pub fn flush(vcpu_id: usize) { } } + // Phase 3: SPI Active interrupts (lowest priority for LR allocation). + for i in 1..MAX_IRQS_WORDS { + if current_lr >= MAX_LR { break; } + let mut spi_active_word = dist.isactiver[i]; + + while spi_active_word != 0 && current_lr < MAX_LR { + let bit = spi_active_word.trailing_zeros(); + let intid = (i * 32) as u32 + bit as u32; + spi_active_word &= !(1 << bit); + let is_pending = is_irq_pending_locked(&redist, &dist, intid); + let is_enabled = is_irq_enabled_locked(&redist, &dist, intid); + let mut state_bits: u64 = 0b10; + + if is_pending && is_enabled { + state_bits |= 0b01; + dist.ispendr[i] &= !(1 << bit); + } + + let lr_val: u64 = (state_bits << 62) | (1 << 60) | (0xA0 << 48) | (intid as u64); + write_lr(current_lr, lr_val); + redist.lr_intids[current_lr] = intid; + current_lr += 1; + } + } + redist.used_lrs = current_lr; } } @@ -510,11 +530,26 @@ pub fn sync(vcpu_id: usize) { let lr_val = read_lr(i); let state = (lr_val >> 62) & 0b11; let intid = redist.lr_intids[i]; + let hw = (lr_val >> 61) & 1; + + // HW-mapped LR with state != Invalid: the physical interrupt is + // Active (Guest hasn't done virtual EOI yet). Invalidating the + // LR without DIR destroys the HW mapping, so the Guest's future + // virtual EOI won't auto-deactivate the physical INTID, leaving + // it permanently Active and silencing future assertions. + // DIR on an Active interrupt transitions it to Inactive; + // on Active-and-Pending it transitions to Pending (re-triggers). + // DIR on an Inactive interrupt is a safe no-op per GICv3 spec. + if hw != 0 && state != 0 { + let p_intid = (lr_val & 0x3FF) as u64; + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) p_intid, options(nostack)); + core::arch::asm!("isb", options(nostack)); + } + if state == 0 { clear_irq_state_locked(&mut redist, &mut dist, intid); } else { sync_irq_state_locked(&mut redist, &mut dist, intid, state); - // redist.push_queue(intid); if (state & 0b01) != 0 { redist.push_queue(intid); } @@ -588,13 +623,11 @@ unsafe fn write_lr(index: usize, val: u64) { fn clear_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistributor, intid: u32) { if intid < 32 { - // redist.ispendr0 &= !(1 << intid); redist.isactiver0 &= !(1 << intid); } else { // SPI should have lock. let index = (intid / 32) as usize; let mask = 1 << (intid % 32); - // dist.ispendr[index] &= !mask; dist.isactiver[index] &= !mask; } } @@ -607,9 +640,7 @@ fn sync_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistribu if is_pending { redist.ispendr0 |= 1 << intid; } - // } else { - // redist.ispendr0 &= !(1 << intid); - // } + if is_active { redist.isactiver0 |= 1 << intid; } else { @@ -621,9 +652,7 @@ fn sync_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistribu if is_pending { dist.ispendr[index] |= mask; } - // else { - // dist.ispendr[index] &= !mask; - // } + if is_active { dist.isactiver[index] |= mask; } else { @@ -681,14 +710,14 @@ mod tests { setup_test_env(); let mut redist = VgicRedistributor::new(); - // verify normal queuing. + // 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. + // Verify deadlock prevention optimization: ghost reuse deduplication. redist.push_queue(27); assert_eq!( redist.pending_count, 2, diff --git a/kernel/src/arch/aarch64/virt/virtio/console.rs b/kernel/src/arch/aarch64/virt/virtio/console.rs index 9b9f1f9f..1de54423 100644 --- a/kernel/src/arch/aarch64/virt/virtio/console.rs +++ b/kernel/src/arch/aarch64/virt/virtio/console.rs @@ -66,8 +66,8 @@ impl VirtioDevice for VirtioConsole { if !q.ready || q.desc_addr == 0 { return false; } - let mut processed = false; + let mut processed = false; unsafe { let desc_table = q.desc_addr as *const VirtqDesc; let avail_ring = q.avail_addr as *const VirtqAvail; @@ -90,8 +90,6 @@ impl VirtioDevice for VirtioConsole { let u_idx = ((*used_ring).idx % q.num as u16) as usize; (*used_ring).ring[u_idx].id = head as u32; (*used_ring).ring[u_idx].len = 0; - - // compiler_fence(Ordering::SeqCst); unsafe { core::arch::asm!("dmb ishst", options(nostack)); (*used_ring).idx = (*used_ring).idx.wrapping_add(1); diff --git a/kernel/src/arch/aarch64/virt/virtio/mmio.rs b/kernel/src/arch/aarch64/virt/virtio/mmio.rs index 7f1ddaa6..06ad4f1e 100644 --- a/kernel/src/arch/aarch64/virt/virtio/mmio.rs +++ b/kernel/src/arch/aarch64/virt/virtio/mmio.rs @@ -91,7 +91,6 @@ impl VirtioMmioTransport { pub fn inject_rx(&mut self, byte: u8) { if self.device.handle_rx(byte, &mut self.queues) { self.interrupt_status |= 1; - // inject_irq(0, 48); (self.irq_handler)(); } diff --git a/kernel/src/arch/aarch64/virt/virtio/mod.rs b/kernel/src/arch/aarch64/virt/virtio/mod.rs index 7dbdaced..5cdae204 100644 --- a/kernel/src/arch/aarch64/virt/virtio/mod.rs +++ b/kernel/src/arch/aarch64/virt/virtio/mod.rs @@ -17,10 +17,12 @@ pub mod mmio; pub use console::VirtioConsole; pub use mmio::VirtioMmioTransport; -use crate::arch::virt::vgic::inject_irq; +use crate::arch::virt::vgic; fn console_irq_handler() { - inject_irq(0, 48); + if let Some(vcpu_id) = super::get_current_vcpu_id() { + vgic::inject_irq(vcpu_id, 48); + } } pub static mut VIRTIO_CONSOLE: VirtioMmioTransport = diff --git a/kernel/src/arch/aarch64/virt/vtimer.rs b/kernel/src/arch/aarch64/virt/vtimer.rs index 8ba0888c..30b2cec5 100644 --- a/kernel/src/arch/aarch64/virt/vtimer.rs +++ b/kernel/src/arch/aarch64/virt/vtimer.rs @@ -26,11 +26,6 @@ pub struct VirtualTimerHandler; impl IsrDesc for VirtualTimerHandler { fn service_isr(&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); vgic::flush(vcpu_id); @@ -88,19 +83,19 @@ mod tests { use blueos_test_macro::test; #[test] - fn test_vtimer_handler_masking() { + fn test_vtimer_handler_injects_irq() { unsafe { MOCK_CNTV_CTL = 0; } let mut handler = VirtualTimerHandler; handler.service_isr(); - // Verify whether we have shield physical interrupt. (Bit 1 = IMASK) + // IMASK is no longer set — vGIC HW LR + EOImode prevents re-trigger. unsafe { assert_eq!( MOCK_CNTV_CTL & (1 << 1), - (1 << 1), - "vTimer must set IMASK (bit 1) to prevent IRQ storms" + 0, + "vTimer should NOT set IMASK (bit 1)" ); } } diff --git a/kernel/src/arch/aarch64/virt/vuart.rs b/kernel/src/arch/aarch64/virt/vuart.rs index e2542724..3b3ce22d 100644 --- a/kernel/src/arch/aarch64/virt/vuart.rs +++ b/kernel/src/arch/aarch64/virt/vuart.rs @@ -15,15 +15,10 @@ use super::{get_current_vcpu_id, vgic, virtio}; use blueos_hal::isr::IsrDesc; -pub fn forward_byte_to_guest(byte: u8) { - if let Some(vcpu_id) = get_current_vcpu_id() { - unsafe { - virtio::VIRTIO_CONSOLE.inject_rx(byte); - } +// PL011 UART addresses for QEMU Virt +const UART0_DR: u32 = 0x0900_0000; + - vgic::inject_irq(vcpu_id, 48); - } -} #[inline] pub fn handle_read(offset: u64) -> u64 { @@ -54,15 +49,11 @@ pub fn handle_write(offset: u64, value: u64) { pub fn handle_physical_uart_interrupt() { unsafe { - let uart_base = 0x0900_0000 as *mut u32; + let uart_base = UART0_DR as *mut u32; let mut injected = false; while (core::ptr::read_volatile(uart_base.add(0x18 / 4)) & (1 << 4)) == 0 { - let mut byte = core::ptr::read_volatile(uart_base) as u8; - if byte == b'\r' { - byte = b'\n'; - } - + let mut byte = core::ptr::read_volatile(uart_base) as u8; virtio::VIRTIO_CONSOLE.inject_rx(byte); injected = true; } @@ -77,11 +68,11 @@ pub fn handle_physical_uart_interrupt() { } } +// Enable UART RX and RT interrupts in PL011 IMSC register +// Without this, the hardware won't assert IRQ 33 when keys are pressed! pub fn enable_physical_uart_interrupts() { unsafe { - // Enable UART RX and RT interrupts in PL011 IMSC register - // Without this, the hardware won't assert IRQ 33 when keys are pressed! - let uart_base = 0x0900_0000 as *mut u32; + let uart_base = UART0_DR as *mut u32; let imsc = uart_base.add(0x38 / 4); core::ptr::write_volatile(imsc, core::ptr::read_volatile(imsc) | (1 << 4) | (1 << 6)); } @@ -89,8 +80,8 @@ pub fn enable_physical_uart_interrupts() { pub fn cleanup_physical_uart_interrupts() { unsafe { - let uart_base = 0x0900_0000 as *mut u32; - + let uart_base = UART0_DR as *mut u32; + // Disable RTI (bit 6) because BlueOS's native driver doesn't handle it. // If we leave it enabled, it causes an infinite interrupt storm in EL1! let imsc = uart_base.add(0x38 / 4); @@ -100,6 +91,8 @@ pub fn cleanup_physical_uart_interrupts() { // Clear any pending RTI to ensure a clean state for BlueOS let icr = uart_base.add(0x44 / 4); - core::ptr::write_volatile(icr, 1 << 6); + core::ptr::write_volatile(icr, (1 << 4) | (1 << 6)); + + let imsc_val = core::ptr::read_volatile(imsc); } } \ No newline at end of file From ef01f83243e8a4017dc3bab7651314b19038c492 Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 29 May 2026 15:41:50 +0800 Subject: [PATCH 08/10] change link.x back --- kernel/src/boards/qemu_virt64_aarch64/link.x | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kernel/src/boards/qemu_virt64_aarch64/link.x b/kernel/src/boards/qemu_virt64_aarch64/link.x index 5c4283f1..b38957a4 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 = 256M + DRAM : ORIGIN = 0x40280000, LENGTH = 32M } PHDRS From fcc3d06a048a86d0b25621000989e9786aa27229 Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 29 May 2026 16:02:42 +0800 Subject: [PATCH 09/10] add virtio so that we can use BlueOS consle --- kernel/src/arch/aarch64/virt/exit.rs | 3 --- 1 file changed, 3 deletions(-) diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index a3dbde2d..8d2f496e 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -87,9 +87,6 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { VmExitReason::InstructionAbortLowerEL => { let iss = esr & 0x1FFFFFF; let ifsc = iss & 0x3F; - let far: u64; - unsafe { core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); } - kearly_println!("[EXIT] InstructionAbort: ELR={:#018x} FAR={:#018x} IFSC={:#x}", elr, far, ifsc); false } VmExitReason::TrappedWfiWfe => trap_wfi_wfe(vcpu, &exit_info), From b5d5cdedb5b008663ad3e00bbc0300d01861a913 Mon Sep 17 00:00:00 2001 From: jinwjinl <2532191107@qq.com> Date: Fri, 29 May 2026 16:28:19 +0800 Subject: [PATCH 10/10] deleting virt_linux_boot call and using rustfmt --- kernel/src/arch/aarch64/virt/exit.rs | 42 ++++++---- kernel/src/arch/aarch64/virt/mod.rs | 6 +- kernel/src/arch/aarch64/virt/vcpu.rs | 2 +- kernel/src/arch/aarch64/virt/vector.rs | 16 +++- kernel/src/arch/aarch64/virt/vgic.rs | 35 ++++++-- .../src/arch/aarch64/virt/virtio/console.rs | 42 ++++++---- kernel/src/arch/aarch64/virt/virtio/mmio.rs | 82 ++++++++++++++++--- kernel/src/arch/aarch64/virt/virtio/mod.rs | 8 +- kernel/src/arch/aarch64/virt/vuart.rs | 30 ++++--- kernel/src/boards/qemu_virt64_aarch64/init.rs | 2 - 10 files changed, 188 insertions(+), 77 deletions(-) diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs index 8d2f496e..5034bd8e 100644 --- a/kernel/src/arch/aarch64/virt/exit.rs +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -13,8 +13,8 @@ // limitations under the License. use super::{guest, hyper, vcpu::Vcpu, vgic, virtio, vuart}; -use core::arch::asm; use crate::{kearly_println, kprintln}; +use core::arch::asm; static mut GUEST_SHUTDOWN: bool = false; @@ -91,7 +91,12 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { } VmExitReason::TrappedWfiWfe => trap_wfi_wfe(vcpu, &exit_info), VmExitReason::Unknown(ec) => { - kearly_println!("[EXIT] Unknown exit: EC={:#x} ESR={:#018x} ELR={:#018x}", ec, esr, elr); + kearly_println!( + "[EXIT] Unknown exit: EC={:#x} ESR={:#018x} ELR={:#018x}", + ec, + esr, + elr + ); false } }; @@ -103,12 +108,16 @@ pub fn handle_vm_exit(vcpu: &mut Vcpu) -> bool { if !result { if let VmExitReason::DataAbortLowerEL = reason { let far: u64; - unsafe { core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); } + unsafe { + core::arch::asm!("mrs {}, far_el2", out(reg) far, options(nostack)); + } kearly_println!(" Faulting IPA / FAR: {:#018x}", far); } loop { - unsafe { core::arch::asm!("wfe"); } + unsafe { + core::arch::asm!("wfe"); + } } } @@ -136,7 +145,7 @@ fn handle_hvc(vcpu: &mut Vcpu, info: &VmExitInfo) -> bool { context.regs[0] = version; } PSCI_MIGRATE_INFO_TYPE => { - context.regs[0] =2; + context.regs[0] = 2; } PSCI_SYSTEM_OFF | PSCI_SYSTEM_RESET => { unsafe { @@ -218,10 +227,10 @@ fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { let exact_ipa = fault_ipa_base | (far & 0xFFF); // Get target register index. let srt = ((iss >> 16) & 0x1F) as usize; - // For vUart. + // For vUart. if (VUART_IPA_BASE..VUART_IPA_BASE + VUART_IPA_SIZE).contains(&exact_ipa) { let offset = exact_ipa - VUART_IPA_BASE; - + if is_write { let value = vcpu.context().get_reg(srt); vuart::handle_write(offset, value); @@ -229,15 +238,17 @@ fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { let value = vuart::handle_read(offset); vcpu.context_mut().set_reg(srt, value); } - + vcpu.context_mut().set_elr(faulting_pc + 4); vgic::flush(vcpu.id()); return true; } - if (VIRTIO_MMIO_IPA_BASE..VIRTIO_MMIO_IPA_BASE + VIRTIO_MMIO_IPA_SIZE).contains(&exact_ipa) { + if (VIRTIO_MMIO_IPA_BASE..VIRTIO_MMIO_IPA_BASE + VIRTIO_MMIO_IPA_SIZE) + .contains(&exact_ipa) + { let offset = exact_ipa - VIRTIO_MMIO_IPA_BASE; - + if is_write { let value = vcpu.context().get_reg(srt) as u32; virtio::VIRTIO_CONSOLE.handle_write(offset, value); @@ -245,19 +256,15 @@ fn handle_data_abort(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { let value = virtio::VIRTIO_CONSOLE.handle_read(offset); vcpu.context_mut().set_reg(srt, value as u64); } - + vcpu.context_mut().set_elr(faulting_pc + 4); vgic::flush(vcpu.id()); return true; } // For vGic. - let handled = vgic::handle_data_abort( - vcpu.id(), - esr, - exact_ipa, - &mut vcpu.context_mut().regs, - ); + let handled = + vgic::handle_data_abort(vcpu.id(), esr, exact_ipa, &mut vcpu.context_mut().regs); if handled { vcpu.context_mut().set_elr(faulting_pc + 4); @@ -302,7 +309,6 @@ pub fn trap_wfi_wfe(vcpu: &mut Vcpu, exit_info: &VmExitInfo) -> bool { } } - pub fn is_guest_shutdown() -> bool { unsafe { GUEST_SHUTDOWN } } diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index 4257f8da..5abff57a 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -20,11 +20,11 @@ pub mod mmu_s2; pub mod vcpu; pub mod vector; pub mod vgic; +pub(crate) mod virtio; pub mod vtimer; pub mod vuart; -pub(crate) mod virtio; pub use crate::arch::aarch64::psci::hvc_call; -use blueos_hal::{PlatPeri, isr::IsrDesc}; +use blueos_hal::{isr::IsrDesc, PlatPeri}; pub use exit::{VmExitInfo, VmExitReason}; pub use hyper::{get_current_el, hyp_init}; pub use vcpu::{Vcpu, VcpuManager, VcpuState}; @@ -102,7 +102,7 @@ pub extern "C" fn hyper_trap_irq(_context: &mut crate::arch::aarch64::Context) - unsafe { core::arch::asm!("msr ICC_EOIR1_EL1, {}", in(reg) iar); - core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); + core::arch::asm!("msr ICC_DIR_EL1, {}", in(reg) iar); } vgic::flush(vcpu_id); diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs index a2a39912..b84f10b9 100644 --- a/kernel/src/arch/aarch64/virt/vcpu.rs +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -98,7 +98,7 @@ impl VcpuStateStruct { self.spsr = spsr; } - #[inline] + #[inline] pub fn set_reg(&mut self, index: usize, value: u64) { if index < 31 { self.regs[index] = value; diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index 236d56e3..63563524 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -233,7 +233,17 @@ unsafe fn save_host_context(frame: *mut u64) { } // A 13-element tuple type would exceed Clippy's complexity threshold. - let (vbar, sctlr, ttbr0, ttbr1, tcr, mair, pmr, sre, ctlr): (u64, u64, u64, u64, u64, u64, u64, u64, u64); + let (vbar, sctlr, ttbr0, ttbr1, tcr, mair, pmr, sre, ctlr): ( + u64, + u64, + u64, + u64, + u64, + u64, + u64, + u64, + u64, + ); let (tpidr_el0, tpidr_el1, tpidrro_el0, sp_el0): (u64, u64, u64, u64); core::arch::asm!( "mrs {vbar}, vbar_el1", @@ -361,7 +371,9 @@ unsafe fn restore_host_to_frame(frame: *mut u64) { ); let restored_pmr: u64; - unsafe { core::arch::asm!("mrs {}, ICC_PMR_EL1", out(reg) restored_pmr); } + unsafe { + core::arch::asm!("mrs {}, ICC_PMR_EL1", out(reg) restored_pmr); + } // Diagnostic: print key system registers after restoring Host let cpacr_after: u64; diff --git a/kernel/src/arch/aarch64/virt/vgic.rs b/kernel/src/arch/aarch64/virt/vgic.rs index b6f68228..4889824d 100644 --- a/kernel/src/arch/aarch64/virt/vgic.rs +++ b/kernel/src/arch/aarch64/virt/vgic.rs @@ -394,7 +394,7 @@ pub fn inject(vcpu_id: usize, intid: u32) { if is_enabled { let mut redist = get_vgic().redists[vcpu_id].lock(); redist.push_queue(intid); - } + } } } } @@ -450,7 +450,12 @@ pub fn flush(vcpu_id: usize) { let hw_bit = if is_hw { 1u64 << 61 } else { 0 }; let pintid_bits = if is_hw { (intid as u64) << 32 } else { 0 }; - let lr_val: u64 = (state_bits << 62) | hw_bit | (1 << 60) | (0xA0 << 48) | pintid_bits | (intid as u64); + let lr_val: u64 = (state_bits << 62) + | hw_bit + | (1 << 60) + | (0xA0 << 48) + | pintid_bits + | (intid as u64); write_lr(current_lr, lr_val); redist.lr_intids[current_lr] = intid; current_lr += 1; @@ -481,7 +486,12 @@ pub fn flush(vcpu_id: usize) { let hw_bit = if is_hw { 1u64 << 61 } else { 0 }; let pintid_bits = if is_hw { (intid as u64) << 32 } else { 0 }; - let lr_val: u64 = (state_bits << 62) | hw_bit | (1 << 60) | (0xA0 << 48) | pintid_bits | (intid as u64); + let lr_val: u64 = (state_bits << 62) + | hw_bit + | (1 << 60) + | (0xA0 << 48) + | pintid_bits + | (intid as u64); write_lr(current_lr, lr_val); redist.lr_intids[current_lr] = intid; current_lr += 1; @@ -490,7 +500,9 @@ pub fn flush(vcpu_id: usize) { // Phase 3: SPI Active interrupts (lowest priority for LR allocation). for i in 1..MAX_IRQS_WORDS { - if current_lr >= MAX_LR { break; } + if current_lr >= MAX_LR { + break; + } let mut spi_active_word = dist.isactiver[i]; while spi_active_word != 0 && current_lr < MAX_LR { @@ -632,7 +644,12 @@ fn clear_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistrib } } -fn sync_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistributor, intid: u32, state: u64) { +fn sync_irq_state_locked( + redist: &mut VgicRedistributor, + dist: &mut VgicDistributor, + intid: u32, + state: u64, +) { let is_pending = (state & 0b01) != 0; let is_active = (state & 0b10) != 0; @@ -651,7 +668,7 @@ fn sync_irq_state_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistribu let mask = 1 << (intid % 32); if is_pending { dist.ispendr[index] |= mask; - } + } if is_active { dist.isactiver[index] |= mask; @@ -685,7 +702,11 @@ fn is_irq_enabled_locked(redist: &VgicRedistributor, dist: &VgicDistributor, int } } -fn clear_irq_pending_locked(redist: &mut VgicRedistributor, dist: &mut VgicDistributor, intid: u32) { +fn clear_irq_pending_locked( + redist: &mut VgicRedistributor, + dist: &mut VgicDistributor, + intid: u32, +) { if intid < 32 { redist.ispendr0 &= !(1 << intid); } else { diff --git a/kernel/src/arch/aarch64/virt/virtio/console.rs b/kernel/src/arch/aarch64/virt/virtio/console.rs index 1de54423..b3705130 100644 --- a/kernel/src/arch/aarch64/virt/virtio/console.rs +++ b/kernel/src/arch/aarch64/virt/virtio/console.rs @@ -12,27 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. +use super::{VirtQueue, VirtioDevice, VirtqAvail, VirtqDesc, VirtqUsed}; +use crate::{kearly_println, kprintln}; use core::sync::atomic::{compiler_fence, Ordering}; -use super::{VirtioDevice, VirtQueue, VirtqDesc, VirtqAvail, VirtqUsed}; -use crate::{kprintln, kearly_println}; pub struct VirtioConsole; impl VirtioConsole { - pub const fn new() -> Self { Self } + pub const fn new() -> Self { + Self + } pub fn handle_rx(&mut self, byte: u8, queues: &mut [VirtQueue]) -> bool { // RX Queue is index 0. let q_idx = 0; let q = &mut queues[q_idx]; - - if !q.ready || q.desc_addr == 0 { return false; } + + if !q.ready || q.desc_addr == 0 { + return false; + } unsafe { let avail = q.avail_addr as *const VirtqAvail; - let used = q.used_addr as *mut VirtqUsed; + let used = q.used_addr as *mut VirtqUsed; let desc_table = q.desc_addr as *const VirtqDesc; - if q.last_idx == (*avail).idx { return false; } + if q.last_idx == (*avail).idx { + return false; + } let ring_index = (q.last_idx % q.num as u16) as usize; let head = (*avail).ring[ring_index] as usize; @@ -52,8 +58,12 @@ impl VirtioConsole { } impl VirtioDevice for VirtioConsole { - fn device_id(&self) -> u32 { 3 } - fn vendor_id(&self) -> u32 { 0x424C5545 } + fn device_id(&self) -> u32 { + 3 + } + fn vendor_id(&self) -> u32 { + 0x424C5545 + } fn handle_status(&mut self, _status: u32) {} fn handle_kick(&mut self, q_idx: usize, queues: &mut [VirtQueue]) -> bool { @@ -61,7 +71,7 @@ impl VirtioDevice for VirtioConsole { if q_idx != 1 { return false; } - + let q = &mut queues[q_idx]; if !q.ready || q.desc_addr == 0 { return false; @@ -71,7 +81,7 @@ impl VirtioDevice for VirtioConsole { unsafe { let desc_table = q.desc_addr as *const VirtqDesc; let avail_ring = q.avail_addr as *const VirtqAvail; - let used_ring = q.used_addr as *mut VirtqUsed; + let used_ring = q.used_addr as *mut VirtqUsed; while q.last_idx != (*avail_ring).idx { processed = true; @@ -83,7 +93,9 @@ impl VirtioDevice for VirtioConsole { for byte in s.as_bytes() { let mut writer = crate::console::EarlyConsole {}; use core::fmt::Write; - writer.write_str(core::str::from_utf8(&[*byte]).unwrap_or("")).unwrap(); + writer + .write_str(core::str::from_utf8(&[*byte]).unwrap_or("")) + .unwrap(); } } @@ -91,13 +103,13 @@ impl VirtioDevice for VirtioConsole { (*used_ring).ring[u_idx].id = head as u32; (*used_ring).ring[u_idx].len = 0; unsafe { - core::arch::asm!("dmb ishst", options(nostack)); + core::arch::asm!("dmb ishst", options(nostack)); (*used_ring).idx = (*used_ring).idx.wrapping_add(1); q.last_idx = q.last_idx.wrapping_add(1); - core::arch::asm!("dsb ishst", options(nostack)); + core::arch::asm!("dsb ishst", options(nostack)); } } } processed } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/virtio/mmio.rs b/kernel/src/arch/aarch64/virt/virtio/mmio.rs index 06ad4f1e..eab7dfd3 100644 --- a/kernel/src/arch/aarch64/virt/virtio/mmio.rs +++ b/kernel/src/arch/aarch64/virt/virtio/mmio.rs @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -use super::{VirtioDevice, VirtQueue, console::VirtioConsole}; +use super::{console::VirtioConsole, VirtQueue, VirtioDevice}; pub struct VirtioMmioTransport { pub device: T, @@ -32,7 +32,14 @@ impl VirtioMmioTransport { queue_sel: 0, devices_features_sel: 0, interrupt_status: 0, - queues: [VirtQueue { ready: false, num: 0, desc_addr: 0, avail_addr: 0, used_addr: 0, last_idx: 0 }; 2], + queues: [VirtQueue { + ready: false, + num: 0, + desc_addr: 0, + avail_addr: 0, + used_addr: 0, + last_idx: 0, + }; 2], irq_handler, } } @@ -51,7 +58,13 @@ impl VirtioMmioTransport { } } 0x034 => 256, - 0x044 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].ready as u32 } else { 0 }, + 0x044 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].ready as u32 + } else { + 0 + } + } 0x60 => self.interrupt_status, 0x070 => self.status, _ => 0, @@ -62,8 +75,16 @@ impl VirtioMmioTransport { match offset { 0x014 => self.devices_features_sel = value, 0x030 => self.queue_sel = value, - 0x038 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].num = value }, - 0x044 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].ready = value == 1 }, + 0x038 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].num = value + } + } + 0x044 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].ready = value == 1 + } + } 0x050 => { let should_interrupt = self.device.handle_kick(value as usize, &mut self.queues); if should_interrupt { @@ -76,12 +97,48 @@ impl VirtioMmioTransport { self.status = value; self.device.handle_status(value); } - 0x080 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].desc_addr = (self.queues[self.queue_sel as usize].desc_addr & !0xFFFF_FFFF) | value as u64 }, - 0x084 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].desc_addr = (self.queues[self.queue_sel as usize].desc_addr & 0xFFFF_FFFF) | ((value as u64) << 32) }, - 0x090 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].avail_addr = (self.queues[self.queue_sel as usize].avail_addr & !0xFFFF_FFFF) | value as u64 }, - 0x094 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].avail_addr = (self.queues[self.queue_sel as usize].avail_addr & 0xFFFF_FFFF) | ((value as u64) << 32) }, - 0x0a0 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].used_addr = (self.queues[self.queue_sel as usize].used_addr & !0xFFFF_FFFF) | value as u64 }, - 0x0a4 => if self.queue_sel < 2 { self.queues[self.queue_sel as usize].used_addr = (self.queues[self.queue_sel as usize].used_addr & 0xFFFF_FFFF) | ((value as u64) << 32) }, + 0x080 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].desc_addr = + (self.queues[self.queue_sel as usize].desc_addr & !0xFFFF_FFFF) + | value as u64 + } + } + 0x084 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].desc_addr = + (self.queues[self.queue_sel as usize].desc_addr & 0xFFFF_FFFF) + | ((value as u64) << 32) + } + } + 0x090 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].avail_addr = + (self.queues[self.queue_sel as usize].avail_addr & !0xFFFF_FFFF) + | value as u64 + } + } + 0x094 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].avail_addr = + (self.queues[self.queue_sel as usize].avail_addr & 0xFFFF_FFFF) + | ((value as u64) << 32) + } + } + 0x0a0 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].used_addr = + (self.queues[self.queue_sel as usize].used_addr & !0xFFFF_FFFF) + | value as u64 + } + } + 0x0a4 => { + if self.queue_sel < 2 { + self.queues[self.queue_sel as usize].used_addr = + (self.queues[self.queue_sel as usize].used_addr & 0xFFFF_FFFF) + | ((value as u64) << 32) + } + } _ => {} } } @@ -92,7 +149,6 @@ impl VirtioMmioTransport { if self.device.handle_rx(byte, &mut self.queues) { self.interrupt_status |= 1; (self.irq_handler)(); - } } -} \ No newline at end of file +} diff --git a/kernel/src/arch/aarch64/virt/virtio/mod.rs b/kernel/src/arch/aarch64/virt/virtio/mod.rs index 5cdae204..9ef70221 100644 --- a/kernel/src/arch/aarch64/virt/virtio/mod.rs +++ b/kernel/src/arch/aarch64/virt/virtio/mod.rs @@ -15,9 +15,9 @@ pub mod console; pub mod mmio; +use crate::arch::virt::vgic; pub use console::VirtioConsole; pub use mmio::VirtioMmioTransport; -use crate::arch::virt::vgic; fn console_irq_handler() { if let Some(vcpu_id) = super::get_current_vcpu_id() { @@ -25,7 +25,7 @@ fn console_irq_handler() { } } -pub static mut VIRTIO_CONSOLE: VirtioMmioTransport = +pub static mut VIRTIO_CONSOLE: VirtioMmioTransport = VirtioMmioTransport::new(VirtioConsole::new(), console_irq_handler); #[repr(C)] @@ -71,5 +71,5 @@ pub trait VirtioDevice { fn device_id(&self) -> u32; fn vendor_id(&self) -> u32; fn handle_status(&mut self, status: u32); - fn handle_kick(&mut self, q_idx: usize, queues: &mut [VirtQueue]) -> bool; -} \ No newline at end of file + fn handle_kick(&mut self, q_idx: usize, queues: &mut [VirtQueue]) -> bool; +} diff --git a/kernel/src/arch/aarch64/virt/vuart.rs b/kernel/src/arch/aarch64/virt/vuart.rs index 3b3ce22d..6e5078dd 100644 --- a/kernel/src/arch/aarch64/virt/vuart.rs +++ b/kernel/src/arch/aarch64/virt/vuart.rs @@ -18,14 +18,18 @@ use blueos_hal::isr::IsrDesc; // PL011 UART addresses for QEMU Virt const UART0_DR: u32 = 0x0900_0000; - - #[inline] pub fn handle_read(offset: u64) -> u64 { - match offset { - 0xFE0 => 0x11, 0xFE4 => 0x10, 0xFE8 => 0x14, 0xFEC => 0x00, - 0xFF0 => 0x0D, 0xFF4 => 0xF0, 0xFF8 => 0x05, 0xFFC => 0xB1, - 0x018 => 0x90, + match offset { + 0xFE0 => 0x11, + 0xFE4 => 0x10, + 0xFE8 => 0x14, + 0xFEC => 0x00, + 0xFF0 => 0x0D, + 0xFF4 => 0xF0, + 0xFF8 => 0x05, + 0xFFC => 0xB1, + 0x018 => 0x90, 0x024 => 0x24, 0x030 => 0x301, 0x038 => 0x00, @@ -40,7 +44,9 @@ pub fn handle_write(offset: u64, value: u64) { let c = (value & 0xFF) as u8; let mut writer = crate::console::EarlyConsole {}; use core::fmt::Write; - writer.write_str(core::str::from_utf8(&[c]).unwrap_or("")).unwrap(); + writer + .write_str(core::str::from_utf8(&[c]).unwrap_or("")) + .unwrap(); } 0x030..=0x044 => (), _ => {} @@ -51,19 +57,19 @@ pub fn handle_physical_uart_interrupt() { unsafe { let uart_base = UART0_DR as *mut u32; let mut injected = false; - + while (core::ptr::read_volatile(uart_base.add(0x18 / 4)) & (1 << 4)) == 0 { - let mut byte = core::ptr::read_volatile(uart_base) as u8; + let mut byte = core::ptr::read_volatile(uart_base) as u8; virtio::VIRTIO_CONSOLE.inject_rx(byte); injected = true; } - + if injected { if let Some(vcpu_id) = get_current_vcpu_id() { vgic::inject_irq(vcpu_id, 48); } } - + core::ptr::write_volatile(uart_base.add(0x44 / 4), (1 << 4) | (1 << 6)); } } @@ -95,4 +101,4 @@ pub fn cleanup_physical_uart_interrupts() { let imsc_val = core::ptr::read_volatile(imsc); } -} \ No newline at end of file +} diff --git a/kernel/src/boards/qemu_virt64_aarch64/init.rs b/kernel/src/boards/qemu_virt64_aarch64/init.rs index ccd1544a..571f815e 100644 --- a/kernel/src/boards/qemu_virt64_aarch64/init.rs +++ b/kernel/src/boards/qemu_virt64_aarch64/init.rs @@ -20,7 +20,6 @@ use crate::{ irq::{IrqTrigger, Priority}, mmu, registers::cntfrq_el0::CNTFRQ_EL0, - virt::virt_boot_linux, }, error::Error, irq::IrqTrace, @@ -80,7 +79,6 @@ pub(crate) fn init() { ), ); let _ = irq::register_handler(config::GENERIC_TIMER_IRQNUM, Box::new(TimerIrq {})); - virt_boot_linux(); } crate::define_peripheral! {