diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
index 5ca4fcade790..c70cd445146a 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop2.c
@@ -642,6 +642,11 @@ struct vop2_video_port {
 	 * @win_mask: Bitmask of wins attached to the video port;
 	 */
 	uint32_t win_mask;
+	/**
+	 * @enabled_win_mask: Bitmask of enabled wins attached to the video port;
+	 */
+	uint32_t enabled_win_mask;
+
 	/**
 	 * @nr_layers: active layers attached to the video port;
 	 */
@@ -4269,13 +4274,24 @@ static int vop2_clk_set_parent_extend(struct vop2_video_port *vp,
 	return 0;
 }
 
-static void vop2_crtc_atomic_disable_for_psr(struct drm_crtc *crtc,
-					     struct drm_crtc_state *old_state)
+static void vop2_crtc_atomic_enter_psr(struct drm_crtc *crtc, struct drm_crtc_state *old_state)
 {
 	struct vop2_video_port *vp = to_vop2_video_port(crtc);
 	struct vop2 *vop2 = vp->vop2;
+	struct vop2_win *win;
+	unsigned long win_mask = vp->enabled_win_mask;
+	int phys_id;
 
-	vop2_disable_all_planes_for_crtc(crtc);
+	for_each_set_bit(phys_id, &win_mask, ROCKCHIP_MAX_LAYER) {
+		win = vop2_find_win_by_phys_id(vop2, phys_id);
+		VOP_WIN_SET(vop2, win, enable, 0);
+
+		if (win->feature & WIN_FEATURE_CLUSTER_MAIN)
+			VOP_CLUSTER_SET(vop2, win, enable, 0);
+	}
+
+	vop2_cfg_done(crtc);
+	vop2_wait_for_fs_by_done_bit_status(vp);
 	drm_crtc_vblank_off(crtc);
 	if (hweight8(vop2->active_vp_mask) == 1) {
 		u32 adjust_aclk_rate = 0;
@@ -4298,6 +4314,30 @@ static void vop2_crtc_atomic_disable_for_psr(struct drm_crtc *crtc,
 	}
 }
 
+static void vop2_crtc_atomic_exit_psr(struct drm_crtc *crtc, struct drm_crtc_state *old_state)
+{
+	struct vop2_video_port *vp = to_vop2_video_port(crtc);
+	struct vop2 *vop2 = vp->vop2;
+	u32 phys_id;
+	struct vop2_win *win;
+	unsigned long enabled_win_mask = vp->enabled_win_mask;
+
+	drm_crtc_vblank_on(crtc);
+	if (vop2->aclk_rate_reset)
+		clk_set_rate(vop2->aclk, vop2->aclk_rate);
+	vop2->aclk_rate_reset = false;
+
+	for_each_set_bit(phys_id, &enabled_win_mask, ROCKCHIP_MAX_LAYER) {
+		win = vop2_find_win_by_phys_id(vop2, phys_id);
+		VOP_WIN_SET(vop2, win, enable, 1);
+		if (win->feature & WIN_FEATURE_CLUSTER_MAIN)
+			VOP_CLUSTER_SET(vop2, win, enable, 1);
+	}
+
+	vop2_cfg_done(crtc);
+	vop2_wait_for_fs_by_done_bit_status(vp);
+}
+
 static void vop2_crtc_atomic_disable(struct drm_crtc *crtc,
 				     struct drm_crtc_state *old_state)
 {
@@ -4312,7 +4352,7 @@ static void vop2_crtc_atomic_disable(struct drm_crtc *crtc,
 	WARN_ON(vp->event);
 
 	if (crtc->state->self_refresh_active) {
-		vop2_crtc_atomic_disable_for_psr(crtc, old_state);
+		vop2_crtc_atomic_enter_psr(crtc, old_state);
 		goto out;
 	}
 
@@ -4777,6 +4817,9 @@ static void vop2_plane_atomic_disable(struct drm_plane *plane, struct drm_plane_
 {
 	struct vop2_win *win = to_vop2_win(plane);
 	struct vop2 *vop2 = win->vop2;
+	struct drm_crtc *crtc;
+	struct vop2_video_port *vp;
+
 #if defined(CONFIG_ROCKCHIP_DRM_DEBUG)
 	struct vop2_plane_state *vpstate = to_vop2_plane_state(plane->state);
 #endif
@@ -4789,9 +4832,15 @@ static void vop2_plane_atomic_disable(struct drm_plane *plane, struct drm_plane_
 
 	spin_lock(&vop2->reg_lock);
 
+	crtc = old_state->crtc;
+	vp = to_vop2_video_port(crtc);
+
 	vop2_win_disable(win, false);
-	if (win->splice_win)
+	vp->enabled_win_mask &= ~BIT(win->phys_id);
+	if (win->splice_win) {
 		vop2_win_disable(win->splice_win, false);
+		vp->enabled_win_mask &= ~BIT(win->splice_win->phys_id);
+	}
 
 #if defined(CONFIG_ROCKCHIP_DRM_DEBUG)
 	kfree(vpstate->planlist);
@@ -5203,6 +5252,7 @@ static void vop2_win_atomic_update(struct vop2_win *win, struct drm_rect *src, s
 	VOP_WIN_SET(vop2, win, dither_up, dither_up);
 
 	VOP_WIN_SET(vop2, win, enable, 1);
+	vp->enabled_win_mask |= BIT(win->phys_id);
 	if (vop2_cluster_window(win)) {
 		lb_mode = vop2_get_cluster_lb_mode(win, vpstate);
 		VOP_CLUSTER_SET(vop2, win, lb_mode, lb_mode);
@@ -5972,6 +6022,7 @@ static int vop2_crtc_loader_protect(struct drm_crtc *crtc, bool on, void *data)
 					win->pd->vp_mask |= BIT(vp->id);
 				}
 
+				vp->enabled_win_mask |= BIT(win->phys_id);
 				crtc_state = drm_atomic_get_crtc_state(crtc->state->state, crtc);
 				mode = &crtc_state->adjusted_mode;
 				if (mode->hdisplay > VOP2_MAX_VP_OUTPUT_WIDTH)	{
@@ -5984,6 +6035,7 @@ static int vop2_crtc_loader_protect(struct drm_crtc *crtc, bool on, void *data)
 					splice_vp->win_mask |=  BIT(splice_win->phys_id);
 					splice_win->vp_mask = BIT(splice_vp->id);
 					vop2->active_vp_mask |= BIT(splice_vp->id);
+					vp->enabled_win_mask |= BIT(splice_win->phys_id);
 
 					if (splice_win->pd &&
 					    VOP_WIN_GET(vop2, splice_win, enable)) {
@@ -7559,10 +7611,7 @@ static void vop2_crtc_atomic_enable(struct drm_crtc *crtc, struct drm_crtc_state
 	int ret;
 
 	if (old_state && old_state->self_refresh_active) {
-		drm_crtc_vblank_on(crtc);
-		if (vop2->aclk_rate_reset)
-			clk_set_rate(vop2->aclk, vop2->aclk_rate);
-		vop2->aclk_rate_reset = false;
+		vop2_crtc_atomic_exit_psr(crtc, old_state);
 
 		return;
 	}