1 files changed, 47 insertions, 10 deletions
diff --git a/c/src/lib/libbsp/powerpc/t32mppc/startup/bspstart.c b/c/src/lib/libbsp/powerpc/t32mppc/startup/bspstart.c
index 73d1d8cf56..f729ae60d1 100644
--- a/c/src/lib/libbsp/powerpc/t32mppc/startup/bspstart.c
+++ b/c/src/lib/libbsp/powerpc/t32mppc/startup/bspstart.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2012, 2017 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -31,7 +31,7 @@ LINKER_SYMBOL(bsp_exc_vector_base);
  * equal.  For now we simulate processor that issues 10000000 instructions per
  * second.
  */
-uint32_t bsp_time_base_frequency = 10000000 / 10;
+uint32_t bsp_time_base_frequency = 10000000;
 
 void BSP_panic(char *s)
 {
@@ -61,6 +61,50 @@ void _BSP_Fatal_error(unsigned n)
   }
 }
 
+#define MTIVPR(base) \
+  __asm__ volatile ("mtivpr %0" : : "r" (base))
+
+#define VECTOR_TABLE_ENTRY_SIZE 16
+
+#define MTIVOR(vec, offset) \
+  do { \
+    __asm__ volatile ("mtspr " RTEMS_XSTRING(vec) ", %0" : : "r" (offset)); \
+    offset += VECTOR_TABLE_ENTRY_SIZE; \
+  } while (0)
+
+static void t32mppc_initialize_exceptions(void *interrupt_stack_begin)
+{
+  uintptr_t addr;
+
+  ppc_exc_initialize_interrupt_stack(
+    (uintptr_t) interrupt_stack_begin,
+    rtems_configuration_get_interrupt_stack_size()
+  );
+
+  addr = (uintptr_t) bsp_exc_vector_base;
+  MTIVPR(addr);
+  MTIVOR(BOOKE_IVOR0,  addr);
+  MTIVOR(BOOKE_IVOR1,  addr);
+  MTIVOR(BOOKE_IVOR2,  addr);
+  MTIVOR(BOOKE_IVOR3,  addr);
+  MTIVOR(BOOKE_IVOR4,  addr);
+  MTIVOR(BOOKE_IVOR5,  addr);
+  MTIVOR(BOOKE_IVOR6,  addr);
+  MTIVOR(BOOKE_IVOR7,  addr);
+  MTIVOR(BOOKE_IVOR8,  addr);
+  MTIVOR(BOOKE_IVOR9,  addr);
+  MTIVOR(BOOKE_IVOR10, addr);
+  MTIVOR(BOOKE_IVOR11, addr);
+  MTIVOR(BOOKE_IVOR12, addr);
+  MTIVOR(BOOKE_IVOR13, addr);
+  MTIVOR(BOOKE_IVOR14, addr);
+  MTIVOR(BOOKE_IVOR15, addr);
+  MTIVOR(BOOKE_IVOR32, addr);
+  MTIVOR(BOOKE_IVOR33, addr);
+  MTIVOR(BOOKE_IVOR34, addr);
+  MTIVOR(BOOKE_IVOR35, addr);
+}
+
 void bsp_start(void)
 {
   get_ppc_cpu_type();
@@ -68,13 +112,6 @@ void bsp_start(void)
 
   rtems_counter_initialize_converter(bsp_time_base_frequency);
 
-  /* Initialize exception handler */
-  ppc_exc_initialize_with_vector_base(
-    (uintptr_t) bsp_section_work_begin,
-    rtems_configuration_get_interrupt_stack_size(),
-    bsp_exc_vector_base
-  );
-
-  /* Initalize interrupt support */
+  t32mppc_initialize_exceptions(bsp_section_work_begin);
   bsp_interrupt_initialize();
 }