media: video: tegra: return next avaiable clock

tegra_camera_clk_set_rate sets the clock rate which is
equal to or greater than requested clock rate. In this way,
user space code doesn't have to scan through to find next
available higher clock through system call.
Remove setting VI/ICP clock to register directly.

Bug 917641

Reviewed-on: http://git-master/r/75097

Change-Id: Iecbeacecb34c6b6f71228932ba4b046658ff905d
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>
Reviewed-on: http://git-master/r/77727
Reviewed-by: Automatic_Commit_Validation_User

1 files changed, 30 insertions, 16 deletions

diff --git a/drivers/media/video/tegra/tegra_camera.c b/drivers/media/video/tegra/tegra_camera.c
index aea6bc2517f5..7b6d8fd868e1 100644
--- a/drivers/media/video/tegra/tegra_camera.c
+++ b/drivers/media/video/tegra/tegra_camera.c
@@ -144,9 +144,9 @@ static bool tegra_camera_enabled(struct tegra_camera_dev *dev)
 
 static int tegra_camera_clk_set_rate(struct tegra_camera_dev *dev)
 {
-	u32 offset;
-	struct clk *clk;
+	struct clk *clk, *clk_parent;
 	struct tegra_camera_clk_info *info = &dev->info;
+	unsigned long parent_rate, parent_div_rate, parent_div_rate_pre;
 
 	if (!info) {
 		dev_err(dev->dev,
@@ -165,11 +165,9 @@ static int tegra_camera_clk_set_rate(struct tegra_camera_dev *dev)
 	switch (info->clk_id) {
 	case TEGRA_CAMERA_VI_CLK:
 		clk = dev->vi_clk;
-		offset = 0x148;
 		break;
 	case TEGRA_CAMERA_VI_SENSOR_CLK:
 		clk = dev->vi_sensor_clk;
-		offset = 0x1a8;
 		break;
 	default:
 		dev_err(dev->dev,
@@ -178,24 +176,40 @@ static int tegra_camera_clk_set_rate(struct tegra_camera_dev *dev)
 		return -EINVAL;
 	}
 
-	clk_set_rate(clk, info->rate);
-
-	if (info->clk_id == TEGRA_CAMERA_VI_CLK) {
-		u32 val = 0x2;
-		void __iomem *car = IO_ADDRESS(TEGRA_CLK_RESET_BASE);
-		void __iomem *apb_misc = IO_ADDRESS(TEGRA_APB_MISC_BASE);
+	clk_parent = clk_get_parent(clk);
+	parent_rate = clk_get_rate(clk_parent);
+	dev_dbg(dev->dev, "%s: clk_id=%d, parent_rate=%lu, clk_rate=%lu\n",
+			__func__, info->clk_id, parent_rate, info->rate);
+	parent_div_rate = parent_rate;
+	parent_div_rate_pre = parent_rate;
+
+	/*
+	 * The requested clock rate from user space should be respected.
+	 * This loop is to search the clock rate that is higher than requested
+	 * clock.
+	 */
+	while (parent_div_rate >= info->rate) {
+		parent_div_rate_pre = parent_div_rate;
+		parent_div_rate = clk_round_rate(clk, parent_div_rate-1);
+	}
 
-		if (info->flag == TEGRA_CAMERA_ENABLE_PD2VI_CLK) {
-			val |= TEGRA_CAMERA_PD2VI_CLK_SEL_VI_SENSOR_CLK;
-		}
+	dev_dbg(dev->dev, "%s: set_rate=%lu",
+			__func__, parent_div_rate_pre);
 
-		writel(val, car + offset);
+	clk_set_rate(clk, parent_div_rate_pre);
 
-		val = readl(apb_misc + 0x42c);
-		writel(val | 0x1, apb_misc + 0x42c);
+	if (info->clk_id == TEGRA_CAMERA_VI_CLK) {
+		/*
+		 * bit 25: 0 = pd2vi_Clk, 1 = vi_sensor_clk
+		 * bit 24: 0 = internal clock, 1 = external clock(pd2vi_clk)
+		 */
+		if (info->flag == TEGRA_CAMERA_ENABLE_PD2VI_CLK)
+			tegra_clk_cfg_ex(clk, TEGRA_CLK_VI_INP_SEL, 2);
 	}
 
 	info->rate = clk_get_rate(clk);
+	dev_dbg(dev->dev, "%s: get_rate=%lu",
+			__func__, info->rate);
 	return 0;
 
 }