diff --git a/lib/system/freertos/zynqmp_r5/sys.c b/lib/system/freertos/zynqmp_r5/sys.c
index 358f79e..67405bb 100644
--- a/lib/system/freertos/zynqmp_r5/sys.c
+++ b/lib/system/freertos/zynqmp_r5/sys.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, Xilinx Inc. and Contributors. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,8 +21,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 +60,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;
 }
diff --git a/lib/system/generic/zynqmp_r5/sys.c b/lib/system/generic/zynqmp_r5/sys.c
index 357bde6..12aa1ab 100644
--- a/lib/system/generic/zynqmp_r5/sys.c
+++ b/lib/system/generic/zynqmp_r5/sys.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, Xilinx Inc. and Contributors. All rights reserved.
+ * Copyright (C) 2022, Advanced Micro Devices, Inc.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,8 +21,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 +60,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;
 }