1 files changed, 388 insertions, 140 deletions

diff --git a/drivers/media/i2c/ov5670.c b/drivers/media/i2c/ov5670.c
index 49189926afd6..04b3183b7bcb 100644
--- a/drivers/media/i2c/ov5670.c
+++ b/drivers/media/i2c/ov5670.c
@@ -2,13 +2,24 @@
 // Copyright (c) 2017 Intel Corporation.
 
 #include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
+#include <linux/mod_devicetable.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/unaligned.h>
+
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 
+#define OV5670_XVCLK_FREQ		19200000
+
 #define OV5670_REG_CHIP_ID		0x300a
 #define OV5670_CHIP_ID			0x005670
 
@@ -19,6 +30,12 @@
 #define OV5670_REG_SOFTWARE_RST		0x0103
 #define OV5670_SOFTWARE_RST		0x01
 
+#define OV5670_MIPI_SC_CTRL0_REG		0x3018
+#define OV5670_MIPI_SC_CTRL0_LANES(v)		((((v) - 1) << 5) & \
+						 GENMASK(7, 5))
+#define OV5670_MIPI_SC_CTRL0_MIPI_EN		BIT(4)
+#define OV5670_MIPI_SC_CTRL0_RESERVED		BIT(1)
+
 /* vertical-timings from sensor */
 #define OV5670_REG_VTS			0x380e
 #define OV5670_VTS_30FPS		0x0808 /* default for 30 fps */
@@ -64,6 +81,10 @@
 #define OV5670_REG_VALUE_16BIT		2
 #define OV5670_REG_VALUE_24BIT		3
 
+/* Pixel Array */
+#define OV5670_NATIVE_WIDTH		2624
+#define OV5670_NATIVE_HEIGHT		1980
+
 /* Initial number of frames to skip to avoid possible garbage */
 #define OV5670_NUM_OF_SKIP_FRAMES	2
 
@@ -78,10 +99,17 @@ struct ov5670_reg_list {
 };
 
 struct ov5670_link_freq_config {
-	u32 pixel_rate;
 	const struct ov5670_reg_list reg_list;
 };
 
