Fix-up DTB for Armada XP registers' base according to the actual settings

Depending on u-boot's flavor some boards have their SoC registers
base address configured to 0xD0000000 and other to 0xF1000000.
U-boot is passing currently set value via CP15 register.
In order to create proper mapping for SoC registers and allow further
successful initialization it is necessary to replace fdt_immr_pa with
the real value and eventually fix-up device tree blob.

Tested by:	kevlo

1 files changed, 13 insertions, 0 deletions

diff --git a/sys/arm/mv/mv_machdep.c b/sys/arm/mv/mv_machdep.c
index 8dfeb7b..6dca733 100644
--- a/sys/arm/mv/mv_machdep.c
+++ b/sys/arm/mv/mv_machdep.c
@@ -324,6 +324,19 @@ platform_devmap_init(void)
 	i = 0;
 	pmap_devmap_bootstrap_table = &fdt_devmap[0];
 
+#ifdef SOC_MV_ARMADAXP
+	vm_paddr_t cur_immr_pa;
+
+	/*
+	 * Acquire SoC registers' base passed by u-boot and fill devmap
+	 * accordingly. DTB is going to be modified basing on this data
+	 * later.
+	 */
+	__asm __volatile("mrc p15, 4, %0, c15, c0, 0" : "=r" (cur_immr_pa));
+	cur_immr_pa = (cur_immr_pa << 13) & 0xff000000;
+	if (cur_immr_pa != 0)
+		fdt_immr_pa = cur_immr_pa;
+#endif
 	/*
 	 * IMMR range.
 	 */