From 9f7d75a79cecace1162816f25166deb5de6f2283 Mon Sep 17 00:00:00 2001
From: Herman Chen <herman.chen@rock-chips.com>
Date: Fri, 5 Aug 2022 15:30:39 +0800
Subject: [PATCH] video: rockchip: mpp: rkvenc2: Allow disabled rkvenc-core
 dtsi

Signed-off-by: Herman Chen <herman.chen@rock-chips.com>
Change-Id: Ic2bc6b469e7efbac9b03666a5a0cf791c130ab5b
---
 drivers/video/rockchip/mpp/mpp_common.c  |  4 +++-
 drivers/video/rockchip/mpp/mpp_common.h  |  1 +
 drivers/video/rockchip/mpp/mpp_rkvenc2.c | 10 +++++-----
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/drivers/video/rockchip/mpp/mpp_common.c b/drivers/video/rockchip/mpp/mpp_common.c
index 72ff0f69b301..738b900cfce4 100644
--- a/drivers/video/rockchip/mpp/mpp_common.c
+++ b/drivers/video/rockchip/mpp/mpp_common.c
@@ -1000,6 +1000,8 @@ static void mpp_attach_workqueue(struct mpp_dev *mpp,
 
 	set_bit(core_id, &queue->core_idle);
 	list_add_tail(&mpp->queue_link, &queue->dev_list);
+	if (queue->core_id_max < (u32)core_id)
+		queue->core_id_max = (u32)core_id;
 
 	mpp->core_id = core_id;
 	mpp->queue = queue;
@@ -1024,7 +1026,7 @@ static void mpp_detach_workqueue(struct mpp_dev *mpp)
 		queue->cores[mpp->core_id] = NULL;
 		queue->core_count--;
 
-		clear_bit(queue->core_count, &queue->core_idle);
+		clear_bit(mpp->core_id, &queue->core_idle);
 		list_del_init(&mpp->queue_link);
 
 		mpp->queue = NULL;
diff --git a/drivers/video/rockchip/mpp/mpp_common.h b/drivers/video/rockchip/mpp/mpp_common.h
index f7d1b97336df..d0be408fbb80 100644
--- a/drivers/video/rockchip/mpp/mpp_common.h
+++ b/drivers/video/rockchip/mpp/mpp_common.h
@@ -502,6 +502,7 @@ struct mpp_taskqueue {
 	atomic_t reset_request;
 	struct mpp_dev *cores[MPP_MAX_CORE_NUM];
 	unsigned long core_idle;
+	u32 core_id_max;
 	u32 core_count;
 };
 
diff --git a/drivers/video/rockchip/mpp/mpp_rkvenc2.c b/drivers/video/rockchip/mpp/mpp_rkvenc2.c
index 428bb1eb411b..c4c383a59270 100644
--- a/drivers/video/rockchip/mpp/mpp_rkvenc2.c
+++ b/drivers/video/rockchip/mpp/mpp_rkvenc2.c
@@ -807,25 +807,25 @@ static void *rkvenc2_prepare(struct mpp_dev *mpp, struct mpp_task *mpp_task)
 	struct mpp_taskqueue *queue = mpp->queue;
 	unsigned long core_idle;
 	unsigned long flags;
-	u32 core_count;
+	u32 core_id_max;
 	s32 core_id;
 	u32 i;
 
 	spin_lock_irqsave(&queue->running_lock, flags);
 
 	core_idle = queue->core_idle;
-	core_count = queue->core_count;
+	core_id_max = queue->core_id_max;
 
-	for (i = 0; i < core_count; i++) {
+	for (i = 0; i < core_id_max; i++) {
 		struct mpp_dev *mpp = queue->cores[i];
 
 		if (mpp && to_rkvenc_dev(mpp)->disable_work)
 			clear_bit(i, &core_idle);
 	}
 
-	core_id = find_first_bit(&core_idle, core_count);
+	core_id = find_first_bit(&core_idle, core_id_max + 1);
 
-	if (core_id >= core_count) {
+	if (core_id >= core_id_max + 1 || !queue->cores[core_id]) {
 		mpp_task = NULL;
 		mpp_dbg_core("core %d all busy %lx\n", core_id, core_idle);
 	} else {