bsps/microblaze: Allow copying FDT from U-Boot

4 files changed, 147 insertions, 2 deletions

diff --git a/bsps/microblaze/microblaze_fpga/include/bsp.h b/bsps/microblaze/microblaze_fpga/include/bsp.h
index 410d4483c6..6890f33395 100644
--- a/bsps/microblaze/microblaze_fpga/include/bsp.h
+++ b/bsps/microblaze/microblaze_fpga/include/bsp.h
@@ -49,14 +49,17 @@ extern "C" {
 
 #ifdef BSP_MICROBLAZE_FPGA_USE_FDT
 #define BSP_FDT_IS_SUPPORTED
+#ifndef BSP_START_COPY_FDT_FROM_U_BOOT
 extern const unsigned char system_dtb[];
 extern const size_t system_dtb_size;
+#endif /* BSP_START_COPY_FDT_FROM_U_BOOT */
 #endif /* BSP_MICROBLAZE_FPGA_USE_FDT */
 
 void microblaze_enable_icache(void);
 void microblaze_enable_dcache(void);
 void microblaze_invalidate_icache(void);
 void microblaze_invalidate_dcache(void);
+void microblaze_invalidate_dcache_range(unsigned int cacheaddr, unsigned int len);
 
 #ifdef __cplusplus
 }
diff --git a/bsps/microblaze/microblaze_fpga/start/crtinit.S b/bsps/microblaze/microblaze_fpga/start/crtinit.S
index d56bee3b19..6c7fc3af23 100644
--- a/bsps/microblaze/microblaze_fpga/start/crtinit.S
+++ b/bsps/microblaze/microblaze_fpga/start/crtinit.S
@@ -30,6 +30,8 @@
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <bspopts.h>
+
 	.globl _crtinit
 	.align 2
 	.ent _crtinit
@@ -75,7 +77,9 @@ _crtinit:
 	brlid   r15, __init					 /* Invoke language initialization functions */
 	nop
 #endif /* __rtems__ */
-
+#ifdef BSP_START_COPY_FDT_FROM_U_BOOT      /* Boot loaders may pass the device tree in r5 */
+	brlid	r15, bsp_fdt_copy              /* Do not touch r5 until bsp_fdt_copy() is called */
+#endif /* BSP_START_COPY_FDT_FROM_U_BOOT */
 	addi	r6, r0, 0					   /* Initialize argc = 1 and argv = NULL and envp = NULL  */
 	addi	r7, r0, 0
 #ifndef __rtems__