+static const char * const ov5670_supply_names[] = {
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital power */
+	"dovdd",	/* Digital output power */
+};
+
+#define OV5670_NUM_SUPPLIES ARRAY_SIZE(ov5670_supply_names)
+
 struct ov5670_mode {
 	/* Frame width in pixels */
 	u32 width;
@@ -98,10 +126,25 @@ struct ov5670_mode {
 	/* Link frequency needed for this resolution */
 	u32 link_freq_index;
 
+	/* Analog crop rectangle */
+	const struct v4l2_rect *analog_crop;
+
 	/* Sensor register settings for this resolution */
 	const struct ov5670_reg_list reg_list;
 };
 
+/*
+ * All the modes supported by the driver are obtained by subsampling the
+ * full pixel array. The below values are reflected in registers from
+ * 0x3800-0x3807 in the modes register-value tables.
+ */
+static const struct v4l2_rect ov5670_analog_crop = {
+	.left	= 12,
+	.top	= 4,
+	.width	= 2600,
+	.height	= 1952,
+};
+
 static const struct ov5670_reg mipi_data_rate_840mbps[] = {
 	{0x0300, 0x04},
 	{0x0301, 0x00},
@@ -126,7 +169,6 @@ static const struct ov5670_reg mode_2592x1944_regs[] = {
 	{0x3005, 0xf0},
 	{0x3007, 0x00},
 	{0x3015, 0x0f},
-	{0x3018, 0x32},
 	{0x301a, 0xf0},
 	{0x301b, 0xf0},
 	{0x301c, 0xf0},
@@ -392,7 +434,6 @@ static const struct ov5670_reg mode_1296x972_regs[] = {
 	{0x3005, 0xf0},
 	{0x3007, 0x00},
 	{0x3015, 0x0f},
-	{0x3018, 0x32},
 	{0x301a, 0xf0},
 	{0x301b, 0xf0},
 	{0x301c, 0xf0},
@@ -658,7 +699,6 @@ static const struct ov5670_reg mode_648x486_regs[] = {
 	{0x3005, 0xf0},
 	{0x3007, 0x00},
 	{0x3015, 0x0f},
-	{0x3018, 0x32},
 	{0x301a, 0xf0},
 	{0x301b, 0xf0},
 	{0x301c, 0xf0},
@@ -924,7 +964,6 @@ static const struct ov5670_reg mode_2560x1440_regs[] = {
 	{0x3005, 0xf0},
 	{0x3007, 0x00},
 	{0x3015, 0x0f},
-	{0x3018, 0x32},
 	{0x301a, 0xf0},
 	{0x301b, 0xf0},
 	{0x301c, 0xf0},
@@ -1189,7 +1228,6 @@ static const struct ov5670_reg mode_1280x720_regs[] = {
 	{0x3005, 0xf0},
 	{0x3007, 0x00},
 	{0x3015, 0x0f},
-	{0x3018, 0x32},
 	{0x301a, 0xf0},
 	{0x301b, 0xf0},
 	{0x301c, 0xf0},
@@ -1455,7 +1493,6 @@ static const struct ov5670_reg mode_640x360_regs[] = {
 	{0x3005, 0xf0},
 	{0x3007, 0x00},
 	{0x3015, 0x0f},
-	{0x3018, 0x32},
 	{0x301a, 0xf0},
 	{0x301b, 0xf0},
 	{0x301c, 0xf0},
@@ -1725,8 +1762,6 @@ static const char * const ov5670_test_pattern_menu[] = {
 #define OV5670_LINK_FREQ_422MHZ_INDEX	0
 static const struct ov5670_link_freq_config link_freq_configs[] = {
 	{
-		/* pixel_rate = link_freq * 2 * nr_of_lanes / bits_per_sample */
-		.pixel_rate = (OV5670_LINK_FREQ_422MHZ * 2 * 2) / 10,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mipi_data_rate_840mbps),
 			.regs = mipi_data_rate_840mbps,
@@ -1749,72 +1784,82 @@ static const struct ov5670_mode supported_modes[] = {
 		.height = 1944,
 		.vts_def = OV5670_VTS_30FPS,
 		.vts_min = OV5670_VTS_30FPS,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_2592x1944_regs),
 			.regs = mode_2592x1944_regs,
 		},
-		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
 	},
 	{
 		.width = 1296,
 		.height = 972,
 		.vts_def = OV5670_VTS_30FPS,
 		.vts_min = 996,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_1296x972_regs),
 			.regs = mode_1296x972_regs,
 		},
-		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
 	},
 	{
 		.width = 648,
 		.height = 486,
 		.vts_def = OV5670_VTS_30FPS,
 		.vts_min = 516,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_648x486_regs),
 			.regs = mode_648x486_regs,
 		},
-		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
 	},
 	{
 		.width = 2560,
 		.height = 1440,
 		.vts_def = OV5670_VTS_30FPS,
 		.vts_min = OV5670_VTS_30FPS,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_2560x1440_regs),
 			.regs = mode_2560x1440_regs,
 		},
-		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
 	},
 	{
 		.width = 1280,
 		.height = 720,
 		.vts_def = OV5670_VTS_30FPS,
 		.vts_min = 1020,
+
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
 			.regs = mode_1280x720_regs,
 		},
-		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
 	},
 	{
 		.width = 640,
 		.height = 360,
 		.vts_def = OV5670_VTS_30FPS,
 		.vts_min = 510,
+		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
+		.analog_crop = &ov5670_analog_crop,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_640x360_regs),
 			.regs = mode_640x360_regs,
 		},
