From 4f00303daf5181d4efb94ee6c312ea49a421b0eb Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Tue, 1 Dec 2020 15:57:45 -0800
Subject: [PATCH] kern: set EL2 id registers on deprivilege

---
 .../kernel/source/arch/arm64/init/start.s     | 30 +++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/mesosphere/kernel/source/arch/arm64/init/start.s b/mesosphere/kernel/source/arch/arm64/init/start.s
index b5303d2f3..e6c8288af 100644
--- a/mesosphere/kernel/source/arch/arm64/init/start.s
+++ b/mesosphere/kernel/source/arch/arm64/init/start.s
@@ -249,6 +249,27 @@ _ZN3ams4kern4init16JumpFromEL2ToEL1Ev:
     bl _ZN3ams4kern4arch5arm643cpu32FlushEntireDataCacheWithoutStackEv
 
     /* Setup system registers for deprivileging. */
+
+    /* Check if we're on cortex A57 or A53. If we are, set ACTLR_EL2. */
+    mrs  x1, midr_el1
+
+    /* Is the manufacturer ID 'A' (ARM)? */
+    ubfx x2, x1, #0x18, #8
+    cmp x2, #0x41
+    b.ne 2f
+
+    /* Is the board ID Cortex-A57? */
+    ubfx x2, x1, #4, #0xC
+    mov x3, #0xD07
+    cmp x2, x3
+    b.eq 1f
+
+    /* Is the board ID Cortex-A53? */
+    mov x3, #0xD03
+    cmp x2, x3
+    b.ne 2f
+
+1:
     /* ACTLR_EL2: */
     /*  - CPUACTLR access control = 1 */
     /*  - CPUECTLR access control = 1 */
@@ -258,6 +279,7 @@ _ZN3ams4kern4init16JumpFromEL2ToEL1Ev:
     mov x0, #0x73
     msr actlr_el2, x0
 
+2:
     /* HCR_EL2: */
     /*  - RW = 1 (el1 is aarch64) */
     mov x0, #0x80000000
@@ -275,6 +297,14 @@ _ZN3ams4kern4init16JumpFromEL2ToEL1Ev:
     mov x0, #0xFFFFFFFF
     msr dacr32_el2, x0
 
+    /* Set VPIDR_EL2 = MIDR_EL1 */
+    mrs x0, midr_el1
+    msr vpidr_el2, x0
+
+    /* SET VMPIDR_EL2 = MPIDR_EL1 */
+    mrs x0, mpidr_el1
+    msr vmpidr_el2, x0
+
     /* SPSR_EL2: */
     /*  - EL1h */
     /*  - IRQ masked */