zynqmp_r5: replace Xil_SetMPURegion with Xil_MemMap

The metal_machine_io_mem_map() used to call Xil_SetMPURegion directly
and could get into an infinite loop. Replace the faulty code in
metal_machine_io_mem_map() with Xil_MemMap() call and assert it succeeds.

Signed-off-by: Sergei Korneichuk <sergei.korneichuk@amd.com>

lib/system/freertos/zynqmp_r5/sys.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, Xilinx Inc. and Contributors. All rights reserved.
+ * Copyright (c) 2016 - 2022, Xilinx Inc. and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,8 +20,6 @@
 #include "xreg_cortexr5.h"
 #include "xscugic.h"
 
-#define MPU_REGION_SIZE_MIN 0x20
-
 void sys_irq_restore_enable(unsigned int flags)
 {
 	Xil_ExceptionEnableMask(~flags);
@@ -61,27 +59,16 @@ void metal_weak metal_generic_default_poll(void)
 	metal_asm volatile("wfi");
 }
 
+/**
+ * The code moved to cortexr5/xil_mpu.c:Xil_MemMap()
+ * NULL in pa masks possible Xil_MemMap() errors.
+ */
 void *metal_machine_io_mem_map(void *va, metal_phys_addr_t pa,
 			       size_t size, unsigned int flags)
 {
-	size_t rsize = MPU_REGION_SIZE_MIN;
-	metal_phys_addr_t base_pa;
+	void *__attribute__((unused)) physaddr;
 
-	if (!flags)
-		return va;
-	while (1) {
-		if (rsize < size) {
-			rsize <<= 1;
-			continue;
-		} else {
-			base_pa = pa & ~(rsize - 1);
-			if ((base_pa + rsize) < (pa + size)) {
-				rsize <<= 1;
-				continue;
-			}
-			break;
-		}
-	}
-	Xil_SetMPURegion(base_pa, rsize, flags);
+	physaddr = Xil_MemMap(pa, size, flags);
+	metal_assert(physaddr == (void *)pa);
 	return va;
 }

lib/system/generic/zynqmp_r5/sys.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, Xilinx Inc. and Contributors. All rights reserved.
+ * Copyright (c) 2016 - 2022, Xilinx Inc. and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,8 +20,6 @@
 #include "xreg_cortexr5.h"
 #include "xscugic.h"
 
-#define MPU_REGION_SIZE_MIN 0x20
-
 void sys_irq_restore_enable(unsigned int flags)
 {
 	Xil_ExceptionEnableMask(~flags);
@@ -61,27 +59,16 @@ void metal_weak metal_generic_default_poll(void)
 	metal_asm volatile("wfi");
 }
 
+/**
+ * The code moved to cortexr5/xil_mpu.c:Xil_MemMap()
+ * NULL in pa masks possible Xil_MemMap() errors.
+ */
 void *metal_machine_io_mem_map(void *va, metal_phys_addr_t pa,
 			       size_t size, unsigned int flags)
 {
-	size_t rsize = MPU_REGION_SIZE_MIN;
-	metal_phys_addr_t base_pa;
+	void *__attribute__((unused)) physaddr;
 
-	if (!flags)
-		return va;
-	while (1) {
-		if (rsize < size) {
-			rsize <<= 1;
-			continue;
-		} else {
-			base_pa = pa & ~(rsize - 1);
-			if ((base_pa + rsize) < (pa + size)) {
-				rsize <<= 1;
-				continue;
-			}
-			break;
-		}
-	}
-	Xil_SetMPURegion(base_pa, rsize, flags);
+	physaddr = Xil_MemMap(pa, size, flags);
+	metal_assert(physaddr == (void *)pa);
 	return va;
 }