-		.link_freq_index = OV5670_LINK_FREQ_422MHZ_INDEX,
 	}
 };
 
 struct ov5670 {
+	struct device *dev;
+
 	struct v4l2_subdev sd;
 	struct media_pad pad;
+	struct v4l2_fwnode_endpoint endpoint;
 
 	struct v4l2_ctrl_handler ctrl_handler;
 	/* V4L2 Controls */
@@ -1827,11 +1872,21 @@ struct ov5670 {
 	/* Current mode */
 	const struct ov5670_mode *cur_mode;
 
-	/* To serialize asynchronus callbacks */
+	/* xvclk input clock */
+	struct clk *xvclk;
+
+	/* Regulators */
+	struct regulator_bulk_data supplies[OV5670_NUM_SUPPLIES];
+
+	/* Power-down and reset gpios. */
+	struct gpio_desc *pwdn_gpio; /* PWDNB pin. */
+	struct gpio_desc *reset_gpio; /* XSHUTDOWN pin. */
+
+	/* To serialize asynchronous callbacks */
 	struct mutex mutex;
 
-	/* Streaming on/off */
-	bool streaming;
+	/* True if the device has been identified */
+	bool identified;
 };
 
 #define to_ov5670(_sd)	container_of(_sd, struct ov5670, sd)
@@ -1907,7 +1962,6 @@ static int ov5670_write_reg(struct ov5670 *ov5670, u16 reg, unsigned int len,
 static int ov5670_write_regs(struct ov5670 *ov5670,
 			     const struct ov5670_reg *regs, unsigned int len)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
 	unsigned int i;
 	int ret;
 
@@ -1915,7 +1969,7 @@ static int ov5670_write_regs(struct ov5670 *ov5670,
 		ret = ov5670_write_reg(ov5670, regs[i].address, 1, regs[i].val);
 		if (ret) {
 			dev_err_ratelimited(
-				&client->dev,
+				ov5670->dev,
 				"Failed to write reg 0x%4.4x. error = %d\n",
 				regs[i].address, ret);
 
@@ -1932,27 +1986,6 @@ static int ov5670_write_reg_list(struct ov5670 *ov5670,
 	return ov5670_write_regs(ov5670, r_list->regs, r_list->num_of_regs);
 }
 
-/* Open sub-device */
-static int ov5670_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
-{
-	struct ov5670 *ov5670 = to_ov5670(sd);
-	struct v4l2_mbus_framefmt *try_fmt =
-				v4l2_subdev_get_try_format(sd, fh->state, 0);
-
-	mutex_lock(&ov5670->mutex);
-
-	/* Initialize try_fmt */
-	try_fmt->width = ov5670->cur_mode->width;
-	try_fmt->height = ov5670->cur_mode->height;
-	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
-	try_fmt->field = V4L2_FIELD_NONE;
-
-	/* No crop or compose */
-	mutex_unlock(&ov5670->mutex);
-
-	return 0;
-}
-
 static int ov5670_update_digital_gain(struct ov5670 *ov5670, u32 d_gain)
 {
 	int ret;
@@ -2001,9 +2034,8 @@ static int ov5670_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct ov5670 *ov5670 = container_of(ctrl->handler,
 					     struct ov5670, ctrl_handler);
-	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
 	s64 max;
-	int ret = 0;
+	int ret;
 
 	/* Propagate change of current control to all related controls */
 	switch (ctrl->id) {
@@ -2017,7 +2049,7 @@ static int ov5670_set_ctrl(struct v4l2_ctrl *ctrl)
 	}
 
 	/* V4L2 controls values will be applied only when power is already up */
-	if (!pm_runtime_get_if_in_use(&client->dev))
+	if (!pm_runtime_get_if_in_use(ov5670->dev))
 		return 0;
 
 	switch (ctrl->id) {
@@ -2042,13 +2074,19 @@ static int ov5670_set_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_TEST_PATTERN:
 		ret = ov5670_enable_test_pattern(ov5670, ctrl->val);
 		break;
+	case V4L2_CID_HBLANK:
+	case V4L2_CID_LINK_FREQ:
+	case V4L2_CID_PIXEL_RATE:
+		ret = 0;
+		break;
 	default:
-		dev_info(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
+		ret = -EINVAL;
+		dev_info(ov5670->dev, "%s Unhandled id:0x%x, val:0x%x\n",
 			 __func__, ctrl->id, ctrl->val);
 		break;
 	}
 
-	pm_runtime_put(&client->dev);
+	pm_runtime_put(ov5670->dev);
 
 	return ret;
 }
@@ -2060,9 +2098,12 @@ static const struct v4l2_ctrl_ops ov5670_ctrl_ops = {
 /* Initialize control handlers */
 static int ov5670_init_controls(struct ov5670 *ov5670)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&ov5670->endpoint.bus.mipi_csi2;
 	struct v4l2_fwnode_device_properties props;
 	struct v4l2_ctrl_handler *ctrl_hdlr;
+	unsigned int lanes_count;
+	s64 mipi_pixel_rate;
 	s64 vblank_max;
 	s64 vblank_def;
 	s64 vblank_min;
@@ -2083,12 +2124,15 @@ static int ov5670_init_controls(struct ov5670 *ov5670)
 		ov5670->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
 	/* By default, V4L2_CID_PIXEL_RATE is read only */
+	lanes_count = bus_mipi_csi2->num_data_lanes;
+	mipi_pixel_rate = OV5670_LINK_FREQ_422MHZ * 2 * lanes_count / 10;
+
 	ov5670->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov5670_ctrl_ops,
 					       V4L2_CID_PIXEL_RATE,
-					       link_freq_configs[0].pixel_rate,
-					       link_freq_configs[0].pixel_rate,
+					       mipi_pixel_rate,
+					       mipi_pixel_rate,
 					       1,
-					       link_freq_configs[0].pixel_rate);
+					       mipi_pixel_rate);
 
 	vblank_max = OV5670_VTS_MAX - ov5670->cur_mode->height;
 	vblank_def = ov5670->cur_mode->vts_def - ov5670->cur_mode->height;
@@ -2133,7 +2177,7 @@ static int ov5670_init_controls(struct ov5670 *ov5670)
 		goto error;
 	}
 
-	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	ret = v4l2_fwnode_device_parse(ov5670->dev, &props);
 	if (ret)
 		goto error;
 
@@ -2152,6 +2196,28 @@ error:
 	return ret;
 }
 
+static int ov5670_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt =
+				v4l2_subdev_state_get_format(state, 0);
+	const struct ov5670_mode *default_mode = &supported_modes[0];
+	struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
+
+	fmt->width = default_mode->width;
+	fmt->height = default_mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB);
+
+	*crop = *default_mode->analog_crop;
+
+	return 0;
+}
+
 static int ov5670_enum_mbus_code(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_state *sd_state,
 				 struct v4l2_subdev_mbus_code_enum *code)