diff --git a/bsps/microblaze/microblaze_fpga/start/microblaze_invalidate_dcache_range.S b/bsps/microblaze/microblaze_fpga/start/microblaze_invalidate_dcache_range.S
new file mode 100644
index 0000000000..89d5fff16b
--- /dev/null
+++ b/bsps/microblaze/microblaze_fpga/start/microblaze_invalidate_dcache_range.S
@@ -0,0 +1,104 @@
+/******************************************************************************
+* Copyright (c) 2008 - 2020 Xilinx, Inc.  All rights reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+/******************************************************************************
+*
+*
+* microblaze_invalidate_dcache_range (unsigned int cacheaddr, unsigned int len)
+*
+*    Invalidate a Dcache range
+*
+*    Parameters:
+*       'cacheaddr' - address in the Dcache where invalidation begins
+*	'len    '   - length (in bytes) worth of Dcache to be invalidated
+*
+*
+*******************************************************************************/
+
+#include <bspopts.h>
+
+#define MICROBLAZE_MSR_DCACHE_ENABLE        0x00000080
+#define MICROBLAZE_MSR_INTR_ENABLE          0x00000002
+
+#ifndef XPAR_MICROBLAZE_USE_DCACHE
+#define XPAR_MICROBLAZE_USE_DCACHE     1
+#endif
+
+#ifndef XPAR_MICROBLAZE_ALLOW_DCACHE_WR
+#define XPAR_MICROBLAZE_ALLOW_DCACHE_WR     1
+#endif
+
+#ifndef XPAR_MICROBLAZE_DCACHE_USE_WRITEBACK
+#define MB_VERSION_LT_v720
+#define MB_HAS_WRITEBACK_SET 0
+#else
+#define MB_HAS_WRITEBACK_SET XPAR_MICROBLAZE_DCACHE_USE_WRITEBACK
+#endif
+
+	.text
+	.globl	microblaze_invalidate_dcache_range
+	.ent	microblaze_invalidate_dcache_range
+	.align	2
+
+microblaze_invalidate_dcache_range:
+#if (XPAR_MICROBLAZE_USE_DCACHE==1) && (XPAR_MICROBLAZE_ALLOW_DCACHE_WR==1)
+
+#ifdef MB_VERSION_LT_v720                                       /* Disable Dcache and interrupts before invalidating */
+	mfs	r9, rmsr
+	andi	r10, r9, ~(MICROBLAZE_MSR_DCACHE_ENABLE | MICROBLAZE_MSR_INTR_ENABLE)
+	mts	rmsr, r10
+#endif
+
+        BEQI    r6, L_done                                      /* Skip loop if size is zero */
+
+        ADD	r6, r5, r6                                      /* Compute end address */
+        ADDIK   r6, r6, -1
+
+        ANDI    r6, r6, -(4 * BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN)  /* Align end down to cache line */
+        ANDI    r5, r5, -(4 * BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN)  /* Align start down to cache line */
+
+#if MB_HAS_WRITEBACK_SET == 0                                   /* Use a different scheme for MB version < v7.20 or when caches are write-through */
+
+L_start:
+	CMPU	r18, r5, r6                                     /* Are we at the end? */
+	BLTI	r18, L_done
+
+	wdc     r5, r0
+
+#if defined (__arch64__ )
+	addlik	r5, r5, (BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN * 4)   /* Increment the address by 4 */
+	breai	L_start                                         /* Branch to the beginning of the loop */
+#else
+	brid	L_start                                         /* Branch to the beginning of the loop */
+	addik	r5, r5, (BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN * 4)   /* Increment the address by 4 (delay slot) */
+#endif
+#else
+
+        RSUBK   r6, r5, r6
+                                                                /* r6 will now contain (count of bytes - (4 * BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN)) */
+L_start:
+        wdc.clear r5, r6                                        /* Invalidate the cache line only if the address matches */
+#if defined (__arch64__ )
+        addlik   r6, r6, -(BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN * 4)
+        beagei r6, L_start
+#else
+        bneid   r6, L_start
+        addik   r6, r6, -(BSP_MICROBLAZE_FPGA_DCACHE_LINE_LEN * 4)
+#endif
+
+#endif
+
+L_done:
+	rtsd	r15, 8
+#ifdef MB_VERSION_LT_v720                                       /* restore MSR only for MB version < v7.20 */
+	mts	rmsr, r9
+#else
+        nop
+#endif
+
+#else
+	rtsd	r15, 8
+	nop
+#endif
+	.end	microblaze_invalidate_dcache_range
diff --git a/bsps/microblaze/shared/fdt/microblaze-fdt-support.c b/bsps/microblaze/shared/fdt/microblaze-fdt-support.c
index d48b051ffd..b1d8d186c4 100644
--- a/bsps/microblaze/shared/fdt/microblaze-fdt-support.c
+++ b/bsps/microblaze/shared/fdt/microblaze-fdt-support.c
@@ -27,12 +27,46 @@
 
 #include <bspopts.h>
 #include <bsp/microblaze-fdt-support.h>
-#ifdef BSP_MICROBLAZE_FPGA_USE_FDT
 #include <bsp/fdt.h>
 
 #include <libfdt.h>
 
+#ifdef BSP_START_COPY_FDT_FROM_U_BOOT
+/* use external dtb provided by u-boot */
+#include <sys/param.h>
+
+#ifndef BSP_FDT_BLOB_SIZE_MAX
+#define BSP_FDT_BLOB_SIZE_MAX 0
+#endif
+
+static RTEMS_ALIGNED(8) uint32_t
+system_dtb[BSP_FDT_BLOB_SIZE_MAX / sizeof(uint32_t)];
+
+void bsp_fdt_copy(const void *src)
+{
+  const volatile uint32_t *s = (const uint32_t *) src;
+  uint32_t *d = RTEMS_DECONST(uint32_t *, &system_dtb[0]);
+
+  if (s != d) {
+    size_t m = MIN(sizeof(system_dtb), fdt_totalsize(src));
+    size_t aligned_size = roundup2(m, CPU_CACHE_LINE_BYTES);
+    size_t n = (m + sizeof(*d) - 1) / sizeof(*d);
+    size_t i;
+
+    for (i = 0; i < n; ++i) {
+      d[i] = s[i];
+    }
+
+    rtems_cache_flush_multiple_data_lines(d, aligned_size);
+  }
+}
+#endif /* BSP_START_COPY_FDT_FROM_U_BOOT */
+
+#ifdef BSP_MICROBLAZE_FPGA_USE_FDT
+#ifndef BSP_START_COPY_FDT_FROM_U_BOOT
+/* use internal bsp dtb */
 #include BSP_MICROBLAZE_FPGA_DTB_HEADER_PATH
+#endif /* BSP_START_COPY_FDT_FROM_U_BOOT */
 
 const void *bsp_fdt_get(void)
 {