diff --git a/kernel/src/arch/aarch64/registers/hcr_el2.rs b/kernel/src/arch/aarch64/registers/hcr_el2.rs index 7cdfecea..d8f8dbc2 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/registers/mair_el2.rs b/kernel/src/arch/aarch64/registers/mair_el2.rs new file mode 100644 index 00000000..5485676c --- /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 {}; 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..f6d0d62e --- /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 {}; 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..52a3848a --- /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 {}; 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..a7b120a1 --- /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 {}; 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..b6e715ce --- /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 {}; 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..c46e8d2f --- /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 {}; 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..5c4fc08a --- /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 {}; diff --git a/kernel/src/arch/aarch64/virt/exit.rs b/kernel/src/arch/aarch64/virt/exit.rs new file mode 100644 index 00000000..49381abb --- /dev/null +++ b/kernel/src/arch/aarch64/virt/exit.rs @@ -0,0 +1,336 @@ +// 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::{guest, hyper, vcpu::Vcpu, vgic}; +use core::arch::asm; +#[cfg(test)] +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; + +#[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 | 0x17 => 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, + }; + + 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::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; + + 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) => { + #[cfg(test)] + 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]; + let vcpu_id = vcpu.id(); + let context = vcpu.context_mut(); + 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 { + GUEST_SHUTDOWN = true; + } + need_advance_pc = false; + return false; + } + PSCI_FEATURES => { + let feature_id = context.regs[1] as u32; + if feature_id == PSCI_SYSTEM_OFF || feature_id == PSCI_SYSTEM_RESET { + context.regs[0] = 0; + } else { + context.regs[0] = 0xFFFF_FFFF; + } + } + _ => { + #[cfg(test)] + semihosting::println!("[EXIT] HVC#0: Ignored PSCI call: {:#x}", psci_func_id); + context.regs[0] = 0xFFFF_FFFF; + } + } + + true + } + HVC_VMM_GET_INFO => { + context.regs[0] = 0x48495001; + true + } + _ => { + #[cfg(test)] + semihosting::println!("[EXIT] Unknown HVC Number"); + true + } + }; + + if result && need_advance_pc { + context.elr_el2 += 4; + } + + 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]; + + match svc_num { + 0 => { + context.regs[0] = 0; + } + 1 => { + context.regs[0] = 1; + } + _ => { + 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); + } +} diff --git a/kernel/src/arch/aarch64/virt/guest.rs b/kernel/src/arch/aarch64/virt/guest.rs new file mode 100644 index 00000000..41b700b6 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/guest.rs @@ -0,0 +1,19 @@ +// 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 LINUX_KERNEL_LOAD_ADDR: usize = 0x4400_0000; +pub const LINUX_DTB_ADDR: usize = 0x4E00_0000; +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 4e1302c4..fd05a823 100644 --- a/kernel/src/arch/aarch64/virt/hyper.rs +++ b/kernel/src/arch/aarch64/virt/hyper.rs @@ -12,7 +12,10 @@ // 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, sctlr_el2::SCTLR_EL2, spsr_el2::SPSR_EL2}, + virt::{mmu_el2, vector}, +}; use tock_registers::interfaces::{Readable, Writeable}; #[inline] @@ -73,6 +76,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 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, + ); + unsafe { + core::arch::asm!("isb"); + } +} + #[inline] fn configure_vector_table(vector_base: usize) { unsafe { @@ -84,10 +113,51 @@ 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)); 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..ae3cfe1a --- /dev/null +++ b/kernel/src/arch/aarch64/virt/mmu_el2.rs @@ -0,0 +1,111 @@ +// 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::{ + 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 [ + 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)); + } +} 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..c9aab92c --- /dev/null +++ b/kernel/src/arch/aarch64/virt/mmu_s2.rs @@ -0,0 +1,198 @@ +// 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::*; + +// 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); + + 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" + ); + } +} diff --git a/kernel/src/arch/aarch64/virt/mod.rs b/kernel/src/arch/aarch64/virt/mod.rs index 9e0f694b..a20249ef 100644 --- a/kernel/src/arch/aarch64/virt/mod.rs +++ b/kernel/src/arch/aarch64/virt/mod.rs @@ -12,13 +12,99 @@ // See the License for the specific language governing permissions and // limitations under the License. +pub mod exit; +pub mod guest; pub mod hyper; +pub mod mmu_el2; +pub mod mmu_s2; +pub mod vcpu; pub mod vector; +pub mod vgic; +pub mod vtimer; +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}; +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; // 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 == 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 { + #[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. + 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 +113,47 @@ 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(); + + // 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(); + 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); + + /// 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(); + #[cfg(test)] + semihosting::println!("Linux shutdown!!!"); + } +} diff --git a/kernel/src/arch/aarch64/virt/vcpu.rs b/kernel/src/arch/aarch64/virt/vcpu.rs new file mode 100644 index 00000000..548c4b5c --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vcpu.rs @@ -0,0 +1,424 @@ +// 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::{ + 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; +/// 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 + } +} + +// Temporarily set 4 vCPUs，though we use one. +#[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" + ); + } +} diff --git a/kernel/src/arch/aarch64/virt/vector.rs b/kernel/src/arch/aarch64/virt/vector.rs index a7aca83e..bf4e99e0 100644 --- a/kernel/src/arch/aarch64/virt/vector.rs +++ b/kernel/src/arch/aarch64/virt/vector.rs @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -use super::hyper; +use super::{ + exit::{clear_guest_shutdown, handle_vm_exit, is_guest_shutdown}, + guest, hyper, + vcpu::Vcpu, + vgic, VCPU_MANAGER, +}; use core::arch::asm; static mut PRINTED_ALIGN: bool = false; @@ -105,7 +110,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 +140,224 @@ 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", + "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", + "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) + 4; + 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); } - // EC = 0x07 (Access to SIMD/FP) - if ec == 0x07 { - asm!("msr cptr_el2, xzr"); - return 1; + 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; + // Restore Host GPRs (x0-x30) + for i in 0..31 { + *frame.add(i) = VCPU_MANAGER.0.host_regs[i]; } - 0 + // 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; + 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, + ); +} + +unsafe fn save_frame_to_context(frame: *mut u64, vcpu: &mut Vcpu) { + let mut ctx = vcpu.context_mut(); + for i in 0..31 { + ctx.regs[i] = *frame.add(i); + } + 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; + + // 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", + 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 @@ -198,16 +388,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/arch/aarch64/virt/vgic.rs b/kernel/src/arch/aarch64/virt/vgic.rs new file mode 100644 index 00000000..9332b138 --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vgic.rs @@ -0,0 +1,602 @@ +// 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::VCPU_MANAGER; +use crate::sync::SpinLock; +use core::arch::asm; +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" + ); + } + } +} diff --git a/kernel/src/arch/aarch64/virt/vtimer.rs b/kernel/src/arch/aarch64/virt/vtimer.rs new file mode 100644 index 00000000..6e0f47fc --- /dev/null +++ b/kernel/src/arch/aarch64/virt/vtimer.rs @@ -0,0 +1,106 @@ +// 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::vgic; +use crate::arch::aarch64::{ + current_cpu_id, + irq::{self, IrqNumber, Priority}, +}; +use alloc::boxed::Box; +use blueos_hal::isr::IsrDesc; +use core::arch::asm; + +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); + } + } +} + +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.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" + ); + } + } +} diff --git a/kernel/src/boards/qemu_virt64_aarch64/link.x b/kernel/src/boards/qemu_virt64_aarch64/link.x index b38957a4..5c4283f1 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