@@ -2197,9 +2263,8 @@ static int ov5670_do_get_pad_format(struct ov5670 *ov5670,
 				    struct v4l2_subdev_format *fmt)
 {
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt->format = *v4l2_subdev_get_try_format(&ov5670->sd,
-							  sd_state,
-							  fmt->pad);
+		fmt->format = *v4l2_subdev_state_get_format(sd_state,
+							    fmt->pad);
 	else
 		ov5670_update_pad_format(ov5670->cur_mode, fmt);
 
@@ -2225,8 +2290,13 @@ static int ov5670_set_pad_format(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_format *fmt)
 {
 	struct ov5670 *ov5670 = to_ov5670(sd);
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&ov5670->endpoint.bus.mipi_csi2;
 	const struct ov5670_mode *mode;
+	unsigned int lanes_count;
+	s64 mipi_pixel_rate;
 	s32 vblank_def;
+	s64 link_freq;
 	s32 h_blank;
 
 	mutex_lock(&ov5670->mutex);
@@ -2239,13 +2309,18 @@ static int ov5670_set_pad_format(struct v4l2_subdev *sd,
 				      fmt->format.width, fmt->format.height);
 	ov5670_update_pad_format(mode, fmt);
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
-		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = fmt->format;
+		*v4l2_subdev_state_get_format(sd_state, fmt->pad) = fmt->format;
 	} else {
 		ov5670->cur_mode = mode;
 		__v4l2_ctrl_s_ctrl(ov5670->link_freq, mode->link_freq_index);
+
+		lanes_count = bus_mipi_csi2->num_data_lanes;
+		link_freq = link_freq_menu_items[mode->link_freq_index];
+		/* pixel_rate = link_freq * 2 * nr_of_lanes / bits_per_sample */
+		mipi_pixel_rate = div_s64(link_freq * 2 * lanes_count, 10);
 		__v4l2_ctrl_s_ctrl_int64(
 			ov5670->pixel_rate,
-			link_freq_configs[mode->link_freq_index].pixel_rate);
+			mipi_pixel_rate);
 		/* Update limits and set FPS to default */
 		vblank_def = ov5670->cur_mode->vts_def -
 			     ov5670->cur_mode->height;
@@ -2272,19 +2347,60 @@ static int ov5670_get_skip_frames(struct v4l2_subdev *sd, u32 *frames)
 	return 0;
 }
 
