4 files changed, 40 insertions, 1 deletions
diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 40326ef..4828365 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -1,6 +1,8 @@
 #ifndef _ARM_BUILTINS_H
 #define _ARM_BUILTINS_H
 
+#include "stdint.h"
+
 #define __exception_return(_x0) asm volatile ("eret\n")
 #define __set_exception_return(_elr) \
     asm volatile("mrs x7, currentel\n"  \
@@ -41,4 +43,12 @@
                  "farel1: mrs %0, far_el1\n"   \
                  "fardone:\n" : "=r" (_addr))
 
+extern uint64_t read_currentel();
+extern void write_currentel(uint64_t);
+extern uint64_t read_scr_el3();
+extern void write_scr_el3(uint64_t);
+extern uint64_t read_sder32_el3();
+extern void write_sder32_el3(uint64_t);
+extern uint64_t read_cptr_el3();
+extern void write_cptr_el3(uint64_t);
 #endif
diff --git a/aarch64/common/builtins.S b/aarch64/common/builtins.S
index dd0da3b..d924f75 100644
--- a/aarch64/common/builtins.S
+++ b/aarch64/common/builtins.S
@@ -1,5 +1,6 @@
 #define __ASSEMBLY__
 
+.section .text
 .globl __smc
 __smc:
     smc #0
@@ -10,4 +11,31 @@ __svc:
     svc #0
     ret
 
+.macro READ_REG reg
+.globl read_\reg
+read_\reg:
+    mrs x0, \reg
+    ret
+.endm
+
+.macro WRITE_REG reg
+.globl write_\reg
+write_\reg:
+    msr \reg, x0
+    ret
+.endm
+
+READ_REG currentel
+WRITE_REG currentel
+
+READ_REG scr_el3
+WRITE_REG scr_el3
+
+READ_REG sder32_el3
+WRITE_REG sder32_el3
+
+READ_REG cptr_el3
+WRITE_REG cptr_el3
+
+
 #undef __ASSEMBLY__
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index d0579d1..32f0f7a 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -31,6 +31,6 @@ typedef struct {
 } op_dispatch_t;
 
 typedef struct {
-    void *datap;
+    uint64_t data;
 } op_get_data_t;
 #endif
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 7e10e69..8c4c43a 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -12,6 +12,7 @@
 #define SVC_MAP 8
 #define SVC_YIELD 9
 #define SVC_GET_SYSCNTL 10
+#define SVC_GET_MODE 11
 
 #ifndef __ASSEMBLY__
 #include "interop.h"