bsps/aarch64: Add MMU driver to relax alignment

Currently, the AArch64 BSPs have a hard time running on real hardware
without building the toolchain and the bsps with -mstrict-align in
multiple places. Configuring the MMU on these chips allows for unaligned
memory accesses for non-device memory which avoids requiring strict
alignment in the toolchain and in the BSPs themselves.

In writing this driver, it was found that the synchronous exception
handling code needed to be rewritten since it relied on clearing SCTLR_EL1 to
avoid thread stack misalignments in RTEMS_DEBUG mode. This is now
avoided by exactly preserving thread mode stack and flags and the new
implementation is compatible with the draft information provided on the
mailing list covering the Exception Management API.

3 files changed, 85 insertions, 0 deletions

diff --git a/bsps/aarch64/xilinx-zynqmp/include/bsp.h b/bsps/aarch64/xilinx-zynqmp/include/bsp.h
index e405cc2ed7..83f2e2f4e4 100644
--- a/bsps/aarch64/xilinx-zynqmp/include/bsp.h
+++ b/bsps/aarch64/xilinx-zynqmp/include/bsp.h
@@ -61,6 +61,13 @@ extern "C" {
 
 #define BSP_RESET_SMC
 
+/**
+ * @brief Zynq UltraScale+ MPSoC specific set up of the MMU.
+ *
+ * Provide in the application to override the defaults in the BSP.
+ */
+BSP_START_TEXT_SECTION void zynqmp_setup_mmu_and_cache(void);
+
 void zynqmp_debug_console_flush(void);
 
 #ifdef __cplusplus
diff --git a/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c b/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
index b165b42909..7bd787592c 100644
--- a/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
+++ b/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
@@ -47,5 +47,6 @@ BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
 {
   AArch64_start_set_vector_base();
   bsp_start_copy_sections();
+  zynqmp_setup_mmu_and_cache();
   bsp_start_clear_bss();
 }
diff --git a/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c b/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
new file mode 100644
index 0000000000..8d302e97b5
--- /dev/null
+++ b/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
@@ -0,0 +1,77 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ * @file
+ *
+ * @ingroup RTEMSBSPsAArch64XilinxZynqMP
+ *
+ * @brief This source file contains the default MMU tables and setup.
+ */
+
+/*
+ * Copyright (C) 2021 On-Line Applications Research Corporation (OAR)
+ * Written by Kinsey Moore <kinsey.moore@oarcorp.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/aarch64-mmu.h>
+
+BSP_START_DATA_SECTION static const aarch64_mmu_config_entry
+zynqmp_mmu_config_table[] = {
+  AARCH64_MMU_DEFAULT_SECTIONS,
+#if defined( RTEMS_SMP )
+  {
+    .begin = 0xffff0000U,
+    .end = 0xffffffffU,
+    .flags = AARCH64_MMU_DEVICE
+  },
+#endif
+  {
+    .begin = 0xf9000000U,
+    .end = 0xf9100000U,
+    .flags = AARCH64_MMU_DEVICE
+  }, {
+    .begin = 0xfd000000U,
+    .end = 0xffc00000U,
+    .flags = AARCH64_MMU_DEVICE
+  }
+};
+
+/*
+ * Make weak and let the user override.
+ */
+BSP_START_TEXT_SECTION void
+zynqmp_setup_mmu_and_cache( void ) __attribute__ ((weak));
+
+BSP_START_TEXT_SECTION void
+zynqmp_setup_mmu_and_cache( void )
+{
+  aarch64_mmu_setup();
+
+  aarch64_mmu_setup_translation_table_and_enable(
+    &zynqmp_mmu_config_table[ 0 ],
+    RTEMS_ARRAY_SIZE( zynqmp_mmu_config_table )
+  );
+}