1 files changed, 60 insertions, 44 deletions

diff --git a/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_pm.c b/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_pm.c
index 7c025045ea90..39375b8ea27c 100644
--- a/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_pm.c
+++ b/drivers/media/platform/mtk-vcodec/mtk_vcodec_enc_pm.c
@@ -27,9 +27,11 @@ int mtk_vcodec_init_enc_pm(struct mtk_vcodec_dev *mtkdev)
 {
 	struct device_node *node;
 	struct platform_device *pdev;
-	struct device *dev;
 	struct mtk_vcodec_pm *pm;
-	int ret = 0;
+	struct mtk_vcodec_clk *enc_clk;
+	struct mtk_vcodec_clk_info *clk_info;
+	int ret = 0, i = 0;
+	struct device *dev;
 
 	pdev = mtkdev->plat_dev;
 	pm = &mtkdev->pm;
@@ -37,6 +39,7 @@ int mtk_vcodec_init_enc_pm(struct mtk_vcodec_dev *mtkdev)
 	pm->mtkdev = mtkdev;
 	pm->dev = &pdev->dev;
 	dev = &pdev->dev;
+	enc_clk = &pm->venc_clk;
 
 	node = of_parse_phandle(dev->of_node, "mediatek,larb", 0);
 	if (!node) {
@@ -68,28 +71,34 @@ int mtk_vcodec_init_enc_pm(struct mtk_vcodec_dev *mtkdev)
 	pdev = mtkdev->plat_dev;
 	pm->dev = &pdev->dev;
 
-	pm->vencpll_d2 = devm_clk_get(&pdev->dev, "venc_sel_src");
-	if (IS_ERR(pm->vencpll_d2)) {
-		mtk_v4l2_err("devm_clk_get vencpll_d2 fail");
-		ret = PTR_ERR(pm->vencpll_d2);
-	}
-
-	pm->venc_sel = devm_clk_get(&pdev->dev, "venc_sel");
-	if (IS_ERR(pm->venc_sel)) {
-		mtk_v4l2_err("devm_clk_get venc_sel fail");
-		ret = PTR_ERR(pm->venc_sel);
+	enc_clk->clk_num = of_property_count_strings(pdev->dev.of_node,
+		"clock-names");
+	if (enc_clk->clk_num > 0) {
+		enc_clk->clk_info = devm_kcalloc(&pdev->dev,
+			enc_clk->clk_num, sizeof(*clk_info),
+			GFP_KERNEL);
+		if (!enc_clk->clk_info)
+			return -ENOMEM;
+	} else {
+		mtk_v4l2_err("Failed to get venc clock count");
+		return -EINVAL;
 	}
 
-	pm->univpll1_d2 = devm_clk_get(&pdev->dev, "venc_lt_sel_src");
-	if (IS_ERR(pm->univpll1_d2)) {
-		mtk_v4l2_err("devm_clk_get univpll1_d2 fail");
-		ret = PTR_ERR(pm->univpll1_d2);
-	}
-
-	pm->venc_lt_sel = devm_clk_get(&pdev->dev, "venc_lt_sel");
-	if (IS_ERR(pm->venc_lt_sel)) {
-		mtk_v4l2_err("devm_clk_get venc_lt_sel fail");
-		ret = PTR_ERR(pm->venc_lt_sel);
+	for (i = 0; i < enc_clk->clk_num; i++) {
+		clk_info = &enc_clk->clk_info[i];
+		ret = of_property_read_string_index(pdev->dev.of_node,
+			"clock-names", i, &clk_info->clk_name);
+		if (ret) {
+			mtk_v4l2_err("venc failed to get clk name %d", i);
+			return ret;
+		}
+		clk_info->vcodec_clk = devm_clk_get(&pdev->dev,
+			clk_info->clk_name);
+		if (IS_ERR(clk_info->vcodec_clk)) {
+			mtk_v4l2_err("venc devm_clk_get (%d)%s fail", i,
+				clk_info->clk_name);
+			return PTR_ERR(clk_info->vcodec_clk);
+		}
 	}
 
 	return ret;
@@ -102,38 +111,45 @@ void mtk_vcodec_release_enc_pm(struct mtk_vcodec_dev *mtkdev)
 
 void mtk_vcodec_enc_clock_on(struct mtk_vcodec_pm *pm)
 {
-	int ret;
-
-	ret = clk_prepare_enable(pm->venc_sel);
-	if (ret)
-		mtk_v4l2_err("clk_prepare_enable fail %d", ret);
-
-	ret = clk_set_parent(pm->venc_sel, pm->vencpll_d2);
-	if (ret)
-		mtk_v4l2_err("clk_set_parent fail %d", ret);
-
-	ret = clk_prepare_enable(pm->venc_lt_sel);
-	if (ret)
-		mtk_v4l2_err("clk_prepare_enable fail %d", ret);
-
-	ret = clk_set_parent(pm->venc_lt_sel, pm->univpll1_d2);
-	if (ret)
-		mtk_v4l2_err("clk_set_parent fail %d", ret);
+	struct mtk_vcodec_clk *enc_clk = &pm->venc_clk;
+	int ret, i = 0;
+
+	for (i = 0; i < enc_clk->clk_num; i++) {
+		ret = clk_prepare_enable(enc_clk->clk_info[i].vcodec_clk);
+		if (ret) {
+			mtk_v4l2_err("venc clk_prepare_enable %d %s fail %d", i,
+				enc_clk->clk_info[i].clk_name, ret);
+			goto clkerr;
+		}
+	}
 
 	ret = mtk_smi_larb_get(pm->larbvenc);
-	if (ret)
+	if (ret) {
 		mtk_v4l2_err("mtk_smi_larb_get larb3 fail %d", ret);
-
+		goto larbvencerr;
+	}
 	ret = mtk_smi_larb_get(pm->larbvenclt);
-	if (ret)
+	if (ret) {
 		mtk_v4l2_err("mtk_smi_larb_get larb4 fail %d", ret);
+		goto larbvenclterr;
+	}
+	return;
 
+larbvenclterr:
+	mtk_smi_larb_put(pm->larbvenc);
+larbvencerr:
+clkerr:
+	for (i -= 1; i >= 0; i--)
+		clk_disable_unprepare(enc_clk->clk_info[i].vcodec_clk);
 }
 
 void mtk_vcodec_enc_clock_off(struct mtk_vcodec_pm *pm)
 {
+	struct mtk_vcodec_clk *enc_clk = &pm->venc_clk;
+	int i = 0;
+
 	mtk_smi_larb_put(pm->larbvenc);
 	mtk_smi_larb_put(pm->larbvenclt);
-	clk_disable_unprepare(pm->venc_lt_sel);
-	clk_disable_unprepare(pm->venc_sel);
+	for (i = enc_clk->clk_num - 1; i >= 0; i--)
+		clk_disable_unprepare(enc_clk->clk_info[i].vcodec_clk);
 }