+/* Verify chip ID */
+static int ov5670_identify_module(struct ov5670 *ov5670)
+{
+	int ret;
+	u32 val;
+
+	if (ov5670->identified)
+		return 0;
+
+	ret = ov5670_read_reg(ov5670, OV5670_REG_CHIP_ID,
+			      OV5670_REG_VALUE_24BIT, &val);
+	if (ret)
+		return ret;
+
+	if (val != OV5670_CHIP_ID) {
+		dev_err(ov5670->dev, "chip id mismatch: %x!=%x\n",
+			OV5670_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	ov5670->identified = true;
+
+	return 0;
+}
+
+static int ov5670_mipi_configure(struct ov5670 *ov5670)
+{
+	struct v4l2_mbus_config_mipi_csi2 *bus_mipi_csi2 =
+		&ov5670->endpoint.bus.mipi_csi2;
+	unsigned int lanes_count = bus_mipi_csi2->num_data_lanes;
+
+	return ov5670_write_reg(ov5670, OV5670_MIPI_SC_CTRL0_REG,
+				OV5670_REG_VALUE_08BIT,
+				OV5670_MIPI_SC_CTRL0_LANES(lanes_count) |
+				OV5670_MIPI_SC_CTRL0_MIPI_EN |
+				OV5670_MIPI_SC_CTRL0_RESERVED);
+}
+
 /* Prepare streaming by writing default values and customized values */
 static int ov5670_start_streaming(struct ov5670 *ov5670)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
 	const struct ov5670_reg_list *reg_list;
 	int link_freq_index;
 	int ret;
 
+	ret = ov5670_identify_module(ov5670);
+	if (ret)
+		return ret;
+
 	/* Get out of from software reset */
 	ret = ov5670_write_reg(ov5670, OV5670_REG_SOFTWARE_RST,
 			       OV5670_REG_VALUE_08BIT, OV5670_SOFTWARE_RST);
 	if (ret) {
-		dev_err(&client->dev, "%s failed to set powerup registers\n",
+		dev_err(ov5670->dev, "%s failed to set powerup registers\n",
 			__func__);
 		return ret;
 	}
@@ -2294,7 +2410,7 @@ static int ov5670_start_streaming(struct ov5670 *ov5670)
 	reg_list = &link_freq_configs[link_freq_index].reg_list;
 	ret = ov5670_write_reg_list(ov5670, reg_list);
 	if (ret) {
-		dev_err(&client->dev, "%s failed to set plls\n", __func__);
+		dev_err(ov5670->dev, "%s failed to set plls\n", __func__);
 		return ret;
 	}
 
@@ -2302,7 +2418,13 @@ static int ov5670_start_streaming(struct ov5670 *ov5670)
 	reg_list = &ov5670->cur_mode->reg_list;
 	ret = ov5670_write_reg_list(ov5670, reg_list);
 	if (ret) {
-		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		dev_err(ov5670->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
+	ret = ov5670_mipi_configure(ov5670);
+	if (ret) {
+		dev_err(ov5670->dev, "%s failed to configure MIPI\n", __func__);
 		return ret;
 	}
 
@@ -2314,7 +2436,7 @@ static int ov5670_start_streaming(struct ov5670 *ov5670)
 	ret = ov5670_write_reg(ov5670, OV5670_REG_MODE_SELECT,
 			       OV5670_REG_VALUE_08BIT, OV5670_MODE_STREAMING);
 	if (ret) {
-		dev_err(&client->dev, "%s failed to set stream\n", __func__);
+		dev_err(ov5670->dev, "%s failed to set stream\n", __func__);
 		return ret;
 	}
 
@@ -2323,13 +2445,12 @@ static int ov5670_start_streaming(struct ov5670 *ov5670)
 
 static int ov5670_stop_streaming(struct ov5670 *ov5670)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
 	int ret;
 
 	ret = ov5670_write_reg(ov5670, OV5670_REG_MODE_SELECT,
 			       OV5670_REG_VALUE_08BIT, OV5670_MODE_STANDBY);
 	if (ret)
-		dev_err(&client->dev, "%s failed to set stream\n", __func__);
+		dev_err(ov5670->dev, "%s failed to set stream\n", __func__);
 
 	/* Return success even if it was an error, as there is nothing the
 	 * caller can do about it.
@@ -2340,15 +2461,12 @@ static int ov5670_stop_streaming(struct ov5670 *ov5670)
 static int ov5670_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct ov5670 *ov5670 = to_ov5670(sd);
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	int ret = 0;
 
 	mutex_lock(&ov5670->mutex);
-	if (ov5670->streaming == enable)
-		goto unlock_and_return;
 
 	if (enable) {
-		ret = pm_runtime_resume_and_get(&client->dev);
+		ret = pm_runtime_resume_and_get(ov5670->dev);
 		if (ret < 0)
 			goto unlock_and_return;
 
@@ -2357,13 +2475,12 @@ static int ov5670_set_stream(struct v4l2_subdev *sd, int enable)
 			goto error;
 	} else {
 		ret = ov5670_stop_streaming(ov5670);
-		pm_runtime_put(&client->dev);
+		pm_runtime_put(ov5670->dev);
 	}
-	ov5670->streaming = enable;
 	goto unlock_and_return;
 
 error:
-	pm_runtime_put(&client->dev);
+	pm_runtime_put(ov5670->dev);
 
 unlock_and_return:
 	mutex_unlock(&ov5670->mutex);
@@ -2371,50 +2488,96 @@ unlock_and_return:
 	return ret;
 }
 
-static int __maybe_unused ov5670_suspend(struct device *dev)
+static int __maybe_unused ov5670_runtime_resume(struct device *dev)
 {
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov5670 *ov5670 = to_ov5670(sd);
+	unsigned long delay_us;
+	int ret;
+
+	ret = clk_prepare_enable(ov5670->xvclk);
+	if (ret)
+		return ret;
+
+	ret = regulator_bulk_enable(OV5670_NUM_SUPPLIES, ov5670->supplies);
+	if (ret) {
+		clk_disable_unprepare(ov5670->xvclk);
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov5670->pwdn_gpio, 0);
+	gpiod_set_value_cansleep(ov5670->reset_gpio, 0);
 
-	if (ov5670->streaming)
-		ov5670_stop_streaming(ov5670);
+	/* 8192 * 2 clock pulses before the first SCCB transaction. */
+	delay_us = DIV_ROUND_UP(8192 * 2 * 1000,
+				DIV_ROUND_UP(OV5670_XVCLK_FREQ, 1000));
+	fsleep(delay_us);
 
 	return 0;
 }
 
-static int __maybe_unused ov5670_resume(struct device *dev)
+static int __maybe_unused ov5670_runtime_suspend(struct device *dev)
 {
-	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov5670 *ov5670 = to_ov5670(sd);
-	int ret;
 
-	if (ov5670->streaming) {
-		ret = ov5670_start_streaming(ov5670);
-		if (ret) {
-			ov5670_stop_streaming(ov5670);
-			return ret;
-		}
-	}
+	gpiod_set_value_cansleep(ov5670->reset_gpio, 1);
+	gpiod_set_value_cansleep(ov5670->pwdn_gpio, 1);
+	regulator_bulk_disable(OV5670_NUM_SUPPLIES, ov5670->supplies);
+	clk_disable_unprepare(ov5670->xvclk);
 
 	return 0;
 }
 
-/* Verify chip ID */
-static int ov5670_identify_module(struct ov5670 *ov5670)
+static const struct v4l2_subdev_core_ops ov5670_core_ops = {
+	.log_status = v4l2_ctrl_subdev_log_status,
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_rect *
+__ov5670_get_pad_crop(struct ov5670 *sensor, struct v4l2_subdev_state *state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&ov5670->sd);
-	int ret;
-	u32 val;
+	const struct ov5670_mode *mode = sensor->cur_mode;
 
-	ret = ov5670_read_reg(ov5670, OV5670_REG_CHIP_ID,
-			      OV5670_REG_VALUE_24BIT, &val);
-	if (ret)
-		return ret;
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_state_get_crop(state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return mode->analog_crop;
+	}
 
-	if (val != OV5670_CHIP_ID) {
-		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
-			OV5670_CHIP_ID, val);
-		return -ENXIO;
+	return NULL;
+}
+
+static int ov5670_get_selection(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	struct ov5670 *sensor = to_ov5670(subdev);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		mutex_lock(&sensor->mutex);
+		sel->r = *__ov5670_get_pad_crop(sensor, state, sel->pad,
+						sel->which);
+		mutex_unlock(&sensor->mutex);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = OV5670_NATIVE_WIDTH;
+		sel->r.height = OV5670_NATIVE_HEIGHT;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		sel->r = ov5670_analog_crop;
+		break;
+	default:
+		return -EINVAL;
 	}
 
 	return 0;
@@ -2429,6 +2592,8 @@ static const struct v4l2_subdev_pad_ops ov5670_pad_ops = {
 	.get_fmt = ov5670_get_pad_format,
 	.set_fmt = ov5670_set_pad_format,
 	.enum_frame_size = ov5670_enum_frame_size,
+	.get_selection = ov5670_get_selection,
+	.set_selection = ov5670_get_selection,
 };
 
 static const struct v4l2_subdev_sensor_ops ov5670_sensor_ops = {
@@ -2436,45 +2601,116 @@ static const struct v4l2_subdev_sensor_ops ov5670_sensor_ops = {
 };
 
 static const struct v4l2_subdev_ops ov5670_subdev_ops = {
+	.core = &ov5670_core_ops,
 	.video = &ov5670_video_ops,
 	.pad = &ov5670_pad_ops,
 	.sensor = &ov5670_sensor_ops,
 };
 
+static const struct v4l2_subdev_internal_ops ov5670_internal_ops = {
+	.init_state = ov5670_init_state,
+};
+
 static const struct media_entity_operations ov5670_subdev_entity_ops = {
 	.link_validate = v4l2_subdev_link_validate,
 };
 
-static const struct v4l2_subdev_internal_ops ov5670_internal_ops = {
-	.open = ov5670_open,
-};
+static int ov5670_regulators_probe(struct ov5670 *ov5670)
+{
+	unsigned int i;
+
+	for (i = 0; i < OV5670_NUM_SUPPLIES; i++)
+		ov5670->supplies[i].supply = ov5670_supply_names[i];
+
+	return devm_regulator_bulk_get(ov5670->dev, OV5670_NUM_SUPPLIES,
+				       ov5670->supplies);
+}
+
+static int ov5670_gpio_probe(struct ov5670 *ov5670)
+{
+	ov5670->pwdn_gpio = devm_gpiod_get_optional(ov5670->dev, "powerdown",
+						    GPIOD_OUT_LOW);
+	if (IS_ERR(ov5670->pwdn_gpio))
+		return PTR_ERR(ov5670->pwdn_gpio);
+
+	ov5670->reset_gpio = devm_gpiod_get_optional(ov5670->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(ov5670->reset_gpio))
+		return PTR_ERR(ov5670->reset_gpio);
+
+	return 0;
+}
 
 static int ov5670_probe(struct i2c_client *client)
 {
+	struct fwnode_handle *handle;
 	struct ov5670 *ov5670;
-	const char *err_msg;
 	u32 input_clk = 0;
+	bool full_power;
 	int ret;
 
-	device_property_read_u32(&client->dev, "clock-frequency", &input_clk);
-	if (input_clk != 19200000)
-		return -EINVAL;
-
 	ov5670 = devm_kzalloc(&client->dev, sizeof(*ov5670), GFP_KERNEL);
-	if (!ov5670) {
-		ret = -ENOMEM;
-		err_msg = "devm_kzalloc() error";
-		goto error_print;
+	if (!ov5670)
+		return -ENOMEM;
+
+	ov5670->dev = &client->dev;
+
+	ov5670->xvclk = devm_v4l2_sensor_clk_get(ov5670->dev, NULL);
+	if (IS_ERR(ov5670->xvclk))
+		return dev_err_probe(ov5670->dev, PTR_ERR(ov5670->xvclk),
+				     "error getting clock\n");
+
+	input_clk = clk_get_rate(ov5670->xvclk);
+	if (input_clk != OV5670_XVCLK_FREQ) {
+		dev_err(ov5670->dev,
+			"Unsupported clock frequency %u\n", input_clk);
+		return -EINVAL;
 	}
 
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&ov5670->sd, client, &ov5670_subdev_ops);
+	ov5670->sd.internal_ops = &ov5670_internal_ops;
 
-	/* Check module identity */
-	ret = ov5670_identify_module(ov5670);
-	if (ret) {
-		err_msg = "ov5670_identify_module() error";
-		goto error_print;
+	ret = ov5670_regulators_probe(ov5670);
+	if (ret)
+		return dev_err_probe(ov5670->dev, ret, "Regulators probe failed\n");
+
+	ret = ov5670_gpio_probe(ov5670);
+	if (ret)
+		return dev_err_probe(ov5670->dev, ret, "GPIO probe failed\n");
+
+	/*
+	 * Graph Endpoint. If it's missing we defer rather than fail, as this
+	 * sensor is known to co-exist on systems with the IPU3 and so it might
+	 * be created by the ipu-bridge.
+	 */
+	handle = fwnode_graph_get_next_endpoint(dev_fwnode(ov5670->dev), NULL);
+	if (!handle)
+		return dev_err_probe(ov5670->dev, -EPROBE_DEFER,
+				     "Endpoint for node get failed\n");
+
+	ov5670->endpoint.bus_type = V4L2_MBUS_CSI2_DPHY;
+	ov5670->endpoint.bus.mipi_csi2.num_data_lanes = 2;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(handle, &ov5670->endpoint);
+	fwnode_handle_put(handle);
+	if (ret)
+		return dev_err_probe(ov5670->dev, ret, "Endpoint parse failed\n");
+
+	full_power = acpi_dev_state_d0(ov5670->dev);
+	if (full_power) {
+		ret = ov5670_runtime_resume(ov5670->dev);
+		if (ret) {
+			dev_err_probe(ov5670->dev, ret, "Power up failed\n");
+			goto error_endpoint;
+		}
+
+		/* Check module identity */
+		ret = ov5670_identify_module(ov5670);
+		if (ret) {
+			dev_err_probe(ov5670->dev, ret, "ov5670_identify_module() error\n");
+			goto error_power_off;
+		}
 	}
 
 	mutex_init(&ov5670->mutex);
@@ -2484,12 +2720,12 @@ static int ov5670_probe(struct i2c_client *client)
 
 	ret = ov5670_init_controls(ov5670);
 	if (ret) {
-		err_msg = "ov5670_init_controls() error";
+		dev_err_probe(ov5670->dev, ret, "ov5670_init_controls() error\n");
 		goto error_mutex_destroy;
 	}
 
-	ov5670->sd.internal_ops = &ov5670_internal_ops;
-	ov5670->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov5670->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			    V4L2_SUBDEV_FL_HAS_EVENTS;
 	ov5670->sd.entity.ops = &ov5670_subdev_entity_ops;
 	ov5670->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
@@ -2497,30 +2733,29 @@ static int ov5670_probe(struct i2c_client *client)
 	ov5670->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&ov5670->sd.entity, 1, &ov5670->pad);
 	if (ret) {
-		err_msg = "media_entity_pads_init() error";
+		dev_err_probe(ov5670->dev, ret, "media_entity_pads_init() error\n");
 		goto error_handler_free;
 	}
 
+	/* Set the device's state to active if it's in D0 state. */
+	if (full_power)
+		pm_runtime_set_active(ov5670->dev);
+	pm_runtime_enable(ov5670->dev);
+
 	/* Async register for subdev */
 	ret = v4l2_async_register_subdev_sensor(&ov5670->sd);
 	if (ret < 0) {
-		err_msg = "v4l2_async_register_subdev() error";
-		goto error_entity_cleanup;
+		dev_err_probe(ov5670->dev, ret, "v4l2_async_register_subdev() error\n");
+		goto error_pm_disable;
 	}
 
-	ov5670->streaming = false;
-
-	/*
-	 * Device is already turned on by i2c-core with ACPI domain PM.
-	 * Enable runtime PM and turn off the device.
-	 */
-	pm_runtime_set_active(&client->dev);
-	pm_runtime_enable(&client->dev);
-	pm_runtime_idle(&client->dev);
+	pm_runtime_idle(ov5670->dev);
 
 	return 0;
 
-error_entity_cleanup:
+error_pm_disable:
+	pm_runtime_disable(ov5670->dev);
+
 	media_entity_cleanup(&ov5670->sd.entity);
 
 error_handler_free:
@@ -2529,13 +2764,17 @@ error_handler_free:
 error_mutex_destroy:
 	mutex_destroy(&ov5670->mutex);
 
-error_print:
-	dev_err(&client->dev, "%s: %s %d\n", __func__, err_msg, ret);
+error_power_off:
+	if (full_power)
+		ov5670_runtime_suspend(ov5670->dev);
+
+error_endpoint:
+	v4l2_fwnode_endpoint_free(&ov5670->endpoint);
 
 	return ret;
 }
 
-static int ov5670_remove(struct i2c_client *client)
+static void ov5670_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov5670 *ov5670 = to_ov5670(sd);
@@ -2545,37 +2784,46 @@ static int ov5670_remove(struct i2c_client *client)
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 	mutex_destroy(&ov5670->mutex);
 
-	pm_runtime_disable(&client->dev);
+	pm_runtime_disable(ov5670->dev);
+	ov5670_runtime_suspend(ov5670->dev);
 
-	return 0;
+	v4l2_fwnode_endpoint_free(&ov5670->endpoint);
 }
 
 static const struct dev_pm_ops ov5670_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(ov5670_suspend, ov5670_resume)
+	SET_RUNTIME_PM_OPS(ov5670_runtime_suspend, ov5670_runtime_resume, NULL)
 };
 
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id ov5670_acpi_ids[] = {
-	{"INT3479"},
+	{ "INT3479" },
 	{ /* sentinel */ }
 };
 
 MODULE_DEVICE_TABLE(acpi, ov5670_acpi_ids);
 #endif
 
+static const struct of_device_id ov5670_of_ids[] = {
+	{ .compatible = "ovti,ov5670" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov5670_of_ids);
+
 static struct i2c_driver ov5670_i2c_driver = {
 	.driver = {
 		.name = "ov5670",
 		.pm = &ov5670_pm_ops,
 		.acpi_match_table = ACPI_PTR(ov5670_acpi_ids),
+		.of_match_table = ov5670_of_ids,
 	},
-	.probe_new = ov5670_probe,
+	.probe = ov5670_probe,
 	.remove = ov5670_remove,
+	.flags = I2C_DRV_ACPI_WAIVE_D0_PROBE,
 };
 
 module_i2c_driver(ov5670_i2c_driver);
 
-MODULE_AUTHOR("Rapolu, Chiranjeevi <chiranjeevi.rapolu@intel.com>");
-MODULE_AUTHOR("Yang, Hyungwoo <hyungwoo.yang@intel.com>");
+MODULE_AUTHOR("Rapolu, Chiranjeevi");
+MODULE_AUTHOR("Yang, Hyungwoo");
 MODULE_DESCRIPTION("Omnivision ov5670 sensor driver");
 MODULE_LICENSE("GPL v2");