ov5640_mipi: add remove *2 and add error checking to OV5640_get_sysclk

1 files changed, 52 insertions, 32 deletions

diff --git a/drivers/media/platform/mxc/capture/ov5640_mipi.c b/drivers/media/platform/mxc/capture/ov5640_mipi.c
index 1a8588920724..36560495349b 100644
--- a/drivers/media/platform/mxc/capture/ov5640_mipi.c
+++ b/drivers/media/platform/mxc/capture/ov5640_mipi.c
@@ -1944,7 +1944,7 @@ static void ov5640_standby(s32 enable)
 		gpio_set_value(pwn_gpio, 1);
 	else
 		gpio_set_value(pwn_gpio, 0);
-
+	pr_debug("ov5640_mipi_camera_powerdown: powerdown=%x, power_gp=0x%x\n", enable, pwn_gpio);
 	msleep(2);
 }
 
@@ -2044,7 +2044,7 @@ static s32 ov5640_write_reg(u16 reg, u8 val)
 			__func__, reg, val);
 		return -1;
 	}
-
+	pr_debug("reg=%x,val=%x\n", reg, val);
 	return 0;
 }
 
@@ -2086,43 +2086,62 @@ void OV5640_stream_off(void)
 	ov5640_write_reg(0x4202, 0x0f);
 }
 
+static const int sclk_rdiv_map[] = {1, 2, 4, 8};
 
 int OV5640_get_sysclk(void)
 {
 	 /* calculate sysclk */
-	int xvclk = ov5640_data.mclk / 10000;
-	int temp1, temp2;
-	int Multiplier, PreDiv, VCO, SysDiv, Pll_rdiv;
-	int Bit_div2x = 1, sclk_rdiv, sysclk;
+	int tmp;
+	unsigned Multiplier, PreDiv, SysDiv, Pll_rdiv, Bit_div2x = 1;
+	unsigned div, sclk_rdiv, sysclk;
 	u8 temp;
 
-	int sclk_rdiv_map[] = {1, 2, 4, 8};
-
-	temp1 = ov5640_read_reg(0x3034, &temp);
-	temp2 = temp1 & 0x0f;
-	if (temp2 == 8 || temp2 == 10)
-		Bit_div2x = temp2 / 2;
-
-	temp1 = ov5640_read_reg(0x3035, &temp);
-	SysDiv = temp1>>4;
+	tmp = ov5640_read_reg(0x3034, &temp);
+	if (tmp < 0)
+		return tmp;
+	tmp &= 0x0f;
+	if (tmp == 8 || tmp == 10)
+		Bit_div2x = tmp / 2;
+
+	tmp = ov5640_read_reg(0x3035, &temp);
+	if (tmp < 0)
+		return tmp;
+	SysDiv = tmp >> 4;
 	if (SysDiv == 0)
-		SysDiv = 16;
-
-	temp1 = ov5640_read_reg(0x3036, &temp);
-	Multiplier = temp1;
-
-	temp1 = ov5640_read_reg(0x3037, &temp);
-	PreDiv = temp1 & 0x0f;
-	Pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
-
-	temp1 = ov5640_read_reg(0x3108, &temp);
-	temp2 = temp1 & 0x03;
-	sclk_rdiv = sclk_rdiv_map[temp2];
-
-	VCO = xvclk * Multiplier / PreDiv;
-
-	sysclk = VCO / SysDiv / Pll_rdiv * 2 / Bit_div2x / sclk_rdiv;
-
+	       SysDiv = 16;
+
+	tmp = ov5640_read_reg(0x3036, &temp);
+	if (tmp < 0)
+		return tmp;
+	Multiplier = tmp;
+
+	tmp = ov5640_read_reg(0x3037, &temp);
+	if (tmp < 0)
+		return tmp;
+	PreDiv = tmp & 0x0f;
+	Pll_rdiv = ((tmp >> 4) & 0x01) + 1;
+
+	tmp = ov5640_read_reg(0x3108, &temp);
+	if (tmp < 0)
+		return tmp;
+	sclk_rdiv = sclk_rdiv_map[tmp & 0x03];
+
+	sysclk = ov5640_data.mclk / 10000 * Multiplier;
+	div = PreDiv * SysDiv * Pll_rdiv * Bit_div2x * sclk_rdiv;
+	if (!div) {
+		pr_err("%s:Error divide by 0, (%d * %d * %d * %d * %d)\n",
+			__func__, PreDiv, SysDiv, Pll_rdiv, Bit_div2x, sclk_rdiv);
+		return -EINVAL;
+	}
+	if (!sysclk) {
+		pr_err("%s:Error 0 clk, ov5640_data.mclk=%d, Multiplier=%d\n",
+			__func__, ov5640_data.mclk, Multiplier);
+		return -EINVAL;
+	}
+	sysclk /= div;
+	pr_debug("%s: sysclk(%d) = %d / 10000 * %d / (%d * %d * %d * %d * %d)\n",
+		__func__, sysclk, ov5640_data.mclk, Multiplier,
+		PreDiv, SysDiv, Pll_rdiv, Bit_div2x, sclk_rdiv);
 	return sysclk;
 }
 
@@ -2263,6 +2282,7 @@ int OV5640_get_light_freq(void)
 			light_freq = 50;
 		} else {
 			/* 60Hz */
+			light_freq = 60;
 		}
 	}
 	return light_freq;