1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170

use super::timer;
use crate::cpu::get_cpu_id;
use crate::gic;
use crate::realm::vcpu::VCPU;
use crate::rmi::error::Error;
use crate::rmi::error::InternalError::*;
use crate::rmi::realm::Rd;

use armv9a::regs::*;

#[repr(C)]
#[derive(Default, Debug)]
pub struct Context {
    pub gp_regs: [u64; 31],
    pub elr: u64,
    pub spsr: u64,
    pub sys_regs: SystemRegister,
    pub gic_state: GICRegister,
    pub timer: TimerRegister,
    pub fp_regs: [u128; 32],
}

pub fn set_reg(rd: &Rd, vcpu: usize, register: usize, value: usize) -> Result<(), Error> {
    match register {
        0..=30 => {
            rd.vcpus
                .get(vcpu)
                .ok_or(Error::RmiErrorOthers(NotExistVCPU))?
                .lock()
                .context
                .gp_regs[register] = value as u64;
            Ok(())
        }
        31 => {
            rd.vcpus
                .get(vcpu)
                .ok_or(Error::RmiErrorOthers(NotExistVCPU))?
                .lock()
                .context
                .elr = value as u64;
            Ok(())
        }
        32 => {
            rd.vcpus
                .get(vcpu)
                .ok_or(Error::RmiErrorOthers(NotExistVCPU))?
                .lock()
                .context
                .spsr = value as u64;
            Ok(())
        }
        _ => Err(Error::RmiErrorInput),
    }?;
    Ok(())
}

pub fn get_reg(rd: &Rd, vcpu: usize, register: usize) -> Result<usize, Error> {
    match register {
        0..=30 => {
            let value = rd
                .vcpus
                .get(vcpu)
                .ok_or(Error::RmiErrorOthers(NotExistVCPU))?
                .lock()
                .context
                .gp_regs[register];
            Ok(value as usize)
        }
        31 => {
            let value = rd
                .vcpus
                .get(vcpu)
                .ok_or(Error::RmiErrorOthers(NotExistVCPU))?
                .lock()
                .context
                .elr;
            Ok(value as usize)
        }
        _ => Err(Error::RmiErrorInput),
    }
}

impl Context {
    pub fn new() -> Self {
        // Set appropriate sys registers
        // TODO: enable floating point
        // CPTR_EL2, CPACR_EL1, update vectors.s, etc..
        Self {
            spsr: SPSR_EL2::D | SPSR_EL2::A | SPSR_EL2::I | SPSR_EL2::F | (SPSR_EL2::M & 0b0101),
            ..Default::default()
        }
    }

    pub unsafe fn into_current(vcpu: &mut VCPU) {
        vcpu.pcpu = Some(get_cpu_id());
        vcpu.context.sys_regs.vmpidr = vcpu.pcpu.unwrap() as u64;
        TPIDR_EL2.set(vcpu as *const _ as u64);
        gic::restore_state(vcpu);
        timer::restore_state(vcpu);
    }

    pub unsafe fn from_current(vcpu: &mut VCPU) {
        gic::save_state(vcpu);
        timer::save_state(vcpu);
        vcpu.pcpu = None;
        //vcpu.context.sys_regs.vmpidr = 0u64;
        //TPIDR_EL2.set(0u64);
    }
}

/// Generic Interrupt Controller Registers
#[repr(C)]
#[derive(Default, Debug)]
pub struct GICRegister {
    // Interrupt Controller Hyp Active Priorities Group 0 Registers
    pub ich_ap0r_el2: [u64; 4],
    // Interrupt Controller Hyp Active Priorities Group 1 Registers
    pub ich_ap1r_el2: [u64; 4],
    // GICv3 Virtual Machine Control Register
    pub ich_vmcr_el2: u64,
    // Interrupt Controller Hyp Control Register
    pub ich_hcr_el2: u64,
    // GICv3 List Registers
    pub ich_lr_el2: [u64; 16],
    // GICv3 Maintenance Interrupt State Register
    pub ich_misr_el2: u64,
}

#[repr(C)]
#[derive(Default, Debug)]
pub struct SystemRegister {
    pub sp: u64,
    pub sp_el0: u64,
    pub esr_el1: u64,
    pub vbar: u64,
    pub ttbr0: u64,
    pub ttbr1: u64,
    pub mair: u64,
    pub amair: u64,
    pub tcr: u64,
    pub tpidr: u64,
    pub tpidr_el0: u64,
    pub tpidrro: u64,
    pub actlr: u64,
    pub vmpidr: u64,
    pub csselr: u64,
    pub cpacr: u64,
    pub afsr0: u64,
    pub afsr1: u64,
    pub far: u64,
    pub contextidr: u64,
    pub cntkctl: u64,
    pub par: u64,
    pub vttbr: u64,
    pub esr_el2: u64,
    pub hpfar: u64,
    pub sctlr: u64,
}

#[repr(C)]
#[derive(Default, Debug)]
pub struct TimerRegister {
    pub cntvoff_el2: u64,
    pub cntv_cval_el0: u64,
    pub cntv_ctl_el0: u64,
    pub cntpoff_el2: u64,
    pub cntp_cval_el0: u64,
    pub cntp_ctl_el0: u64,
    pub cnthctl_el2: u64,
}