diff --git a/arch/arm/mach-zynqmp/mp.c b/arch/arm/mach-zynqmp/mp.c
index 656678a155..74783ae5a7 100644
--- a/arch/arm/mach-zynqmp/mp.c
+++ b/arch/arm/mach-zynqmp/mp.c
@@ -37,6 +37,8 @@
 
 #define ZYNQMP_CORE_APU0	0
 #define ZYNQMP_CORE_APU3	3
+#define ZYNQMP_CORE_RPU0	4
+#define ZYNQMP_CORE_RPU1	5
 
 #define ZYNQMP_MAX_CORES	6
 
@@ -54,18 +56,20 @@ int cpu_reset(u32 nr)
 	return 0;
 }
 
-static void set_r5_halt_mode(u8 halt, u8 mode)
+static void set_r5_halt_mode(u32 nr, u8 halt, u8 mode)
 {
 	u32 tmp;
 
-	tmp = readl(&rpu_base->rpu0_cfg);
-	if (halt == HALT)
-		tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
-	else
-		tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
-	writel(tmp, &rpu_base->rpu0_cfg);
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0) {
+		tmp = readl(&rpu_base->rpu0_cfg);
+		if (halt == HALT)
+			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+		else
+			tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
+		writel(tmp, &rpu_base->rpu0_cfg);
+	}
 
-	if (mode == LOCK) {
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1) {
 		tmp = readl(&rpu_base->rpu1_cfg);
 		if (halt == HALT)
 			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
@@ -93,30 +97,34 @@ static void set_r5_tcm_mode(u8 mode)
 	writel(tmp, &rpu_base->rpu_glbl_ctrl);
 }
 
-static void set_r5_reset(u8 mode)
+static void set_r5_reset(u32 nr, u8 mode)
 {
 	u32 tmp;
 
 	tmp = readl(&crlapb_base->rst_lpd_top);
-	tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
-	       ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0)
+		tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
 
-	if (mode == LOCK)
-		tmp |= ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1)
+		tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK);
 
 	writel(tmp, &crlapb_base->rst_lpd_top);
 }
 
-static void release_r5_reset(u8 mode)
+static void release_r5_reset(u32 nr, u8 mode)
 {
 	u32 tmp;
 
 	tmp = readl(&crlapb_base->rst_lpd_top);
-	tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
-	       ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0)
+		tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			 ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
 
-	if (mode == LOCK)
-		tmp &= ~ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
+	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1)
+		tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
+			 ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK);
 
 	writel(tmp, &crlapb_base->rst_lpd_top);
 }
@@ -141,7 +149,7 @@ int cpu_disable(u32 nr)
 		val |= 1 << nr;
 		writel(val, &crfapb_base->rst_fpd_apu);
 	} else {
-		set_r5_reset(LOCK);
+		set_r5_reset(nr, SPLIT);
 	}
 
 	return 0;
@@ -212,14 +220,14 @@ void initialize_tcm(bool mode)
 {
 	if (!mode) {
 		set_r5_tcm_mode(LOCK);
-		set_r5_halt_mode(HALT, LOCK);
+		set_r5_halt_mode(ZYNQMP_CORE_RPU0, HALT, LOCK);
 		enable_clock_r5();
-		release_r5_reset(LOCK);
+		release_r5_reset(ZYNQMP_CORE_RPU0, LOCK);
 	} else {
 		set_r5_tcm_mode(SPLIT);
-		set_r5_halt_mode(HALT, SPLIT);
+		set_r5_halt_mode(ZYNQMP_CORE_RPU1, HALT, SPLIT);
 		enable_clock_r5();
-		release_r5_reset(SPLIT);
+		release_r5_reset(ZYNQMP_CORE_RPU1, SPLIT);
 	}
 }
 
@@ -268,28 +276,28 @@ int cpu_release(u32 nr, int argc, char *const argv[])
 
 		if (!strncmp(argv[1], "lockstep", 8)) {
 			printf("R5 lockstep mode\n");
-			set_r5_reset(LOCK);
+			set_r5_reset(nr, LOCK);
 			set_r5_tcm_mode(LOCK);
-			set_r5_halt_mode(HALT, LOCK);
+			set_r5_halt_mode(nr, HALT, LOCK);
 			set_r5_start(boot_addr);
 			enable_clock_r5();
-			release_r5_reset(LOCK);
+			release_r5_reset(nr, LOCK);
 			dcache_disable();
 			write_tcm_boot_trampoline(boot_addr_uniq);
 			dcache_enable();
-			set_r5_halt_mode(RELEASE, LOCK);
+			set_r5_halt_mode(nr, RELEASE, LOCK);
 		} else if (!strncmp(argv[1], "split", 5)) {
 			printf("R5 split mode\n");
-			set_r5_reset(SPLIT);
+			set_r5_reset(nr, SPLIT);
 			set_r5_tcm_mode(SPLIT);
-			set_r5_halt_mode(HALT, SPLIT);
+			set_r5_halt_mode(nr, HALT, SPLIT);
 			set_r5_start(boot_addr);
 			enable_clock_r5();
-			release_r5_reset(SPLIT);
+			release_r5_reset(nr, SPLIT);
 			dcache_disable();
 			write_tcm_boot_trampoline(boot_addr_uniq);
 			dcache_enable();
-			set_r5_halt_mode(RELEASE, SPLIT);
+			set_r5_halt_mode(nr, RELEASE, SPLIT);
 		} else {
 			printf("Unsupported mode\n");
 			return 1;