re-enter system mode on invalid data access from PL0 code

1 files changed, 22 insertions, 0 deletions

diff --git a/demo_functionality.c b/demo_functionality.c
index fbf5eb4..60b0bfa 100644
--- a/demo_functionality.c
+++ b/demo_functionality.c
@@ -165,3 +165,25 @@ void demo_go_unprivileged(void)
 	       "r" (VIRTUAL_PL0_MEMORY_START)
 	       : "memory", "r4", "r5", "r0");
 }
+
+extern char
+  __interrupt_vectors_start,
+  __interrupt_vectors_end,
+  __interrupt_vectors_size;
+
+extern void (*volatile system_reentry_point)(void);
+
+void system_reentry(void)
+{
+  uart_puts("re-entered system");
+  while(1);
+}
+
+void demo_setup_interrupts(void)
+{
+  system_reentry_point = system_reentry;
+
+  for (size_t i = 0; i < (size_t) &__interrupt_vectors_size; i++)
+    ((volatile char*) 0)[i] =
+      (&__interrupt_vectors_start)[i];
+}