media: i2c: Add 2 lane support for IMX290imx290-db410c

Add 2 lane support for IMX290 so that it can work with both 4
and 2 lane host systems.

Signed-off-by: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>

1 files changed, 97 insertions, 10 deletions

diff --git a/drivers/media/i2c/imx290.c b/drivers/media/i2c/imx290.c
index 52ff06941bca..892b6733fb59 100644
--- a/drivers/media/i2c/imx290.c
+++ b/drivers/media/i2c/imx290.c
@@ -24,7 +24,17 @@
 #define IMX290_STANDBY 0x3000
 #define IMX290_REGHOLD 0x3001
 #define IMX290_XMSTA 0x3002
+#define IMX290_FR_FDG_SEL 0x3009
 #define IMX290_GAIN 0x3014
+#define IMX290_HMAX_LOW	0x301C
+#define IMX290_HMAX_HIGH 0x301D
+#define IMX290_PHY_LANE_NUM 0x3407
+#define IMX290_CSI_LANE_MODE 0x3443
+
+#define IMX290_HMAX_2_1920 0x1130
+#define IMX290_HMAX_4_1920 0x0898
+#define IMX290_HMAX_2_720 0x19C8
+#define IMX290_HMAX_4_720 0x0CE4
 
 static const char * const imx290_supply_name[] = {
 	"vdda",
@@ -53,6 +63,7 @@ struct imx290 {
 	struct device *dev;
 	struct clk *xclk;
 	struct regmap *regmap;
+	int nlanes;
 
 	struct v4l2_subdev sd;
 	struct v4l2_fwnode_endpoint ep;
@@ -87,14 +98,11 @@ static struct regmap_config imx290_regmap_config = {
 
 static const struct imx290_regval imx290_global_init_settings[] = {
 	{ 0x3007, 0x00 },
-	{ 0x3009, 0x00 },
 	{ 0x3018, 0x65 },
 	{ 0x3019, 0x04 },
 	{ 0x301a, 0x00 },
-	{ 0x3443, 0x03 },
 	{ 0x3444, 0x20 },
 	{ 0x3445, 0x25 },
-	{ 0x3407, 0x03 },
 	{ 0x303a, 0x0c },
 	{ 0x3040, 0x00 },
 	{ 0x3041, 0x00 },
@@ -167,7 +175,6 @@ static const struct imx290_regval imx290_1080p_settings[] = {
 	{ 0x3164, 0x1a },
 	{ 0x3480, 0x49 },
 	/* data rate settings */
-	{ 0x3009, 0x01 },
 	{ 0x3405, 0x10 },
 	{ 0x3446, 0x57 },
 	{ 0x3447, 0x00 },
@@ -185,8 +192,6 @@ static const struct imx290_regval imx290_1080p_settings[] = {
 	{ 0x3453, 0x00 },
 	{ 0x3454, 0x17 },
 	{ 0x3455, 0x00 },
-	{ 0x301c, 0x98 },
-	{ 0x301d, 0x08 },
 };
 
 static const struct imx290_regval imx290_720p_settings[] = {
@@ -208,7 +213,6 @@ static const struct imx290_regval imx290_720p_settings[] = {
 	{ 0x3164, 0x1a },
 	{ 0x3480, 0x49 },
 	/* data rate settings */
-	{ 0x3009, 0x01 },
 	{ 0x3405, 0x10 },
 	{ 0x3446, 0x4f },
 	{ 0x3447, 0x00 },
@@ -226,8 +230,6 @@ static const struct imx290_regval imx290_720p_settings[] = {
 	{ 0x3453, 0x00 },
 	{ 0x3454, 0x17 },
 	{ 0x3455, 0x00 },
-	{ 0x301c, 0xe4 },
-	{ 0x301d, 0x0c },
 };
 
 static const struct imx290_regval imx290_10bit_settings[] = {
@@ -417,7 +419,7 @@ static void imx290_set_power_off(struct imx290 *imx290)
 static int imx290_s_power(struct v4l2_subdev *sd, int on)
 {
 	struct imx290 *imx290 = to_imx290(sd);
-	int ret = 0;
+	int ret = 0, laneval, frsel;
 
 	mutex_lock(&imx290->lock);
 
@@ -426,6 +428,40 @@ static int imx290_s_power(struct v4l2_subdev *sd, int on)
 		if (ret < 0)
 			goto exit;
 
+		switch(imx290->nlanes) {
+		case 2:
+			laneval = 0x01;
+			frsel = 0x02;
+			break;
+		case 4:
+			laneval = 0x03;
+			frsel = 0x01;
+			break;
+		default:
+			dev_dbg(imx290->dev,
+				"Lane configuration not supported\n");
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		ret = imx290_write_reg(imx290, IMX290_PHY_LANE_NUM, laneval);
+		if (ret) {
+			dev_err(imx290->dev, "Error setting Physical Lane number register\n");
+			goto exit;
+		}
+
+		ret = imx290_write_reg(imx290, IMX290_CSI_LANE_MODE, laneval);
+		if (ret) {
+			dev_err(imx290->dev, "Error setting CSI Lane mode register\n");
+			goto exit;
+		}
+
+		ret = imx290_write_reg(imx290, IMX290_FR_FDG_SEL, frsel);
+		if (ret) {
+			dev_err(imx290->dev, "Error setting FR/FDG SEL register\n");
+			goto exit;
+		}
+
 		ret = imx290_set_register_array(imx290,
 				imx290_global_init_settings,
 				ARRAY_SIZE(imx290_global_init_settings));
@@ -600,6 +636,25 @@ static int imx290_write_current_format(struct imx290 *imx290,
 	return 0;
 }
 
+int imx290_set_hmax(struct imx290 *imx290, u32 val)
+{
+	int ret;
+
+	ret = imx290_write_reg(imx290, IMX290_HMAX_LOW, (val & 0xFF));
+	if (ret) {
+		dev_err(imx290->dev, "Error setting HMAX register\n");
+		return ret;
+	}
+
+	ret = imx290_write_reg(imx290, IMX290_HMAX_HIGH, ((val >> 8) & 0xFF));
+	if (ret) {
+		dev_err(imx290->dev, "Error setting HMAX register\n");
+		return ret;
+	}
+
+	return 0;
+}
+
 /* Start streaming */
 static int imx290_start_streaming(struct imx290 *imx290)
 {
@@ -620,6 +675,36 @@ static int imx290_start_streaming(struct imx290 *imx290)
 		return ret;
 	}
 
+	switch(imx290->nlanes) {
+	case 2:
+		if (imx290->current_mode->width == 1920) {
+			ret = imx290_set_hmax(imx290, IMX290_HMAX_2_1920);
+			if (ret < 0)
+				return ret;
+		} else {
+			ret = imx290_set_hmax(imx290, IMX290_HMAX_2_720);
+			if (ret < 0)
+				return ret;
+		}
+
+		break;
+	case 4:
+		if (imx290->current_mode->width == 1920) {
+			ret = imx290_set_hmax(imx290, IMX290_HMAX_4_1920);
+			if (ret < 0)
+				return ret;
+		} else {
+			ret = imx290_set_hmax(imx290, IMX290_HMAX_4_720);
+			if (ret < 0)
+				return ret;
+		}
+
+		break;
+	default:
+		dev_dbg(imx290->dev, "Lane configuration not supported\n");
+		return -EINVAL;
+	}
+
 	/* Apply customized values from user */
 	ret = v4l2_ctrl_handler_setup(imx290->sd.ctrl_handler);
 	if (ret) {
@@ -725,6 +810,8 @@ static int imx290_probe(struct i2c_client *client,
 		return -EINVAL;
 	}
 
+	imx290->nlanes = imx290->ep.bus.mipi_csi2.num_data_lanes;
+
 	/* Set default mode to max resolution */
 	imx290->current_mode = &imx290_modes[0];