add processor mode changing

1 files changed, 39 insertions, 0 deletions

diff --git a/cpsr.h b/cpsr.h
new file mode 100644
index 0000000..327bdf0
--- /dev/null
+++ b/cpsr.h
@@ -0,0 +1,39 @@
+#include <stdint.h>
+
+enum execution_mode {
+  MODE_USER = 0x10,
+  MODE_FIQ = 0x11,
+  MODE_IRQ = 0x12,
+  MODE_SUPERVISOR = 0x13,
+  MODE_MONITOR = 0x16,
+  MODE_ABORT = 0x17,
+  MODE_HYPERVISOR = 0x1a,
+  MODE_UNDEFINED = 0x1b,
+  MODE_SYSTEM = 0x1f,
+};
+
+inline static uint32_t read_CPSR(void)
+{
+  uint32_t CPSR;
+  // get content of current program status register
+  asm("mrs %0, cpsr" : "=r" (CPSR) ::);
+  
+  return CPSR;
+}
+
+inline static enum execution_mode read_processor_mode(void)
+{
+  /* lowest 5 bits indicate processor mode */
+  return read_CPSR() & 0x1f;
+}
+
+inline static void set_system_mode(void)
+{
+  uint32_t CPSR = read_CPSR();
+
+  CPSR = (CPSR >> 5) << 5;
+
+  CPSR |= MODE_SYSTEM;
+  
+  asm("msr cpsr, %0" :: "r" (CPSR) : "memory");  
+}