3 files changed, 85 insertions, 25 deletions

diff --git a/Makefile b/Makefile
index ec5a0e2..f2b734c 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-CFLAGS=-mcpu=cortex-a7 -ffreestanding -std=gnu99 -Wall -Wextra -I.
+CFLAGS=-mcpu=cortex-a7 -ffreestanding -std=gnu11 -Wall -Wextra -I.
 
 ELFFLAGS=-ffreestanding -O2 -nostdlib -lgcc -I.
 
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");  
+}
diff --git a/kernel.c b/kernel.c
index c5918a1..a8df084 100644
--- a/kernel.c
+++ b/kernel.c
@@ -1,4 +1,5 @@
-#include <uart.h>
+#include "uart.h"
+#include "cpsr.h"
 
 void kernel_main(uint32_t r0, uint32_t r1, uint32_t atags)
 {
@@ -22,15 +23,16 @@ void kernel_main(uint32_t r0, uint32_t r1, uint32_t atags)
 
   char *paging;
 
-  switch(ID_MMFR0 & 0xf) /* lowest 4 bits indicate VMSA support */ {
-  case 0 : paging = "no paging\n\r"; break;
-  case 1 : paging = "implementation defined paging\n\r"; break;
-  case 2 : paging = "VMSAv6, with cache and TLB type registers\n\r"; break;
-  case 3 : paging = "VMSAv7, with support for remapping and access flag\n\r"; break;
-  case 4 : paging = "VMSAv7 with PXN bit supported\n\r"; break;
-  case 5 : paging = "VMSAv7, PXN and long format descriptors. EPAE is supported.\n\r"; break;
-  default : paging = "?_? unknown paging ?_?\n\r";
-  }
+  switch(ID_MMFR0 & 0xf) /* lowest 4 bits indicate VMSA support */
+    {
+    case 0 : paging = "no paging\n\r"; break;
+    case 1 : paging = "implementation defined paging\n\r"; break;
+    case 2 : paging = "VMSAv6, with cache and TLB type registers\n\r"; break;
+    case 3 : paging = "VMSAv7, with support for remapping and access flag\n\r"; break;
+    case 4 : paging = "VMSAv7 with PXN bit supported\n\r"; break;
+    case 5 : paging = "VMSAv7, PXN and long format descriptors. EPAE is supported.\n\r"; break;
+    default : paging = "?_? unknown paging ?_?\n\r";
+    }
 
   uart_puts(paging);
 
@@ -38,24 +40,43 @@ void kernel_main(uint32_t r0, uint32_t r1, uint32_t atags)
   // get content of current program status register to check the current
   // processor mode
   asm("mrs %0, cpsr" : "=r" (CPSR) :: "memory");
-
+  
   char *mode;
   
-  switch(CPSR & 0x1f) /* lowest 5 bits indicate processor mode */ {
-  case 0x10 : mode = "User (PL0)"; break;
-  case 0x11 : mode = "FIQ (PL1)"; break;
-  case 0x12 : mode = "IRQ (PL1)"; break;
-  case 0x13 : mode = "Supervisor (PL1)"; break;
-  case 0x16 : mode = "Monitor (PL1)"; break;
-  case 0x17 : mode = "Abort (PL1)"; break;
-  case 0x1a : mode = "Hyp (PL2)"; break;
-  case 0x1b : mode = "Undefined (PL1)"; break;
-  case 0x1f : mode = "System (PL1)"; break;
-  default : mode = "Unknown mode"; break;
-  }
+  switch(read_processor_mode())
+    {
+    case 0x10 : mode = "User (PL0)\r\n"; break;
+    case 0x11 : mode = "FIQ (PL1)\r\n"; break;
+    case 0x12 : mode = "IRQ (PL1)\r\n"; break;
+    case 0x13 : mode = "Supervisor (PL1)\r\n"; break;
+    case 0x16 : mode = "Monitor (PL1)\r\n"; break;
+    case 0x17 : mode = "Abort (PL1)\r\n"; break;
+    case 0x1a : mode = "Hyp (PL2)\r\n"; break;
+    case 0x1b : mode = "Undefined (PL1)\r\n"; break;
+    case 0x1f : mode = "System (PL1)\r\n"; break;
+    default : mode = "Unknown mode\r\n"; break;
+    }
 
   uart_puts(mode);
-  
+
+  set_system_mode();
+
+  switch(read_processor_mode())
+    {
+    case 0x10 : mode = "User (PL0)\r\n"; break;
+    case 0x11 : mode = "FIQ (PL1)\r\n"; break;
+    case 0x12 : mode = "IRQ (PL1)\r\n"; break;
+    case 0x13 : mode = "Supervisor (PL1)\r\n"; break;
+    case 0x16 : mode = "Monitor (PL1)\r\n"; break;
+    case 0x17 : mode = "Abort (PL1)\r\n"; break;
+    case 0x1a : mode = "Hyp (PL2)\r\n"; break;
+    case 0x1b : mode = "Undefined (PL1)\r\n"; break;
+    case 0x1f : mode = "System (PL1)\r\n"; break;
+    default : mode = "Unknown mode\r\n"; break;
+    }
+
+  uart_puts(mode);
+
   while (1)
     uart_putc(uart_getc());
 }