2 files changed, 39 insertions, 1 deletions

diff --git a/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c b/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c
index 4132ef9693..07ef9e5b90 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/irq/irq.c
@@ -120,5 +120,9 @@ void bsp_interrupt_handler_default(rtems_vector_number vector)
 rtems_status_code bsp_interrupt_facility_initialize(void)
 {
    raspberrypi_set_exception_handler(ARM_EXCEPTION_IRQ, _ARMV4_Exception_interrupt);
+   BCM2835_REG(BCM2835_IRQ_DISABLE1) = 0xffffffff;
+   BCM2835_REG(BCM2835_IRQ_DISABLE2) = 0xffffffff;
+   BCM2835_REG(BCM2835_IRQ_DISABLE_BASIC) = 0xffffffff;
+   BCM2835_REG(BCM2835_IRQ_FIQ_CTRL) = 0;
    return RTEMS_SUCCESSFUL;
 }
diff --git a/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
index 047c8ad138..d44be03ac3 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/startup/bspstarthooks.c
@@ -26,11 +26,45 @@
 #include <bsp/start.h>
 #include <bsp/raspberrypi.h>
 #include <bsp/mm.h>
+#include <libcpu/arm-cp15.h>
 
 void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
 {
-}
+  uint32_t sctlr_val;
+
+  sctlr_val = arm_cp15_get_control();
+
+  /*
+   * Current U-boot loader seems to start kernel image
+   * with I and D caches on and MMU enabled.
+   * If RTEMS application image finds that cache is on
+   * during startup then disable caches.
+   */
+  if (sctlr_val & (ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
+    if (sctlr_val & (ARM_CP15_CTRL_C | ARM_CP15_CTRL_M)) {
+      /*
+       * If the data cache is on then ensure that it is clean
+       * before switching off to be extra carefull.
+       */
+      arm_cp15_drain_write_buffer();
+      arm_cp15_data_cache_clean_and_invalidate();
+    }
+    arm_cp15_flush_prefetch_buffer();
+    sctlr_val &= ~(ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M | ARM_CP15_CTRL_A);
+    arm_cp15_set_control(sctlr_val);
 
+    arm_cp15_tlb_invalidate();
+    arm_cp15_flush_prefetch_buffer();
+    arm_cp15_data_cache_invalidate();
+    arm_cp15_instruction_cache_invalidate();
+  }
+
+  /* Clear Translation Table Base Control Register */
+  arm_cp15_set_translation_table_base_control_register(0);
+
+  /* Clear Secure or Non-secure Vector Base Address Register */
+  arm_cp15_set_vector_base_address(0);
+}
 
 void BSP_START_TEXT_SECTION bsp_start_hook_1(void)
 {