Set aarch64 thread pointers

src/arch/aarch64/device/cpu/registers/control_regs.rs

@@ -40,6 +40,12 @@ pub unsafe fn mair_el1_write(val: MairEl1) {
     asm!("msr mair_el1, {}", in(reg) val.bits());
 }
 
+pub unsafe fn tpidr_el0() -> u64 {
+    let ret: u64;
+    asm!("mrs {}, tpidr_el0", out(reg) ret);
+    ret
+}
+
 pub unsafe fn tpidr_el0_write(val: u64) {
     asm!("msr tpidr_el0, {}", in(reg) val);
 }
@@ -48,6 +54,16 @@ pub unsafe fn tpidr_el1_write(val: u64) {
     asm!("msr tpidr_el1, {}", in(reg) val);
 }
 
+pub unsafe fn tpidrro_el0() -> u64 {
+    let ret: u64;
+    asm!("mrs {}, tpidrro_el0", out(reg) ret);
+    ret
+}
+
+pub unsafe fn tpidrro_el0_write(val: u64) {
+    asm!("msr tpidrro_el0, {}", in(reg) val);
+}
+
 pub unsafe fn esr_el1() -> u32 {
     let ret: u32;
     asm!("mrs {}, esr_el1", out(reg) ret);

src/context/arch/aarch64.rs

@@ -25,8 +25,8 @@ pub const KFX_ALIGN: usize = 16;
 pub struct Context {
     elr_el1: usize,
     sp_el0: usize,
-    tpidr_el0: usize,   /* Pointer to TLS region for this Context               */
-    tpidrro_el0: usize, /* Pointer to TLS (read-only) region for this Context   */
+    pub(crate) tpidr_el0: usize,   /* Pointer to TLS region for this Context               */
+    pub(crate) tpidrro_el0: usize, /* Pointer to TLS (read-only) region for this Context   */
     spsr_el1: usize,
     esr_el1: usize,
     fx_loadable: bool,

src/scheme/proc.rs

@@ -400,10 +400,26 @@ impl ProcScheme {
 
     #[cfg(target_arch = "aarch64")]
     fn read_env_regs(&self, info: &Info) -> Result<EnvRegisters> {
-        //TODO: aarch64 EnvRegisters
+        use crate::device::cpu::registers::control_regs;
+
+        let (tpidr_el0, tpidrro_el0) = if info.pid == context::context_id() {
+            unsafe {
+                (
+                    control_regs::tpidr_el0() as usize,
+                    control_regs::tpidrro_el0() as usize,
+                )
+            }
+        } else {
+            try_stop_context(info.pid, |context| {
+                Ok((
+                    context.arch.tpidr_el0,
+                    context.arch.tpidrro_el0
+                ))
+            })?
+        };
         Ok(EnvRegisters {
-            tpidr_el0: 0,
-            tpidrro_el0: 0,
+            tpidr_el0,
+            tpidrro_el0,
         })
     }
 
@@ -458,7 +474,20 @@ impl ProcScheme {
 
     #[cfg(target_arch = "aarch64")]
     fn write_env_regs(&self, info: &Info, regs: EnvRegisters) -> Result<()> {
-        //TODO: aarch64 EnvRegisters
+        use crate::device::cpu::registers::control_regs;
+
+        if info.pid == context::context_id() {
+            unsafe {
+                control_regs::tpidr_el0_write(regs.tpidr_el0 as u64);
+                control_regs::tpidrro_el0_write(regs.tpidrro_el0 as u64);
+            }
+        } else {
+            try_stop_context(info.pid, |context| {
+                context.arch.tpidr_el0 = regs.tpidr_el0;
+                context.arch.tpidrro_el0 = regs.tpidrro_el0;
+                Ok(())
+            })?;
+        }
         Ok(())
     }