1 files changed, 69 insertions, 2 deletions

diff --git a/bsps/arm/raspberrypi/start/bspstarthooks.c b/bsps/arm/raspberrypi/start/bspstarthooks.c
index 3f8c680bc0..c2a9707d71 100644
--- a/bsps/arm/raspberrypi/start/bspstarthooks.c
+++ b/bsps/arm/raspberrypi/start/bspstarthooks.c
@@ -27,6 +27,7 @@
 #include <bsp/raspberrypi.h>
 #include <libcpu/arm-cp15.h>
 #include <bsp.h>
+#include <bsp/bootcard.h>
 #include <bsp/linker-symbols.h>
 #include <bsp/arm-cp15-start.h>
 
@@ -34,6 +35,34 @@
 #include <rtems/score/smp.h>
 #endif
 
+#if defined(HAS_UBOOT) && !defined(BSP_DISABLE_UBOOT_WORK_AREA_CONFIG)
+  #define USE_UBOOT
+#endif
+
+#ifdef USE_UBOOT
+  #include <bsp/u-boot.h>
+#else
+  #include <bsp/vc.h>
+#endif
+
+BSP_START_DATA_SECTION static arm_cp15_start_section_config
+raspberrypi_mmu_config_table[] = {
+  ARMV7_CP15_START_DEFAULT_SECTIONS,
+  {
+    .begin = RPI_PERIPHERAL_BASE,
+    .end =   RPI_PERIPHERAL_BASE + RPI_PERIPHERAL_SIZE,
+    .flags = ARMV7_MMU_DEVICE
+  }
+#if (BSP_IS_RPI2 == 1)
+  /* Core local peripherals area - timer, mailboxes */
+  , {
+    .begin = BCM2836_CORE_LOCAL_PERIPH_BASE,
+    .end =   BCM2836_CORE_LOCAL_PERIPH_BASE + BCM2836_CORE_LOCAL_PERIPH_SIZE,
+    .flags = ARMV7_MMU_DEVICE
+  }
+#endif
+};
+
 void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
 {
   uint32_t sctlr_val;
@@ -98,18 +127,43 @@ void BSP_START_TEXT_SECTION bsp_start_hook_0(void)
 #endif
 }
 
+BSP_START_TEXT_SECTION static uintptr_t raspberrypi_get_ram_end(void)
+{
+  uintptr_t ram_end;
+
+#ifdef USE_UBOOT
+  ram_end = (uintptr_t) bsp_uboot_board_info.bi_memstart +
+                        bsp_uboot_board_info.bi_memsize;
+#else
+  bcm2835_get_arm_memory_entries spec;
+
+  if (bcm2835_mailbox_get_arm_memory( &spec ) >= 0) {
+    ram_end = spec.base + spec.size;
+  } else {
+    /* Use the workspace end from the linker command file for fallback. */
+    ram_end = (uintptr_t) bsp_section_work_end;
+  }
+#endif
+
+  return ram_end;
+}
+
 BSP_START_TEXT_SECTION static void bsp_memory_management_initialize(void)
 {
+  uintptr_t ram_end = raspberrypi_get_ram_end();
   uint32_t ctrl = arm_cp15_get_control();
 
   ctrl |= ARM_CP15_CTRL_AFE | ARM_CP15_CTRL_S | ARM_CP15_CTRL_XP;
 
+  raspberrypi_mmu_config_table[ARMV7_CP15_START_WORKSPACE_ENTRY_INDEX].end =
+    ram_end;
+
   arm_cp15_start_setup_translation_table_and_enable_mmu_and_cache(
     ctrl,
     (uint32_t *) bsp_translation_table_base,
     ARM_MMU_DEFAULT_CLIENT_DOMAIN,
-    &arm_cp15_start_mmu_config_table[0],
-    arm_cp15_start_mmu_config_table_size
+    &raspberrypi_mmu_config_table[0],
+    RTEMS_ARRAY_SIZE(raspberrypi_mmu_config_table)
   );
 }
 
@@ -121,3 +175,16 @@ void BSP_START_TEXT_SECTION bsp_start_hook_1(void)
 
   rpi_video_init();
 }
+
+void bsp_work_area_initialize(void)
+{
+  uintptr_t begin;
+  uintptr_t end;
+
+  begin = raspberrypi_mmu_config_table[ARMV7_CP15_START_WORKSPACE_ENTRY_INDEX]
+          .begin;
+  end = raspberrypi_mmu_config_table[ARMV7_CP15_START_WORKSPACE_ENTRY_INDEX]
+        .end;
+
+  bsp_work_area_initialize_default((void *) begin, end - begin);
+}