From c76b34db77f06450ba1c957e87a0b1df3c6c29b4 Mon Sep 17 00:00:00 2001
From: Wojtek Kosior <kwojtus@protonmail.com>
Date: Tue, 3 Dec 2019 16:23:18 +0100
Subject: separate libkernel setus, PL0 code setup and jump to PL0 code into
 different functions

---
 demo_functionality.c | 59 ++++++++++++++++++++++++++++++++--------------------
 1 file changed, 36 insertions(+), 23 deletions(-)

(limited to 'demo_functionality.c')

diff --git a/demo_functionality.c b/demo_functionality.c
index 753a7d3..fbf5eb4 100644
--- a/demo_functionality.c
+++ b/demo_functionality.c
@@ -56,40 +56,56 @@ void demo_mode_to_system(void)
   set_system_mode();
 }
 
+#define TRANSLATION_TABLE						\
+  ((short_section_descriptor_t volatile*) TRANSLATION_TABLE_BASE)
+
 extern char
   __renamed_start,
   __renamed_end,
   __renamed_size;
 
+void demo_setup_libkernel(void) {
+  short_section_descriptor_t volatile *libkernel_section_entry =
+    &TRANSLATION_TABLE[LIBKERNEL_SECTION_NUMBER];
+
+  short_section_descriptor_t
+    libkernel_section = *libkernel_section_entry;
+
+  // make libkernel section executable and read-only for
+  // unprivileged code
+  libkernel_section.ACCESS_PERMISSIONS_2 =
+    AP_2_0_MODEL_RW_PL1_RO_PL0 >> 2;
+  libkernel_section.ACCESS_PERMISSIONS_1_0 =
+    AP_2_0_MODEL_RW_PL1_RO_PL0 & 0b011;
+
+  *libkernel_section_entry = libkernel_section;  
+
+  // invalidate main Translation Lookup Buffer (just in case)
+  asm("mcr p15, 0, %0, c8, c7, 0\n\r"
+      "isb" :: "r" (0) : "memory");
+  
+  // copy libkernel code to libkernel section
+  for (size_t i = 0; i < (size_t) &__renamed_size; i++)
+    ((volatile char*) LIBKERNEL_SECTION_START)[i] =
+      (&__renamed_start)[i];  
+}
+
 extern char
   _binary_PL_0_test_img_start,
   _binary_PL_0_test_img_end,
   _binary_PL_0_test_img_size;
 
-void demo_go_unprivileged(void)
+void demo_setup_PL0(void)
 {
-  short_section_descriptor_t *translation_table =
-    (short_section_descriptor_t*) TRANSLATION_TABLE_BASE;
-
-  short_section_descriptor_t volatile *libkernel_section_entry =
-    &translation_table[LIBKERNEL_SECTION_NUMBER];
   short_section_descriptor_t volatile *PL0_section_entry =
-    &translation_table[PL0_SECTION_NUMBER];
+    &TRANSLATION_TABLE[PL0_SECTION_NUMBER];
   short_section_descriptor_t volatile *UART_memory_section_entry =
-    &translation_table[((uint32_t) GPIO_BASE) >> 20];
+    &TRANSLATION_TABLE[((uint32_t) GPIO_BASE) >> 20];
 
   short_section_descriptor_t
-    libkernel_section = *libkernel_section_entry,
     PL0_section = *PL0_section_entry,
     UART_memory_section = *UART_memory_section_entry;
 
-  // make libkernel section executable and read-only for
-  // unprivileged code
-  libkernel_section.ACCESS_PERMISSIONS_2 =
-    AP_2_0_MODEL_RW_PL1_RO_PL0 >> 2;
-  libkernel_section.ACCESS_PERMISSIONS_1_0 =
-    AP_2_0_MODEL_RW_PL1_RO_PL0 & 0b011;
-  
   // set up address of PL0 section
   PL0_section.SECTION_BASE_ADDRESS_31_20 =
     UNPRIVILEGED_MEMORY_START >> 20;
@@ -105,7 +121,6 @@ void demo_go_unprivileged(void)
   UART_memory_section.ACCESS_PERMISSIONS_1_0 =
     AP_2_0_MODEL_RW_ALL & 0b011;
 
-  *libkernel_section_entry = libkernel_section;
   *PL0_section_entry = PL0_section;
   *UART_memory_section_entry = UART_memory_section;
 
@@ -123,18 +138,16 @@ void demo_go_unprivileged(void)
   
   uart_puts((char*) VIRTUAL_PL0_MEMORY_START);
 
-  // copy libkernel code to libkernel section
-  for (size_t i = 0; i < (size_t) &__renamed_size; i++)
-    ((volatile char*) LIBKERNEL_SECTION_START)[i] =
-      (&__renamed_start)[i];  
-  
   // now paste a userspace program to that section
   for (size_t i = 0; i < (size_t) &_binary_PL_0_test_img_size; i++)
     ((volatile char*) VIRTUAL_PL0_MEMORY_START)[i] =
       (&_binary_PL_0_test_img_start)[i];
 
   uart_puts("copied PL0 and libkernel code to their sections");
-  
+}
+
+void demo_go_unprivileged(void)
+{
   size_t call_unprivileged_offset =
     (size_t) &call_unprivileged - (size_t) &__renamed_start;
 
-- 
cgit v1.2.3