1 files changed, 24 insertions, 0 deletions
diff --git a/arch/arm/mach-zynqmp/mp.c b/arch/arm/mach-zynqmp/mp.c
index 9b46a25a1cb..6e6da8008f4 100644
--- a/arch/arm/mach-zynqmp/mp.c
+++ b/arch/arm/mach-zynqmp/mp.c
@@ -12,7 +12,9 @@
 #include <asm/arch/hardware.h>
 #include <asm/arch/sys_proto.h>
 #include <asm/io.h>
+#include <linux/bitfield.h>
 #include <linux/delay.h>
+#include <linux/errno.h>
 #include <linux/string.h>
 
 #define LOCK		0
@@ -264,6 +266,28 @@ void initialize_tcm(bool mode)
 	}
 }
 
+int check_tcm_mode(bool mode)
+{
+	u32 tmp, cpu_state;
+	bool mode_prev;
+
+	tmp = readl(&rpu_base->rpu_glbl_ctrl);
+	mode_prev = FIELD_GET(ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK, tmp);
+
+	tmp = readl(&crlapb_base->rst_lpd_top);
+	cpu_state = FIELD_GET(ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK |
+			      ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK, tmp);
+	cpu_state = cpu_state ? false : true;
+
+	if ((mode_prev == SPLIT && mode == LOCK) && cpu_state)
+		return -EACCES;
+
+	if (mode_prev == mode)
+		return -EAGAIN;
+
+	return 0;
+}
+
 static void mark_r5_used(u32 nr, u8 mode)
 {
 	u32 mask = 0;