diff --git a/arch/arm64/boot/dts/freescale/imx8mq-librem5.dtsi b/arch/arm64/boot/dts/freescale/imx8mq-librem5.dtsi
index 7ea2848b799325f56ec6244e4f2b370fd6d608e6..c61c64adc2872a1e32dec081c617cf47ae6538da 100644
--- a/arch/arm64/boot/dts/freescale/imx8mq-librem5.dtsi
+++ b/arch/arm64/boot/dts/freescale/imx8mq-librem5.dtsi
@@ -1284,7 +1284,7 @@ camera_front: camera1@20 {
 		status = "okay";
 		port {
 			camera1_ep: endpoint {
-				data-lanes = <1 2 3 4>;
+				data-lanes = <1 2>;
 				remote-endpoint = <&mipi1_sensor_ep>;
 			};
 		};
@@ -1412,7 +1412,7 @@ port {
 		mipi1_sensor_ep: endpoint@1 {
 			reg = <1>;
 			remote-endpoint = <&camera1_ep>;
-			data-lanes = <1 2 3 4>;
+			data-lanes = <1 2>;
 		};
 
 		csi1_mipi_ep: endpoint@2 {
diff --git a/drivers/media/i2c/hi846.c b/drivers/media/i2c/hi846.c
index 6cdcb642797adcb4ee3c8eda8160884468068ec5..8e9fc572106be6cac410bbfefc2e7bd959d059c7 100644
--- a/drivers/media/i2c/hi846.c
+++ b/drivers/media/i2c/hi846.c
@@ -1,5 +1,5 @@
 // SPDX-License-Identifier: GPL-2.0
-// Copyright (c) 2020 Purism SPC
+// Copyright (c) 2021 Purism SPC
 
 #include <asm/unaligned.h>
 #include <linux/acpi.h>
@@ -17,70 +17,23 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
 
-#define HI846_LINK_FREQ_437MHZ		437000000ULL
-#define HI846_LINK_FREQ_288MHZ		288000000ULL
-#define HI846_LINK_FREQ_200MHZ		200000000ULL
+#include "hi846.h"
 
-#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR8_1X8
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SGBRG8_1X8
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SGRBG8_1X8
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SRGGB8_1X8
+//#define HI846_LINK_FREQ			437000000
+//#define HI846_LINK_FREQ			288000000
+//#define HI846_LINK_FREQ			200000000
+//#define HI846_LINK_FREQ			144000000
+#define HI846_LINK_FREQ			80000000
 
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR10_1X10
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SGBRG10_1X10
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SGRBG10_1X10
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SRGGB10_1X10
+#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR10_1X10
 
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_BE
-//#define HI846_MEDIA_BUS_FORMAT		MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_LE
-
-#define HI846_LINE_LENGTH		3800
-
-#define HI846_DATA_LANES		4
-#define HI846_RGB_DEPTH			8
+#define HI846_DATA_LANES		2
+#define HI846_RGB_DEPTH			10
 
 #define DEFAULT_FPS			30
 
-#define HI846_REG_CHIP_ID_L		0x0f16
-#define HI846_REG_CHIP_ID_H		0x0f17
-#define HI846_CHIP_ID_L			0x46
-#define HI846_CHIP_ID_H			0x08
-
-/* Digital gain controls from sensor */
-#define HI846_REG_MWB_GR_GAIN_H		0x0078
-#define HI846_REG_MWB_GR_GAIN_L		0x0079
-#define HI846_REG_MWB_GB_GAIN_H		0x007a
-#define HI846_REG_MWB_GB_GAIN_L		0x007b
-#define HI846_REG_MWB_R_GAIN_H		0x007c
-#define HI846_REG_MWB_R_GAIN_L		0x007d
-#define HI846_REG_MWB_B_GAIN_H		0x007e
-#define HI846_REG_MWB_B_GAIN_L		0x007f
-#define HI846_DGTL_GAIN_MIN		0
-#define HI846_DGTL_GAIN_MAX		8191
-#define HI846_DGTL_GAIN_STEP		1
-#define HI846_DGTL_GAIN_DEFAULT		256
-
-/* Analog gain controls from sensor */
-#define HI846_REG_ANALOG_GAIN		0x0077 /* 0x0076 ? */
-#define HI846_ANAL_GAIN_MIN		0
-#define HI846_ANAL_GAIN_MAX		240
-#define HI846_ANAL_GAIN_STEP		8
-
-/* Test Pattern Control */
-#define HI846_REG_ISP			0X0a05
-#define HI846_REG_ISP_TPG_EN		0x01
-#define HI846_REG_TEST_PATTERN		0x020a /* 1-9 */
-
-/* ISP Common */
-#define HI846_REG_MODE_SELECT		0x0a00
-#define HI846_MODE_STANDBY		0x00
-#define HI846_MODE_STREAMING		0x01
-
-
 enum {
-	HI846_LINK_FREQ_437MHZ_INDEX,
+	HI846_LINK_FREQ_INDEX,
 };
 
 struct hi846_reg {
@@ -101,6 +54,15 @@ struct hi846_mode {
 	/* Frame height in pixels */
 	u32 height;
 
+	/* Horizontal timing size */
+	u32 llp;
+
+	/* Default vertical timining size */
+	u32 fll_def;
+
+	/* Min vertical timining size */
+	u32 fll_min;
+
 	/* Link frequency needed for this resolution */
 	u32 link_freq;
 
@@ -114,8 +76,8 @@ struct hi846_mode {
 #define to_hi846(_sd) container_of(_sd, struct hi846, sd)
 
 static const struct hi846_reg hi846_init_regs[] = {
-//#include "hi846_init_android_patch.txt"
-#include "hi846_init_android_github1.txt"
+#include "hi846_init_android_patch.txt"
+//#include "hi846_init_android_github1.txt"
 };
 
 static const struct hi846_reg mode_640x480_regs[] = {
@@ -124,25 +86,29 @@ static const struct hi846_reg mode_640x480_regs[] = {
 };
 
 static const struct hi846_reg mode_1280x720_regs[] = {
-//#include "hi846_1280_android_patch.txt"
-#include "hi846_1280_android_github1.txt"
+#include "hi846_1280_android_patch.txt"
+//#include "hi846_1280_android_github1.txt"
+};
+
+static const struct hi846_reg mode_1920x1080_regs[] = {
+#include "hi846_1920_android_patch.txt"
 };
 
 static const struct hi846_reg mode_1632x1224_regs[] = {
-//#include "hi846_1632_android_patch.txt"
-#include "hi846_1632_android_github1.txt"
+#include "hi846_1632_android_patch.txt"
+//#include "hi846_1632_android_github1.txt"
 };
 
 static const struct hi846_reg mode_3264x2448_regs[] = {
-//#include "hi846_3264_android_patch.txt"
-#include "hi846_3264_android_github1.txt"
+#include "hi846_3264_android_patch.txt"
+//#include "hi846_3264_android_github1.txt"
 };
 
 static const struct hi846_reg hi846_set_test_pattern[] = {
-	{0x0A00, 0x0000},
-	{0x0a04, 0x0141},
-	{0x020a, 0x0700},
-	{0x0A00, 0x0100},
+	{0x0a00, 0x0000},
+	{0x0a04, 0x014b},
+	{0x020a, 0x0300},
+	{0x0a00, 0x0100},
 };
 
 static const char * const hi846_test_pattern_menu[] = {
@@ -159,7 +125,7 @@ static const char * const hi846_test_pattern_menu[] = {
 };
 
 static const s64 link_freq_menu_items[] = {
-	HI846_LINK_FREQ_200MHZ,
+	HI846_LINK_FREQ,
 };
 
 static const struct hi846_reg_list hi846_init_regs_list = {
@@ -174,37 +140,58 @@ static const struct hi846_reg_list hi846_set_test_pattern_regs_list = {
 
 /* hi846_valid_res */
 static const struct hi846_mode supported_modes[] = {
-#if 0
 	/* the reg_list does 2-lane settings */
 	{
 		.width = 640,
 		.height = 480,
 		.link_freq = 80000000,
-		.fps = 120,
+		.fps = 120, /* 120 ? */
 		.frame_len = 631,
+		.fll_def = 0x0277,
+		.fll_min = 0x0277,
+		.llp = 0x0ed8,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_640x480_regs),
 			.regs = mode_640x480_regs,
 		},
 	},
-#endif
 	{
 		.width = 1280,
 		.height = 720,
 		.link_freq = 200000000, /* 288000000 or 200000000 */
-		.fps = 30,
+		.fps = 90, /* 90 ? */
 		.frame_len = 842,
+		.fll_def = 0x034a,
+		.fll_min = 0x034a,
+		.llp = 0x0ed8,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
 			.regs = mode_1280x720_regs,
 		},
 	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.link_freq = 200000000,
+		.fps = 60,
+		.frame_len = 1263,
+		.fll_def = 1263,
+		.fll_min = 1263,
+		.llp = HI846_LINE_LENGTH,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920x1080_regs),
+			.regs = mode_1920x1080_regs,
+		},
+	},
 	{
 		.width = 1632,
 		.height = 1224,
 		.link_freq = 200000000,
 		.fps = 30,
 		.frame_len = 2526,
+		.fll_def = 2526,
+		.fll_min = 2526,
+		.llp = HI846_LINE_LENGTH,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_1632x1224_regs),
 			.regs = mode_1632x1224_regs,
@@ -216,6 +203,9 @@ static const struct hi846_mode supported_modes[] = {
 		.link_freq = 300000000,
 		.fps = 30,
 		.frame_len = 2526,
+		.fll_def = 2526,
+		.fll_min = 2526,
+		.llp = HI846_LINE_LENGTH,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_3264x2448_regs),
 			.regs = mode_3264x2448_regs,
@@ -253,10 +243,16 @@ struct hi846 {
 	/* V4L2 Controls */
 	struct v4l2_ctrl *link_freq;
 	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
 
 	/* Current mode */
 	const struct hi846_mode *cur_mode;
 
+	/* TODO cleanup mode struct */
+	/* TODO save copy of state values here */
+
 	/* To serialize asynchronus callbacks */
 	struct mutex mutex;
 
@@ -330,23 +326,20 @@ static int hi846_read_reg(struct hi846 *hi846, u16 reg, u8 *val)
 	return 0;
 }
 
-static int hi556_write_reg(struct hi846 *hi846, u16 reg, u16 len, u32 val)
+static int hi846_write_reg_16(struct hi846 *hi846, u16 reg, u16 val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
 	u8 buf[6];
 
-	if (len > 4)
-		return -EINVAL;
-
 	put_unaligned_be16(reg, buf);
-	put_unaligned_be32(val << 8 * (4 - len), buf + 2);
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
+	put_unaligned_be32(val << 8 * 2, buf + 2);
+	if (i2c_master_send(client, buf, 4) != 4)
 		return -EIO;
 
 	return 0;
 }
 
-static int hi556_write_reg_list(struct hi846 *hi846,
+static int hi846_write_reg_list(struct hi846 *hi846,
 				const struct hi846_reg_list *r_list)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
@@ -354,9 +347,8 @@ static int hi556_write_reg_list(struct hi846 *hi846,
 	int ret;
 
 	for (i = 0; i < r_list->num_of_regs; i++) {
-		ret = hi556_write_reg(hi846, r_list->regs[i].address,
-				      2,
-				      r_list->regs[i].val);
+		ret = hi846_write_reg_16(hi846, r_list->regs[i].address,
+					 r_list->regs[i].val);
 		if (ret) {
 			dev_err_ratelimited(&client->dev,
 					    "failed to write reg 0x%4.4x. error = %d",
@@ -443,9 +435,20 @@ static int hi846_set_ctrl(struct v4l2_ctrl *ctrl)
 	struct hi846 *hi846 = container_of(ctrl->handler,
 					     struct hi846, ctrl_handler);
 	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
+	s64 exposure_max;
 	int ret = 0;
-
-	/* V4L2 controls values will be applied only when power is already up */
+	u32 shutter, frame_len;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = hi846->cur_mode->height + ctrl->val -
+			       HI846_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(hi846->exposure,
+					 hi846->exposure->minimum,
+					 exposure_max, hi846->exposure->step,
+					 exposure_max);
+	}
 
 	ret = pm_runtime_get_sync(&client->dev);
 	if (ret < 0) {
@@ -457,18 +460,50 @@ static int hi846_set_ctrl(struct v4l2_ctrl *ctrl)
 
 	switch (ctrl->id) {
 	case V4L2_CID_ANALOGUE_GAIN:
+		dev_dbg(&client->dev, "%s: V4L2_CID_ANALOGUE_GAIN\n", __func__);
 		ret = hi846_write_reg(hi846, HI846_REG_ANALOG_GAIN, ctrl->val);
 		break;
 
 	case V4L2_CID_DIGITAL_GAIN:
+		dev_dbg(&client->dev, "%s: V4L2_CID_DIGITAL_GAIN\n", __func__);
 		ret = hi846_update_digital_gain(hi846, ctrl->val);
 		break;
-#if 0
+
 	case V4L2_CID_EXPOSURE:
-		/* TODO integ_time registers */
+		shutter = ctrl->val;
+		frame_len = hi846->cur_mode->frame_len;
+
+		dev_dbg(&client->dev, "%s: V4L2_CID_EXPOSURE: writing %d\n",
+			__func__, shutter);
+		if (shutter > frame_len - 6) { /* margin */
+			dev_dbg(&client->dev, "SHUTTER > FRAME_LENGTH %d - 6\n",
+				frame_len);
+
+			frame_len = shutter + 6;
+			if (frame_len > 0xffff) { /* max frame len */
+				frame_len = 0xffff;
+			}
+		}
+
+		if (shutter < 6)
+			shutter = 6;
+		if (shutter > (0xffff - 6))
+			shutter = 0xffff - 6;
+
+		ret = hi846_write_reg_16(hi846, HI846_REG_FLL,
+					 frame_len);
+		ret = hi846_write_reg_16(hi846, HI846_REG_EXPOSURE,
+					 shutter);
 		break;
-#endif
 
+	case V4L2_CID_VBLANK:
+		dev_dbg(&client->dev, "%s: V4L2_CID_VBLANK: writing %d+%d\n",
+			__func__,
+			hi846->cur_mode->height, ctrl->val);
+		/* Update FLL that meets expected vertical blanking */
+		ret = hi846_write_reg_16(hi846, HI846_REG_FLL,
+					 hi846->cur_mode->height + ctrl->val);
+		break;
 	case V4L2_CID_TEST_PATTERN:
 		ret = hi846_test_pattern(hi846, ctrl->val);
 		break;
@@ -491,6 +526,7 @@ static const struct v4l2_ctrl_ops hi846_ctrl_ops = {
 static int hi846_init_controls(struct hi846 *hi846)
 {
 	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
 	int ret;
 
 	ctrl_hdlr = &hi846->ctrl_handler;
@@ -500,18 +536,48 @@ static int hi846_init_controls(struct hi846 *hi846)
 
 	ctrl_hdlr->lock = &hi846->mutex;
 
+	hi846->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &hi846_ctrl_ops,
+						  V4L2_CID_LINK_FREQ,
+					ARRAY_SIZE(link_freq_menu_items) - 1,
+					0, link_freq_menu_items);
+	if (hi846->link_freq)
+		hi846->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
 	hi846->pixel_rate = v4l2_ctrl_new_std
 			    (ctrl_hdlr, &hi846_ctrl_ops,
 			     V4L2_CID_PIXEL_RATE, 0,
-			     to_pixel_rate(HI846_LINK_FREQ_437MHZ_INDEX),
+			     to_pixel_rate(HI846_LINK_FREQ_INDEX),
 			     1,
-			     to_pixel_rate(HI846_LINK_FREQ_437MHZ_INDEX));
+			     to_pixel_rate(HI846_LINK_FREQ_INDEX));
+	hi846->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+					  V4L2_CID_VBLANK,
+					  hi846->cur_mode->fll_min -
+					  hi846->cur_mode->height,
+					  HI846_FLL_MAX -
+					  hi846->cur_mode->height, 1,
+					  hi846->cur_mode->fll_def -
+					  hi846->cur_mode->height);
+
+	h_blank = hi846->cur_mode->llp - hi846->cur_mode->width;
+
+	hi846->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+					  V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					  h_blank);
+	if (hi846->hblank)
+		hi846->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
 	v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
 			  HI846_ANAL_GAIN_MIN, HI846_ANAL_GAIN_MAX,
 			  HI846_ANAL_GAIN_STEP, HI846_ANAL_GAIN_MIN);
 	v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
 			  HI846_DGTL_GAIN_MIN, HI846_DGTL_GAIN_MAX,
 			  HI846_DGTL_GAIN_STEP, HI846_DGTL_GAIN_DEFAULT);
+	exposure_max = hi846->cur_mode->fll_def - HI846_EXPOSURE_MAX_MARGIN;
+	hi846->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &hi846_ctrl_ops,
+					    V4L2_CID_EXPOSURE,
+					    HI846_EXPOSURE_MIN, exposure_max,
+					    HI846_EXPOSURE_STEP,
+					    exposure_max);
 	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &hi846_ctrl_ops,
 				     V4L2_CID_TEST_PATTERN,
 				     ARRAY_SIZE(hi846_test_pattern_menu) - 1,
@@ -527,14 +593,13 @@ static int hi846_init_controls(struct hi846 *hi846)
 static int hi846_start_streaming(struct hi846 *hi846)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&hi846->sd);
-	//int link_freq_index;
 	int ret;
 
 	ret = __v4l2_ctrl_handler_setup(hi846->sd.ctrl_handler);
 	if (ret)
 		return ret;
 
-#if 0
+#if 1
 	ret = hi846_write_reg(hi846, HI846_REG_MODE_SELECT,
 			      HI846_MODE_STREAMING);
 	if (ret) {
@@ -544,8 +609,8 @@ static int hi846_start_streaming(struct hi846 *hi846)
 	mdelay(10);
 #endif
 
-#if 1
-	ret = hi556_write_reg_list(hi846, &hi846_set_test_pattern_regs_list);
+#if 0
+	ret = hi846_write_reg_list(hi846, &hi846_set_test_pattern_regs_list);
 	if (ret) {
 		dev_err(&client->dev, "failed to set test pattern: %d\n", ret);
 		return ret;
@@ -659,7 +724,7 @@ static int __maybe_unused hi846_resume(struct device *dev)
 	if (ret < 0)
 		goto error_regulator;
 
-	msleep(500);
+	msleep(100);
 
 	//mutex_lock(&hi846->mutex);
 	if (hi846->streaming) {
@@ -702,11 +767,6 @@ static int hi846_set_frame_interval(struct v4l2_subdev *sd,
 		timeperframe->numerator = 1;
 	}
 
-/* FIXME */
-dev_dbg(&client->dev, "overwriting with default\n");
-timeperframe->denominator = DEFAULT_FPS;
-timeperframe->numerator = 1;
-
 	hi846->streamcap.timeperframe = f_interval->interval;
 
 	return 0;
@@ -722,6 +782,7 @@ static int hi846_get_frame_interval(struct v4l2_subdev *sd,
 
 	memset(f_interval, 0, sizeof(*f_interval));
 
+	dev_dbg(&client->dev, "%s: 1 / %d\n", __func__, hi846->streamcap.timeperframe.denominator);
 	f_interval->interval = hi846->streamcap.timeperframe;
 
 	return 0;
@@ -731,15 +792,18 @@ static int hi846_set_video_mode(struct hi846 *hi846, int fps)
 {
 	int frame_length;
 	int ret;
+	int dummy_lines;
 
 	frame_length = hi846->cur_mode->link_freq / fps / HI846_LINE_LENGTH;
-	if (frame_length < hi846->cur_mode->frame_len)
-		frame_length = hi846->cur_mode->frame_len;
+	dummy_lines = (frame_length > hi846->cur_mode->frame_len) ?
+			(frame_length - hi846->cur_mode->frame_len) : 0;
+
+	frame_length = hi846->cur_mode->frame_len + dummy_lines;
 
 	pr_debug("%s: frame length calculated: %d\n", __func__, frame_length);
 
-	ret = hi556_write_reg(hi846, 0x0006, 2, frame_length & 0xFFFF);
-	ret = hi556_write_reg(hi846, 0x0008, 2, HI846_LINE_LENGTH & 0xFFFF);
+	ret = hi846_write_reg_16(hi846, HI846_REG_FLL, frame_length & 0xFFFF);
+	ret = hi846_write_reg_16(hi846, HI846_REG_LLP, HI846_LINE_LENGTH & 0xFFFF);
 
 	return ret;
 }
@@ -750,12 +814,12 @@ static int hi846_set_format(struct v4l2_subdev *sd,
 {
 	struct hi846 *hi846 = to_hi846(sd);
 	struct v4l2_mbus_framefmt *mf = &format->format;
-	struct v4l2_fract *timeperframe = &hi846->streamcap.timeperframe;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct hi846_datafmt *fmt = hi846_find_datafmt(mf->code);
 	int capturemode;
 	u32 tgt_fps;
 	int ret;
+	s32 vblank_def, h_blank;
 
 	dev_dbg(&client->dev, "------------ starting %s ---------\n", __func__);
 	if (!fmt) {
@@ -764,8 +828,6 @@ static int hi846_set_format(struct v4l2_subdev *sd,
 		fmt = &hi846_colour_fmts[0];
 	}
 
-	mf->field = V4L2_FIELD_NONE;
-
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
 		return 0;
 
@@ -785,10 +847,14 @@ static int hi846_set_format(struct v4l2_subdev *sd,
 				      mf->height);
 	dev_dbg(&client->dev, "%s: found mode: %dx%d\n", __func__, hi846->cur_mode->width, hi846->cur_mode->height);
 
-	/* TODO then use cur_mode here */
-	tgt_fps = timeperframe->denominator / timeperframe->numerator;
+	tgt_fps = hi846->cur_mode->fps;
 	dev_dbg(&client->dev, "%s: target fps: %d\n", __func__, tgt_fps);
 
+	mf->width = hi846->cur_mode->width;
+	mf->height = hi846->cur_mode->height;
+	mf->code = HI846_MEDIA_BUS_FORMAT;
+	mf->field = V4L2_FIELD_NONE;
+
 	ret = pm_runtime_get_sync(&client->dev);
 	if (ret < 0) {
 		dev_err(&client->dev, "%s: pm_runtime_get failed: %d\n",
@@ -798,35 +864,41 @@ static int hi846_set_format(struct v4l2_subdev *sd,
 	}
 
 	dev_dbg(&client->dev, "writing regs now\n");
-	ret = hi556_write_reg_list(hi846, &hi846_init_regs_list);
+	ret = hi846_write_reg_list(hi846, &hi846_init_regs_list);
 	if (ret) {
 		dev_err(&client->dev, "failed to set plls: %d\n", ret);
 		return ret;
 	}
 
-	ret = hi556_write_reg_list(hi846, &hi846->cur_mode->reg_list);
+	ret = hi846_write_reg_list(hi846, &hi846->cur_mode->reg_list);
 	if (ret) {
 		dev_err(&client->dev, "failed to set mode: %d\n", ret);
 		return ret;
 	}
 
-#if 0
-	if (tgt_fps == 15)
-		hi846_capture_setting_normal_video_2(hi846);
-	else if (tgt_fps == 30)
-		hi846_capture_setting_normal_video(hi846);
-	else {
-		dev_err(&client->dev, "frame rate %d not supported\n", tgt_fps);
-		return -EINVAL;
-	}
-#endif
-
-	/* TODO line length pck and frame length lines */
 	hi846_set_video_mode(hi846, tgt_fps);
 
 	hi846->streamcap.capturemode = capturemode;
 	hi846->pix.width = mf->width;
 	hi846->pix.height = mf->height;
+
+	__v4l2_ctrl_s_ctrl(hi846->link_freq, HI846_LINK_FREQ_INDEX);
+	__v4l2_ctrl_s_ctrl_int64(hi846->pixel_rate,
+				 to_pixel_rate(HI846_LINK_FREQ_INDEX));
+
+	/* Update limits and set FPS to default */
+	vblank_def = hi846->cur_mode->fll_def - hi846->cur_mode->height;
+	__v4l2_ctrl_modify_range(hi846->vblank,
+				 hi846->cur_mode->fll_min - hi846->cur_mode->height,
+				 HI846_FLL_MAX - hi846->cur_mode->height, 1,
+				 vblank_def);
+	__v4l2_ctrl_s_ctrl(hi846->vblank, vblank_def);
+
+	h_blank = hi846->cur_mode->llp - hi846->cur_mode->width;
+
+	__v4l2_ctrl_modify_range(hi846->hblank, h_blank, h_blank, 1,
+				 h_blank);
+
 	dev_dbg(&client->dev, "Set format w=%d h=%d mode=%d code=0x%x colorspace=0x%x\n",
 		mf->width, mf->height, capturemode,
 		fmt->code, fmt->colorspace);
@@ -922,8 +994,9 @@ static int hi846_enum_frame_intervals(struct v4l2_subdev *sd,
 		return -EINVAL;
 
 	fie->interval.numerator = 1;
-/* TODO FIXME for index 1?*/
-	fie->interval.denominator = DEFAULT_FPS;
+	fie->interval.denominator = supported_modes[fie->index].fps;
+	dev_dbg(dev, "%s: index: %d: %d / %d\n", __func__, fie->index,
+		fie->interval.numerator, fie->interval.denominator);
 
 	return 0;
 }
@@ -957,26 +1030,18 @@ static const struct v4l2_subdev_ops hi846_subdev_ops = {
 	.pad = &hi846_pad_ops,
 };
 
-static void hi846_gpio_assert(struct hi846 *hi846, int id)
+static void hi846_gpio_assert(struct hi846 *hi846, int id, bool on)
 {
 	struct hi846_gpio *gpio = &hi846->gpios[id];
 	if (gpio == 0)
 		return;
 
-	gpio_set_value(gpio->gpio, gpio->level);
+	if (on)
+		gpio_set_value(gpio->gpio, gpio->level);
+	else
+		gpio_set_value(gpio->gpio, !gpio->level);
 }
 
-#if 0
-static void hi846_gpio_deassert(struct hi846 *hi846, int id)
-{
-	struct hi846_gpio *gpio = &hi846->gpios[id];
-	if (gpio == 0)
-		return;
-
-	gpio_set_value(gpio->gpio, !gpio->level);
-}
-#endif
-
 static int hi846_parse_gpios(struct hi846_gpio *gpios, struct device *dev)
 {
 	static const char * const names[] = {
@@ -1130,7 +1195,7 @@ static int hi846_probe(struct i2c_client *client)
 	if (ret < 0)
 		goto probe_error_regulator;
 
-	msleep(500);
+	msleep(100);
 
 	hi846->streamcap.capability = V4L2_MODE_HIGHQUALITY |
 					V4L2_CAP_TIMEPERFRAME;
@@ -1138,7 +1203,7 @@ static int hi846_probe(struct i2c_client *client)
 	hi846->streamcap.timeperframe.denominator = DEFAULT_FPS;
 	hi846->streamcap.timeperframe.numerator = 1;
 
-	hi846_gpio_assert(hi846, RST);
+	hi846_gpio_assert(hi846, RST, true);
 
 	ret = hi846_identify_module(hi846);
 	if (ret) {
@@ -1167,7 +1232,7 @@ static int hi846_probe(struct i2c_client *client)
 	pm_runtime_set_active(&client->dev);
 	pm_runtime_enable(&client->dev);
 
-	pm_runtime_set_autosuspend_delay(&client->dev, 3000);
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
 	pm_runtime_use_autosuspend(&client->dev);
 
 	return 0;
@@ -1207,5 +1272,6 @@ static struct i2c_driver hi846_i2c_driver = {
 module_i2c_driver(hi846_i2c_driver);
 
 MODULE_AUTHOR("Angus Ainslie <angus@akkea.ca>");
+MODULE_AUTHOR("Martin Kepplinger <martin.kepplinger@puri.sm>");
 MODULE_DESCRIPTION("Hynix HI846 sensor driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/hi846.h b/drivers/media/i2c/hi846.h
new file mode 100644
index 0000000000000000000000000000000000000000..e12da0ba3cdc8e1f1617b4d19030e08dee1fd202
--- /dev/null
+++ b/drivers/media/i2c/hi846.h
@@ -0,0 +1,175 @@
+/* Frame length lines / Vertical timings */
+#define HI846_REG_FLL			0x0006
+#define HI846_FLL_MAX			0xffff
+
+/* Horizontal timing */
+#define HI846_REG_LLP			0x0008
+#define HI846_LINE_LENGTH		3800
+
+#define HI846_REG_BINNING_MODE		0x000c
+
+#define HI846_REG_IMAGE_ORIENTATION	0x000e
+
+#define HI846_REG_UNKNOWN_0022		0x0022
+
+#define HI846_REG_Y_ADDR_START_VACT_H	0x0026
+#define HI846_REG_Y_ADDR_START_VACT_L	0x0027
+#define HI846_REG_UNKNOWN_0028		0x0028
+
+#define HI846_REG_Y_ADDR_END_VACT_H	0x002c
+#define HI846_REG_Y_ADDR_END_VACT_L	0x002d
+
+#define HI846_REG_Y_ODD_INC_FOBP	0x002e
+#define HI846_REG_Y_EVEN_INC_FOBP	0x002f
+
+#define HI846_REG_Y_ODD_INC_VACT	0x0032
+#define HI846_REG_Y_EVEN_INC_VACT	0x0033
+
+#define HI846_REG_GROUPED_PARA_HOLD	0x0046
+
+#define HI846_REG_TG_ENABLE		0x004c
+
+#define HI846_REG_UNKNOWN_005C		0x005c
+
+#define HI846_REG_UNKNOWN_006A		0x006a
+
+/* Long exposure time. Actually, 4 bits of 0x0073 too */
+#define HI846_REG_EXPOSURE		0x0074
+#define HI846_EXPOSURE_MIN		6
+#define HI846_EXPOSURE_MAX_MARGIN	2
+#define HI846_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define HI846_REG_ANALOG_GAIN		0x0076
+#define HI846_ANAL_GAIN_MIN		0
+#define HI846_ANAL_GAIN_MAX		240
+#define HI846_ANAL_GAIN_STEP		8
+
+/* Digital gain controls from sensor */
+#define HI846_REG_MWB_GR_GAIN_H		0x0078
+#define HI846_REG_MWB_GR_GAIN_L		0x0079
+#define HI846_REG_MWB_GB_GAIN_H		0x007a
+#define HI846_REG_MWB_GB_GAIN_L		0x007b
+#define HI846_REG_MWB_R_GAIN_H		0x007c
+#define HI846_REG_MWB_R_GAIN_L		0x007d
+#define HI846_REG_MWB_B_GAIN_H		0x007e
+#define HI846_REG_MWB_B_GAIN_L		0x007f
+#define HI846_DGTL_GAIN_MIN		0
+#define HI846_DGTL_GAIN_MAX		8191
+#define HI846_DGTL_GAIN_STEP		1
+#define HI846_DGTL_GAIN_DEFAULT		256
+
+#define HI846_REG_X_ADDR_START_HACT_H	0x0120
+#define HI846_REG_X_ADDR_END_HACT_H	0x0122
+
+#define HI846_REG_UNKNOWN_012A		0x012a
+
+#define HI846_REG_UNKNOWN_0200		0x0200
+
+#define HI846_REG_UNKNOWN_021C		0x021c
+#define HI846_REG_UNKNOWN_021E		0x021e
+
+#define HI846_REG_UNKNOWN_0402		0x0402
+#define HI846_REG_UNKNOWN_0404		0x0404
+#define HI846_REG_UNKNOWN_0408		0x0408
+#define HI846_REG_UNKNOWN_0410		0x0410
+#define HI846_REG_UNKNOWN_0412		0x0412
+#define HI846_REG_UNKNOWN_0414		0x0414
+
+#define HI846_REG_UNKNOWN_0418		0x0418
+
+#define HI846_REG_UNKNOWN_051E		0x051e
+
+/* Formatter */
+#define HI846_REG_X_START_H		0x0804
+#define HI846_REG_X_START_L		0x0805
+
+/* MIPI */
+#define HI846_REG_UNKNOWN_0900		0x0900
+#define HI846_REG_MIPI_TX_OP_EN		0x0901
+#define HI846_REG_MIPI_TX_OP_MODE	0x0902
+
+#define HI846_REG_UNKNOWN_090C		0x090c
+#define HI846_REG_UNKNOWN_090E		0x090e
+
+#define HI846_REG_UNKNOWN_0914		0x0914
+#define HI846_REG_TLPX			0x0915
+#define HI846_REG_TCLK_PREPARE		0x0916
+#define HI846_REG_TCLK_ZERO		0x0917
+#define HI846_REG_UNKNOWN_0918		0x0918
+#define HI846_REG_THS_PREPARE		0x0919
+#define HI846_REG_THS_ZERO		0x091a
+#define HI846_REG_THS_TRAIL		0x091b
+#define HI846_REG_TCLK_POST		0x091c
+#define HI846_REG_TCLK_TRAIL_MIN	0x091d
+#define HI846_REG_UNKNOWN_091E		0x091e
+
+#define HI846_REG_UNKNOWN_0954		0x0954
+#define HI846_REG_UNKNOWN_0956		0x0956
+#define HI846_REG_UNKNOWN_0958		0x0958
+#define HI846_REG_UNKNOWN_095A		0x095a
+
+/* ISP Common */
+#define HI846_REG_MODE_SELECT		0x0a00
+#define HI846_MODE_STANDBY		0x00
+#define HI846_MODE_STREAMING		0x01
+#define HI846_REG_FAST_STANDBY_MODE	0x0a02
+#define HI846_REG_ISP_EN_H		0x0a04
+
+/* Test Pattern Control */
+#define HI846_REG_ISP			0x0a05
+#define HI846_REG_ISP_TPG_EN		0x01
+#define HI846_REG_TEST_PATTERN		0x020a /* 1-9 */
+
+#define HI846_REG_UNKNOWN_0A0C		0x0a0c
+
+/* Windowing */
+#define HI846_REG_X_OUTPUT_SIZE_H	0x0a12
+#define HI846_REG_X_OUTPUT_SIZE_L	0x0a13
+#define HI846_REG_Y_OUTPUT_SIZE_H	0x0a14
+#define HI846_REG_Y_OUTPUT_SIZE_L	0x0a15
+
+/* ISP Common */
+#define HI846_REG_PEDESTAL_EN		0x0a1a
+
+#define HI846_REG_UNKNOWN_0A1E		0x0a1e
+
+/* Horizontal Binning Mode */
+#define HI846_REG_HBIN_MODE		0x0a22
+
+#define HI846_REG_UNKNOWN_0A24		0x0a24
+#define HI846_REG_UNKNOWN_0B02		0x0b02
+#define HI846_REG_UNKNOWN_0B10		0x0b10
+#define HI846_REG_UNKNOWN_0B12		0x0b12
+#define HI846_REG_UNKNOWN_0B14		0x0b14
+
+/* BLC (Black Level Calibration) */
+#define HI846_REG_BLC_CTL0		0x0c00
+
+#define HI846_REG_UNKNOWN_0C06		0x0c06
+#define HI846_REG_UNKNOWN_0C10		0x0c10
+#define HI846_REG_UNKNOWN_0C12		0x0c12
+#define HI846_REG_UNKNOWN_0C14		0x0c14
+#define HI846_REG_UNKNOWN_0C16		0x0c16
+
+#define HI846_REG_UNKNOWN_0E04		0x0e04
+
+#define HI846_REG_CHIP_ID_L		0x0f16
+#define HI846_REG_CHIP_ID_H		0x0f17
+#define HI846_CHIP_ID_L			0x46
+#define HI846_CHIP_ID_H			0x08
+
+#define HI846_REG_UNKNOWN_0F04		0x0f04
+#define HI846_REG_UNKNOWN_0F08		0x0f08
+
+/* PLL */
+#define HI846_REG_PLL_CFG_MIPI2_H	0x0f2a
+#define HI846_REG_PLL_CFG_MIPI2_L	0x0f2b
+
+#define HI846_REG_UNKNOWN_0F30		0x0f30
+#define HI846_REG_PLL_CFG_RAMP1_H	0x0f32
+#define HI846_REG_UNKNOWN_0F36		0x0f36
+#define HI846_REG_PLL_CFG_MIPI1_H	0x0f38
+
+#define HI846_REG_UNKNOWN_2008		0x2008
+#define HI846_REG_UNKNOWN_326E		0x326e
diff --git a/drivers/media/i2c/hi846_1280_android_patch.txt b/drivers/media/i2c/hi846_1280_android_patch.txt
index 533965922cde2b872f4a66ded496344b85875d93..eed15f69f388863feccede95e75f9ec8ce273b17 100644
--- a/drivers/media/i2c/hi846_1280_android_patch.txt
+++ b/drivers/media/i2c/hi846_1280_android_patch.txt
@@ -1,39 +1,39 @@
 /* from android patch (2lane) */
-  {0x0A00, 0x0000},
-  {0x002E, 0x3311},
-  {0x0032, 0x3311},
-  {0x0026, 0x0238},
-  {0x002C, 0x07D7},
-  {0x005C, 0x4202},
-  {0x0006, 0x034A},
-  {0x0008, 0x0ED8},
-  {0x000C, 0x0122},
-  {0x0A22, 0x0100},
-  {0x0A24, 0x0000},
-  {0x0804, 0x00B0},
-  {0x0A12, 0x0500},
-  {0x0A14, 0x02D0},
-  {0x0A04, 0x017A},
-  {0x0418, 0x0410},
-  {0x0B02, 0xE04D},
-  {0x0B10, 0x6C21},
-  {0x0B12, 0x0120},
-  {0x0B14, 0x0005},
-  {0x2008, 0x38FD},
-  {0x326E, 0x0000},
-  {0x0900, 0x0300},
-  {0x0902, 0x4319},
-  {0x0914, 0xC109},
-  {0x0916, 0x061A},
-  {0x0918, 0x0407},
-  {0x091A, 0x0A0B},
-  {0x091C, 0x0E08},
-  {0x091E, 0x0A00},
-  {0x090C, 0x0427},
-  {0x090E, 0x0145},
-  {0x0954, 0x0089},
-  {0x0956, 0x0000},
-  {0x0958, 0xCA80},
-  {0x095A, 0x9240},
-  {0x0F2A, 0x4124},
-  {0x004C, 0x0100},
+  {HI846_REG_MODE_SELECT,		0x0000},
+  {HI846_REG_Y_ODD_INC_FOBP,		0x3311},
+  {HI846_REG_Y_ODD_INC_VACT,		0x3311},
+  {HI846_REG_Y_ADDR_START_VACT_H,	0x0238},
+  {HI846_REG_Y_ADDR_END_VACT_H,		0x07D7},
+  {HI846_REG_UNKNOWN_005C,		0x4202},
+  {HI846_REG_FLL,			0x034A},
+  {HI846_REG_LLP,			0x0ED8},
+  {HI846_REG_BINNING_MODE,		0x0022},
+  {HI846_REG_HBIN_MODE,			0x0000},
+  {HI846_REG_UNKNOWN_0A24,		0x0000},
+  {HI846_REG_X_START_H,			0x00B0},
+  {HI846_REG_X_OUTPUT_SIZE_H,		0x0500},
+  {HI846_REG_Y_OUTPUT_SIZE_H,		0x02D0},
+  {HI846_REG_ISP_EN_H,			0x017A},
+  {HI846_REG_UNKNOWN_0418,		0x0410},
+  {HI846_REG_UNKNOWN_0B02,		0xE04D},
+  {HI846_REG_UNKNOWN_0B10,		0x6C21},
+  {HI846_REG_UNKNOWN_0B12,		0x0120},
+  {HI846_REG_UNKNOWN_0B14,		0x0005},
+  {HI846_REG_UNKNOWN_2008,		0x38FD},
+  {HI846_REG_UNKNOWN_326E,		0x0000},
+  {HI846_REG_UNKNOWN_0900,		0x0300},
+  {HI846_REG_MIPI_TX_OP_MODE,		0x4319},
+  {HI846_REG_UNKNOWN_0914,		0xC109},
+  {HI846_REG_TCLK_PREPARE,		0x061A},
+  {HI846_REG_UNKNOWN_0918,		0x0407},
+  {HI846_REG_THS_ZERO,			0x0A0B},
+  {HI846_REG_TCLK_POST,			0x0E08},
+  {HI846_REG_UNKNOWN_091E,		0x0A00},
+  {HI846_REG_UNKNOWN_090C,		0x0427},
+  {HI846_REG_UNKNOWN_090E,		0x0145},
+  {HI846_REG_UNKNOWN_0954,		0x0089},
+  {HI846_REG_UNKNOWN_0956,		0x0000},
+  {HI846_REG_UNKNOWN_0958,		0xCA80},
+  {HI846_REG_UNKNOWN_095A,		0x9240},
+  {HI846_REG_PLL_CFG_MIPI2_H,		0x4124},
+  {HI846_REG_TG_ENABLE,			0x0100},
diff --git a/drivers/media/i2c/hi846_1920_android_patch.txt b/drivers/media/i2c/hi846_1920_android_patch.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bea83cbd7f0573d0f71ca34970c43e8d278b65ea
--- /dev/null
+++ b/drivers/media/i2c/hi846_1920_android_patch.txt
@@ -0,0 +1,38 @@
+  {0x0A00, 0x0000},
+  {0x002E, 0x1111},
+  {0x0032, 0x1111},
+  {0x0026, 0x02EC},
+  {0x002C, 0x0723},
+  {0x005C, 0x2101},
+  {0x0006, 0x04EF},
+  {0x0008, 0x0ED8},
+  {0x000C, 0x0022},
+  {0x0A22, 0x0000},
+  {0x0A24, 0x0000},
+  {0x0804, 0x02A0},
+  {0x0A12, 0x0780},
+  {0x0A14, 0x0438},
+  {0x0A04, 0x015A},
+  {0x0418, 0x04C4},
+  {0x0B02, 0xE04D},
+  {0x0B10, 0x6821},
+  {0x0B12, 0x0120},
+  {0x0B14, 0x0001},
+  {0x2008, 0x38FD},
+  {0x326E, 0x0000},
+  {0x0900, 0x0300},
+  {0x0902, 0x631A},
+  {0x0914, 0xC109},
+  {0x0916, 0x061A},
+  {0x0918, 0x0306},
+  {0x091A, 0x0B09},
+  {0x091C, 0x0C07},
+  {0x091E, 0x0A00},
+  {0x090C, 0x042A},
+  {0x090E, 0x020F},
+  {0x0954, 0x0089},
+  {0x0956, 0x0000},
+  {0x0958, 0xCA00},
+  {0x095A, 0x9240},
+  {0x0F2A, 0x0024},
+  {0x004C, 0x0100},
diff --git a/drivers/media/i2c/hi846_640_android_patch.txt b/drivers/media/i2c/hi846_640_android_patch.txt
index 062e82bdbbc285eb73539234e141149524c8a9f7..5b3295a48a77295dc9966261117fa76180fb7a03 100644
--- a/drivers/media/i2c/hi846_640_android_patch.txt
+++ b/drivers/media/i2c/hi846_640_android_patch.txt
@@ -1,39 +1,39 @@
 /* from android patch (2lane) */
-  {0x0A00, 0x0000},
-  {0x002E, 0x7711},
-  {0x0032, 0x7711},
-  {0x0026, 0x0148},
-  {0x002C, 0x08C7},
-  {0x005C, 0x4404},
-  {0x0006, 0x0277},
-  {0x0008, 0x0ED8},
-  {0x000C, 0x0322},
-  {0x0A22, 0x0200},
-  {0x0A24, 0x0000},
-  {0x0804, 0x0058},
-  {0x0A12, 0x0280},
-  {0x0A14, 0x01E0},
-  {0x0A04, 0x017A},
-  {0x0418, 0x0210},
-  {0x0B02, 0xE04D},
-  {0x0B10, 0x7021},
-  {0x0B12, 0x0120},
-  {0x0B14, 0x0001},
-  {0x2008, 0x38FD},
-  {0x326E, 0x0000},
-  {0x0900, 0x0300},
-  {0x0902, 0x4319},
-  {0x0914, 0xC105},
-  {0x0916, 0x030C},
-  {0x0918, 0x0304},
-  {0x091A, 0x0708},
-  {0x091C, 0x0B04},
-  {0x091E, 0x0500},
-  {0x090C, 0x0208},
-  {0x090E, 0x009A},
-  {0x0954, 0x0089},
-  {0x0956, 0x0000},
-  {0x0958, 0xCA80},
-  {0x095A, 0x9240},
-  {0x0F2A, 0x4924},
-  {0x004C, 0x0100},
+  {HI846_REG_MODE_SELECT,		0x0000},
+  {HI846_REG_Y_ODD_INC_FOBP,		0x7711},
+  {HI846_REG_Y_ODD_INC_VACT,		0x7711},
+  {HI846_REG_Y_ADDR_START_VACT_H,	0x0148},
+  {HI846_REG_Y_ADDR_END_VACT_H,		0x08C7},
+  {HI846_REG_UNKNOWN_005C,		0x4404},
+  {HI846_REG_FLL,			0x0277},
+  {HI846_REG_LLP,			0x0ED8},
+  {HI846_REG_BINNING_MODE,		0x0022},
+  {HI846_REG_HBIN_MODE,			0x0000},
+  {HI846_REG_UNKNOWN_0A24,		0x0000},
+  {HI846_REG_X_START_H,			0x0058},
+  {HI846_REG_X_OUTPUT_SIZE_H,		0x0280},
+  {HI846_REG_Y_OUTPUT_SIZE_H,		0x01E0},
+  {HI846_REG_ISP_EN_H,			0x017A},
+  {HI846_REG_UNKNOWN_0418,		0x0210},
+  {HI846_REG_UNKNOWN_0B02,		0xE04D},
+  {HI846_REG_UNKNOWN_0B10,		0x7021},
+  {HI846_REG_UNKNOWN_0B12,		0x0120},
+  {HI846_REG_UNKNOWN_0B14,		0x0001},
+  {HI846_REG_UNKNOWN_2008,		0x38FD},
+  {HI846_REG_UNKNOWN_326E,		0x0000},
+  {HI846_REG_UNKNOWN_0900,		0x0300},
+  {HI846_REG_MIPI_TX_OP_MODE,		0x4319},
+  {HI846_REG_UNKNOWN_0914,		0xC105},
+  {HI846_REG_TCLK_PREPARE,		0x030C},
+  {HI846_REG_UNKNOWN_0918,		0x0304},
+  {HI846_REG_THS_ZERO,			0x0708},
+  {HI846_REG_TCLK_POST,			0x0B04},
+  {HI846_REG_UNKNOWN_091E,		0x0500},
+  {HI846_REG_UNKNOWN_090C,		0x0208},
+  {HI846_REG_UNKNOWN_090E,		0x009A},
+  {HI846_REG_UNKNOWN_0954,		0x0089},
+  {HI846_REG_UNKNOWN_0956,		0x0000},
+  {HI846_REG_UNKNOWN_0958,		0xCA80},
+  {HI846_REG_UNKNOWN_095A,		0x9240},
+  {HI846_REG_PLL_CFG_MIPI2_H,		0x4924},
+  {HI846_REG_TG_ENABLE,			0x0100},
diff --git a/drivers/media/i2c/hi846_init_android_patch.txt b/drivers/media/i2c/hi846_init_android_patch.txt
index ce9f682867e19da1eb1f7a667d4a3c0e57e7ba00..2891d947b9a101baa6fbc0909c0a83f1ab24847b 100644
--- a/drivers/media/i2c/hi846_init_android_patch.txt
+++ b/drivers/media/i2c/hi846_init_android_patch.txt
@@ -1,5 +1,6 @@
 /* from android lib patch */
-  {0x0A00, 0x0000},
+  {HI846_REG_MODE_SELECT,		0x0000},
+/* regs below are unknown */
   {0x2000, 0x100A},
   {0x2002, 0x00FF},
   {0x2004, 0x0007},
@@ -136,78 +137,79 @@
   {0x3312, 0x0000},
   {0x3314, 0xFC94},
   {0x3316, 0xC3D8},
-  {0x0A00, 0x0000},
-  {0x0E04, 0x0012},
-  {0x002E, 0x1111},
-  {0x0032, 0x1111},
-  {0x0022, 0x0008},
-  {0x0026, 0x0040},
-  {0x0028, 0x0017},
-  {0x002C, 0x09CF},
-  {0x005C, 0x2101},
-  {0x0006, 0x09DE},
-  {0x0008, 0x0ED8},
-  {0x000E, 0x0100},
-  {0x000C, 0x0022},
-  {0x0A22, 0x0000},
-  {0x0A24, 0x0000},
-  {0x0804, 0x0000},
-  {0x0A12, 0x0CC0},
-  {0x0A14, 0x0990},
-  {0x0074, 0x09D8},
-  {0x0076, 0x0000},
-  {0x0046, 0x0000},
-  {0x051E, 0x0000},
-  {0x0200, 0x0400},
-  {0x0A1A, 0x0C00},
-  {0x0A0C, 0x0010},
-  {0x0A1E, 0x0CCF},
-  {0x0402, 0x0110},
-  {0x0404, 0x00F4},
-  {0x0408, 0x0000},
-  {0x0410, 0x008D},
-  {0x0412, 0x011A},
-  {0x0414, 0x864C},
-  {0x021C, 0x0003},
-  {0x021E, 0x0235},
-  {0x0C00, 0x9150},
-  {0x0C06, 0x0021},
-  {0x0C10, 0x0040},
-  {0x0C12, 0x0040},
-  {0x0C14, 0x0040},
-  {0x0C16, 0x0040},
-  {0x0A02, 0x0100},
-  {0x0A04, 0x014A},
-  {0x0418, 0x0000},
-  {0x012A, 0x03B4},
-  {0x0120, 0x0046},
-  {0x0122, 0x0376},
-  {0x0B02, 0xE04D},
-  {0x0B10, 0x6821},
-  {0x0B12, 0x0120},
-  {0x0B14, 0x0001},
-  {0x2008, 0x38FD},
-  {0x326E, 0x0000},
-  {0x0900, 0x0320},
-  {0x0902, 0xC31A},
-  {0x0914, 0xC109},
-  {0x0916, 0x061A},
-  {0x0918, 0x0306},
-  {0x091A, 0x0B09},
-  {0x091C, 0x0C07},
-  {0x091E, 0x0A00},
-  {0x090C, 0x042A},
-  {0x090E, 0x006B},
-  {0x0954, 0x0089},
-  {0x0956, 0x0000},
-  {0x0958, 0xCA00},
-  {0x095A, 0x9240},
-  {0x0F08, 0x2F04},
-  {0x0F30, 0x001F},
-  {0x0F36, 0x001F},
-  {0x0F04, 0x3A00},
-  {0x0F32, 0x025A},
-  {0x0F38, 0x025A},
-  {0x0F2A, 0x0024},
-  {0x006A, 0x0100},
-  {0x004C, 0x0100},
+/* regs above are unknown */
+  {HI846_REG_MODE_SELECT,		0x0000},
+  {HI846_REG_UNKNOWN_0E04,		0x0012},
+  {HI846_REG_Y_ODD_INC_FOBP,		0x1111},
+  {HI846_REG_Y_ODD_INC_VACT,		0x1111},
+  {HI846_REG_UNKNOWN_0022,		0x0008},
+  {HI846_REG_Y_ADDR_START_VACT_H,	0x0040},
+  {HI846_REG_UNKNOWN_0028,		0x0017},
+  {HI846_REG_Y_ADDR_END_VACT_H,		0x09CF},
+  {HI846_REG_UNKNOWN_005C,		0x2101},
+  {HI846_REG_FLL,			0x09DE},
+  {HI846_REG_LLP,			0x0ED8},
+  {HI846_REG_IMAGE_ORIENTATION,		0x0100},
+  {HI846_REG_BINNING_MODE,		0x0022}, /* 0x22 in 0x000d unknown */
+  {HI846_REG_HBIN_MODE,			0x0000},
+  {HI846_REG_UNKNOWN_0A24,		0x0000},
+  {HI846_REG_X_START_H,			0x0000},
+  {HI846_REG_X_OUTPUT_SIZE_H,		0x0CC0},
+  {HI846_REG_Y_OUTPUT_SIZE_H,		0x0990},
+  {HI846_REG_EXPOSURE,			0x09D8},
+  {HI846_REG_ANALOG_GAIN,		0x0000},
+  {HI846_REG_GROUPED_PARA_HOLD,		0x0000},
+  {HI846_REG_UNKNOWN_051E,		0x0000},
+  {HI846_REG_UNKNOWN_0200,		0x0400},
+  {HI846_REG_PEDESTAL_EN,		0x0C00},
+  {HI846_REG_UNKNOWN_0A0C,		0x0010},
+  {HI846_REG_UNKNOWN_0A1E,		0x0CCF},
+  {HI846_REG_UNKNOWN_0402,		0x0110},
+  {HI846_REG_UNKNOWN_0404,		0x00F4},
+  {HI846_REG_UNKNOWN_0408,		0x0000},
+  {HI846_REG_UNKNOWN_0410,		0x008D},
+  {HI846_REG_UNKNOWN_0412,		0x011A},
+  {HI846_REG_UNKNOWN_0414,		0x864C},
+  {HI846_REG_UNKNOWN_021C,		0x0003},
+  {HI846_REG_UNKNOWN_021E,		0x0235},
+  {HI846_REG_BLC_CTL0,			0x9150},
+  {HI846_REG_UNKNOWN_0C06,		0x0021},
+  {HI846_REG_UNKNOWN_0C10,		0x0040},
+  {HI846_REG_UNKNOWN_0C12,		0x0040},
+  {HI846_REG_UNKNOWN_0C14,		0x0040},
+  {HI846_REG_UNKNOWN_0C16,		0x0040},
+  {HI846_REG_FAST_STANDBY_MODE,		0x0100},
+  {HI846_REG_ISP_EN_H,			0x014A},
+  {HI846_REG_UNKNOWN_0418,		0x0000},
+  {HI846_REG_UNKNOWN_012A,		0x03B4},
+  {HI846_REG_X_ADDR_START_HACT_H,	0x0046},
+  {HI846_REG_X_ADDR_END_HACT_H,		0x0376},
+  {HI846_REG_UNKNOWN_0B02,		0xE04D},
+  {HI846_REG_UNKNOWN_0B10,		0x6821},
+  {HI846_REG_UNKNOWN_0B12,		0x0120},
+  {HI846_REG_UNKNOWN_0B14,		0x0001},
+  {HI846_REG_UNKNOWN_2008,		0x38FD},
+  {HI846_REG_UNKNOWN_326E,		0x0000},
+  {HI846_REG_UNKNOWN_0900,		0x0320}, /* 2-lane merge mode enable in 0x0901 */
+  {HI846_REG_MIPI_TX_OP_MODE,		0xC31A}, /* 0x1a in 0x0903 unknown */
+  {HI846_REG_UNKNOWN_0914,		0xC109}, /* 0x09 in 0x0915 tlpx */
+  {HI846_REG_TCLK_PREPARE,		0x061A},
+  {HI846_REG_UNKNOWN_0918,		0x0306}, /* 0x06 in 0x0919 ths prepare */
+  {HI846_REG_THS_ZERO,			0x0B09},
+  {HI846_REG_TCLK_POST,			0x0C07},
+  {HI846_REG_UNKNOWN_091E,		0x0A00},
+  {HI846_REG_UNKNOWN_090C,		0x042A},
+  {HI846_REG_UNKNOWN_090E,		0x006B},
+  {HI846_REG_UNKNOWN_0954,		0x0089},
+  {HI846_REG_UNKNOWN_0956,		0x0000},
+  {HI846_REG_UNKNOWN_0958,		0xCA00},
+  {HI846_REG_UNKNOWN_095A,		0x9240},
+  {HI846_REG_UNKNOWN_0F08,		0x2F04},
+  {HI846_REG_UNKNOWN_0F30,		0x001F},
+  {HI846_REG_UNKNOWN_0F36,		0x001F},
+  {HI846_REG_UNKNOWN_0F04,		0x3A00},
+  {HI846_REG_PLL_CFG_RAMP1_H,		0x025A},
+  {HI846_REG_PLL_CFG_MIPI1_H,		0x025A},
+  {HI846_REG_PLL_CFG_MIPI2_H,		0x0024},
+  {HI846_REG_UNKNOWN_006A,		0x0100},
+  {HI846_REG_TG_ENABLE,			0x0100},
diff --git a/drivers/media/platform/mxc/capture/mx6s_capture.c b/drivers/media/platform/mxc/capture/mx6s_capture.c
index 86b2f9cfaa7974ae585582bb8a738d9de72a887f..2b9fe9195afc1400a7ae98bee965ee8f1d5eb014 100644
--- a/drivers/media/platform/mxc/capture/mx6s_capture.c
+++ b/drivers/media/platform/mxc/capture/mx6s_capture.c
@@ -130,11 +130,15 @@
 
 /* csi control reg 18 */
 #define BIT_CSI_ENABLE			(0x1 << 31)
-#define BIT_MIPI_DATA_FORMAT_RAW8		(0x2a << 25)
-#define BIT_MIPI_DATA_FORMAT_RAW10		(0x2b << 25)
-#define BIT_MIPI_DATA_FORMAT_YUV422_8B	(0x1e << 25)
+#define BIT_MIPI_DATA_FORMAT_YUV420_8B	(0x18 << 25)
+#define BIT_MIPI_DATA_FORMAT_YUV420_8BL	(0x1A << 25)
+#define BIT_MIPI_DATA_FORMAT_YUV420_8BC	(0x1C << 25)
+#define BIT_MIPI_DATA_FORMAT_YUV422_8B	(0x1E << 25)
+#define BIT_MIPI_DATA_FORMAT_RAW8	(0x2A << 25)
+#define BIT_MIPI_DATA_FORMAT_RAW10	(0x2B << 25)
 #define BIT_MIPI_DATA_FORMAT_MASK	(0x3F << 25)
 #define BIT_MIPI_DATA_FORMAT_OFFSET	25
+#define BIT_LINE_STRIDE_EN		(0x1 << 24)
 #define BIT_DATA_FROM_MIPI		(0x1 << 22)
 #define BIT_MIPI_YU_SWAP		(0x1 << 21)
 #define BIT_MIPI_DOUBLE_CMPNT	(0x1 << 20)
@@ -267,6 +271,12 @@ static struct mx6s_fmt formats[] = {
 		.pixelformat	= V4L2_PIX_FMT_SBGGR8,
 		.mbus_code	= MEDIA_BUS_FMT_SBGGR8_1X8,
 		.bpp		= 1,
+	}, {
+		.name		= "RAWRGB10 (SBGGR10)",
+		.fourcc		= V4L2_PIX_FMT_SBGGR10,
+		.pixelformat	= V4L2_PIX_FMT_SBGGR10,
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR10_1X10,
+		.bpp		= 1,
 	}
 };
 
@@ -601,7 +611,7 @@ static void csi_dmareq_rff_enable(struct mx6s_csi_dev *csi_dev)
 		dev_dbg(csi_dev->dev, "%s: UYVY/YUYV\n", __func__);
 		break;
 	case V4L2_PIX_FMT_SBGGR8:
-		pr_debug("mx6s: %s: RAW8 bayer\n", __func__);
+	case V4L2_PIX_FMT_SBGGR10:
 		/* Bayer Starting point GR/RG/BG/GB [20:19] */
 		/* BG */
 		cr2 |= 0x00100000;
@@ -891,6 +901,7 @@ static int mx6s_configure_csi(struct mx6s_csi_dev *csi_dev)
 	switch (csi_dev->fmt->pixelformat) {
 	case V4L2_PIX_FMT_YUV32:
 	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SBGGR10:
 		width = pix->width;
 		break;
 	case V4L2_PIX_FMT_UYVY:
@@ -915,7 +926,6 @@ static int mx6s_configure_csi(struct mx6s_csi_dev *csi_dev)
 	if (csi_dev->csi_mipi_mode == true) {
 		cr1 = csi_read(csi_dev, CSI_CSICR1);
 		cr1 &= ~BIT_GCLK_MODE;
-		csi_write(csi_dev, cr1, CSI_CSICR1);
 
 		cr18 = csi_read(csi_dev, CSI_CSICR18);
 		cr18 &= ~BIT_MIPI_DATA_FORMAT_MASK;
@@ -930,12 +940,16 @@ static int mx6s_configure_csi(struct mx6s_csi_dev *csi_dev)
 		case V4L2_PIX_FMT_SBGGR8:
 			cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
 			break;
+		case V4L2_PIX_FMT_SBGGR10:
+			cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
+			break;
 		default:
 			pr_debug("   fmt not supported\n");
 			return -EINVAL;
 		}
 
 		csi_write(csi_dev, cr18, CSI_CSICR18);
+		csi_write(csi_dev, cr1, CSI_CSICR1);
 	}
 	return 0;
 }