islet_rmm/exception/lower/synchronous/
sys_reg.rs

1use crate::exception::trap;
2use crate::rec::Rec;
3
4use aarch64_cpu::registers::*;
5use armv9a::regs::*;
6
7fn check_sysreg_id_access(esr: u64) -> bool {
8    let esr = ISS::new(esr);
9    (esr.get_masked(ISS::Op0) | esr.get_masked(ISS::Op1) | esr.get_masked(ISS::CRn)) == ISS::Op0
10}
11
12fn check_sysreg_icc_access(esr: u64) -> bool {
13    let esr = ISS::new(esr);
14    // direction: 0b0 - write, 0b1 - read
15    let direction = esr.get_masked_value(ISS::Direction);
16    let esr_iss = esr.get_masked(ISS::Op0 | ISS::Op1 | ISS::CRn | ISS::CRm) as u32;
17
18    // Only writing to the system register is valid.
19    direction == 0 && (esr_iss == ISS_ID_ICC_MASK || esr_iss == ISS_ID_ICC_PMR_EL1)
20}
21
22pub fn handle(rec: &mut Rec<'_>, esr: u64) -> u64 {
23    if check_sysreg_id_access(esr) {
24        handle_sysreg_id(rec, esr);
25    } else if check_sysreg_icc_access(esr) {
26        return trap::RET_TO_RMM;
27    } else {
28        warn!("Unhandled MSR/MRS instruction. ESR_EL2:{:X}", esr);
29    }
30    trap::RET_TO_REC
31}
32
33fn handle_sysreg_id(rec: &mut Rec<'_>, esr: u64) -> u64 {
34    let esr = ISS::new(esr);
35    let il = esr.get_masked_value(ISS::IL);
36    let rt = esr.get_masked_value(ISS::Rt) as usize;
37    // direction: 0b0 - write, 0b1 - read
38    let direction = esr.get_masked_value(ISS::Direction);
39
40    if il == 0 {
41        error!("Exception taken from 32bit arch. Realm needs to be 64bit(arm64).");
42    }
43    if direction == 0 {
44        warn!("Unable to write id system reg. Will ignore this request!");
45        return trap::RET_TO_REC;
46    }
47    if rt == 31 {
48        trace!("handle_sysreg_id(): Rt = xzr");
49        return trap::RET_TO_REC;
50    }
51
52    let idreg = esr.get_masked(ISS::Op0 | ISS::Op1 | ISS::CRn | ISS::CRm | ISS::Op2);
53
54    let mut mask: u64 = match idreg as u32 {
55        ISS_ID_AA64PFR0_EL1 => {
56            (ID_AA64PFR0_EL1::AMU.mask << ID_AA64PFR0_EL1::AMU.shift)
57                + if !rec.context.simd.cfg.sve_en {
58                    ID_AA64PFR0_EL1::SVE.mask << ID_AA64PFR0_EL1::SVE.shift
59                } else {
60                    0
61                }
62        }
63        // Present FEAT_SVE only if Rec is set to use SVE.
64        ISS_ID_AA64ZFR0_EL1 => (!rec.context.simd.cfg.sve_en as u64).wrapping_neg(),
65        ISS_ID_AA64PFR1_EL1 => {
66            (ID_AA64PFR1_SME_EL1::MTE.mask << ID_AA64PFR1_SME_EL1::MTE.shift)
67                + (ID_AA64PFR1_SME_EL1::SME.mask << ID_AA64PFR1_SME_EL1::SME.shift)
68        }
69        ISS_ID_AA64DFR0_EL1 => {
70            (ID_AA64DFR0_EL1::BRBE.mask << ID_AA64DFR0_EL1::BRBE.shift)
71                + (ID_AA64DFR0_EL1::MTPMU.mask << ID_AA64DFR0_EL1::MTPMU.shift)
72                + (ID_AA64DFR0_EL1::TraceBuffer.mask << ID_AA64DFR0_EL1::TraceBuffer.shift)
73                + (ID_AA64DFR0_EL1::TraceFilt.mask << ID_AA64DFR0_EL1::TraceFilt.shift)
74                + (ID_AA64DFR0_EL1::PMSVer.mask << ID_AA64DFR0_EL1::PMSVer.shift)
75                + (ID_AA64DFR0_EL1::CTX_CMPs.mask << ID_AA64DFR0_EL1::CTX_CMPs.shift)
76                + (ID_AA64DFR0_EL1::WRPs.mask << ID_AA64DFR0_EL1::WRPs.shift)
77                + (ID_AA64DFR0_EL1::BRPs.mask << ID_AA64DFR0_EL1::BRPs.shift)
78                + (ID_AA64DFR0_EL1::TraceVer.mask << ID_AA64DFR0_EL1::TraceVer.shift)
79                + (ID_AA64DFR0_EL1::DebugVer.mask << ID_AA64DFR0_EL1::DebugVer.shift)
80        }
81        _ => 0,
82    };
83    mask = !mask;
84
85    rec.context.gp_regs[rt] = match idreg as u32 {
86        ISS_ID_AA64PFR0_EL1 => ID_AA64PFR0_EL1.get() & mask,
87        ISS_ID_AA64PFR1_EL1 => ID_AA64PFR1_EL1.get() & mask,
88        ISS_ID_AA64ZFR0_EL1 => ID_AA64ZFR0_EL1.get() & mask,
89        ISS_ID_AA64DFR0_EL1 => {
90            let mut dfr0_set = 0u64;
91            dfr0_set &= 6 << ID_AA64DFR0_EL1::DebugVer.shift;
92            dfr0_set &= 1 << ID_AA64DFR0_EL1::BRPs.shift;
93            dfr0_set &= 1 << ID_AA64DFR0_EL1::WRPs.shift;
94            ID_AA64DFR0_EL1.get() & mask | dfr0_set
95        }
96        ISS_ID_AA64DFR1_EL1 => ID_AA64DFR1_EL1.get() & mask,
97        ISS_ID_AA64AFR0_EL1 => ID_AA64AFR0_EL1.get(),
98        ISS_ID_AA64AFR1_EL1 => ID_AA64AFR1_EL1.get(),
99        ISS_ID_AA64ISAR0_EL1 => ID_AA64ISAR0_EL1.get(),
100        ISS_ID_AA64ISAR1_EL1 => ID_AA64ISAR1_EL1.get(),
101        ISS_ID_AA64MMFR0_EL1 => ID_AA64MMFR0_EL1.get(),
102        ISS_ID_AA64MMFR1_EL1 => ID_AA64MMFR1_EL1.get(),
103        ISS_ID_AA64MMFR2_EL1 => ID_AA64MMFR2_EL1.get(), //0x10211122,
104        _ => 0x0,
105    };
106    trap::RET_TO_REC
107}
islet_rmm/exception/lower/synchronous/sys_reg.rs

islet_rmm/exception/lower/synchronous/
sys_reg.rs
