ov5695.c

   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * ov5695 driver
   4 *
   5 * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
   6 */
   7
   8#include <linux/clk.h>
   9#include <linux/device.h>
  10#include <linux/delay.h>
  11#include <linux/gpio/consumer.h>
  12#include <linux/i2c.h>
  13#include <linux/module.h>
  14#include <linux/pm_runtime.h>
  15#include <linux/regulator/consumer.h>
  16#include <linux/sysfs.h>
  17#include <media/media-entity.h>
  18#include <media/v4l2-async.h>
  19#include <media/v4l2-ctrls.h>
  20#include <media/v4l2-subdev.h>
  21
  22#ifndef V4L2_CID_DIGITAL_GAIN
  23#define V4L2_CID_DIGITAL_GAIN		V4L2_CID_GAIN
  24#endif
  25
  26/* 45Mhz * 4 Binning */
  27#define OV5695_PIXEL_RATE		(45 * 1000 * 1000 * 4)
  28#define OV5695_XVCLK_FREQ		24000000
  29
  30#define CHIP_ID				0x005695
  31#define OV5695_REG_CHIP_ID		0x300a
  32
  33#define OV5695_REG_CTRL_MODE		0x0100
  34#define OV5695_MODE_SW_STANDBY		0x0
  35#define OV5695_MODE_STREAMING		BIT(0)
  36
  37#define OV5695_REG_EXPOSURE		0x3500
  38#define	OV5695_EXPOSURE_MIN		4
  39#define	OV5695_EXPOSURE_STEP		1
  40#define OV5695_VTS_MAX			0x7fff
  41
  42#define OV5695_REG_ANALOG_GAIN		0x3509
  43#define	ANALOG_GAIN_MIN			0x10
  44#define	ANALOG_GAIN_MAX			0xf8
  45#define	ANALOG_GAIN_STEP		1
  46#define	ANALOG_GAIN_DEFAULT		0xf8
  47
  48#define OV5695_REG_DIGI_GAIN_H		0x350a
  49#define OV5695_REG_DIGI_GAIN_L		0x350b
  50#define OV5695_DIGI_GAIN_L_MASK		0x3f
  51#define OV5695_DIGI_GAIN_H_SHIFT	6
  52#define OV5695_DIGI_GAIN_MIN		0
  53#define OV5695_DIGI_GAIN_MAX		(0x4000 - 1)
  54#define OV5695_DIGI_GAIN_STEP		1
  55#define OV5695_DIGI_GAIN_DEFAULT	1024
  56
  57#define OV5695_REG_TEST_PATTERN		0x4503
  58#define	OV5695_TEST_PATTERN_ENABLE	0x80
  59#define	OV5695_TEST_PATTERN_DISABLE	0x0
  60
  61#define OV5695_REG_VTS			0x380e
  62
  63#define REG_NULL			0xFFFF
  64
  65#define OV5695_REG_VALUE_08BIT		1
  66#define OV5695_REG_VALUE_16BIT		2
  67#define OV5695_REG_VALUE_24BIT		3
  68
  69#define OV5695_LANES			2
  70#define OV5695_BITS_PER_SAMPLE		10
  71
  72static const char * const ov5695_supply_names[] = {
  73	"avdd",		/* Analog power */
  74	"dovdd",	/* Digital I/O power */
  75	"dvdd",		/* Digital core power */
  76};
  77
  78#define OV5695_NUM_SUPPLIES ARRAY_SIZE(ov5695_supply_names)
  79
  80struct regval {
  81	u16 addr;
  82	u8 val;
  83};
  84
  85struct ov5695_mode {
  86	u32 width;
  87	u32 height;
  88	u32 max_fps;
  89	u32 hts_def;
  90	u32 vts_def;
  91	u32 exp_def;
  92	const struct regval *reg_list;
  93};
  94
  95struct ov5695 {
  96	struct i2c_client	*client;
  97	struct clk		*xvclk;
  98	struct gpio_desc	*reset_gpio;
  99	struct regulator_bulk_data supplies[OV5695_NUM_SUPPLIES];
 100
 101	struct v4l2_subdev	subdev;
 102	struct media_pad	pad;
 103	struct v4l2_ctrl_handler ctrl_handler;
 104	struct v4l2_ctrl	*exposure;
 105	struct v4l2_ctrl	*anal_gain;
 106	struct v4l2_ctrl	*digi_gain;
 107	struct v4l2_ctrl	*hblank;
 108	struct v4l2_ctrl	*vblank;
 109	struct v4l2_ctrl	*test_pattern;
 110	struct mutex		mutex;
 111	const struct ov5695_mode *cur_mode;
 112};
 113
 114#define to_ov5695(sd) container_of(sd, struct ov5695, subdev)
 115
 116/*
 117 * Xclk 24Mhz
 118 * Pclk 45Mhz
 119 * linelength 672(0x2a0)
 120 * framelength 2232(0x8b8)
 121 * grabwindow_width 1296
 122 * grabwindow_height 972
 123 * max_framerate 30fps
 124 * mipi_datarate per lane 840Mbps
 125 */
 126static const struct regval ov5695_global_regs[] = {
 127	{0x0103, 0x01},
 128	{0x0100, 0x00},
 129	{0x0300, 0x04},
 130	{0x0301, 0x00},
 131	{0x0302, 0x69},
 132	{0x0303, 0x00},
 133	{0x0304, 0x00},
 134	{0x0305, 0x01},
 135	{0x0307, 0x00},
 136	{0x030b, 0x00},
 137	{0x030c, 0x00},
 138	{0x030d, 0x1e},
 139	{0x030e, 0x04},
 140	{0x030f, 0x03},
 141	{0x0312, 0x01},
 142	{0x3000, 0x00},
 143	{0x3002, 0xa1},
 144	{0x3008, 0x00},
 145	{0x3010, 0x00},
 146	{0x3022, 0x51},
 147	{0x3106, 0x15},
 148	{0x3107, 0x01},
 149	{0x3108, 0x05},
 150	{0x3500, 0x00},
 151	{0x3501, 0x45},
 152	{0x3502, 0x00},
 153	{0x3503, 0x08},
 154	{0x3504, 0x03},
 155	{0x3505, 0x8c},
 156	{0x3507, 0x03},
 157	{0x3508, 0x00},
 158	{0x3509, 0x10},
 159	{0x350c, 0x00},
 160	{0x350d, 0x80},
 161	{0x3510, 0x00},
 162	{0x3511, 0x02},
 163	{0x3512, 0x00},
 164	{0x3601, 0x55},
 165	{0x3602, 0x58},
 166	{0x3614, 0x30},
 167	{0x3615, 0x77},
 168	{0x3621, 0x08},
 169	{0x3624, 0x40},
 170	{0x3633, 0x0c},
 171	{0x3634, 0x0c},
 172	{0x3635, 0x0c},
 173	{0x3636, 0x0c},
 174	{0x3638, 0x00},
 175	{0x3639, 0x00},
 176	{0x363a, 0x00},
 177	{0x363b, 0x00},
 178	{0x363c, 0xff},
 179	{0x363d, 0xfa},
 180	{0x3650, 0x44},
 181	{0x3651, 0x44},
 182	{0x3652, 0x44},
 183	{0x3653, 0x44},
 184	{0x3654, 0x44},
 185	{0x3655, 0x44},
 186	{0x3656, 0x44},
 187	{0x3657, 0x44},
 188	{0x3660, 0x00},
 189	{0x3661, 0x00},
 190	{0x3662, 0x00},
 191	{0x366a, 0x00},
 192	{0x366e, 0x0c},
 193	{0x3673, 0x04},
 194	{0x3700, 0x14},
 195	{0x3703, 0x0c},
 196	{0x3715, 0x01},
 197	{0x3733, 0x10},
 198	{0x3734, 0x40},
 199	{0x373f, 0xa0},
 200	{0x3765, 0x20},
 201	{0x37a1, 0x1d},
 202	{0x37a8, 0x26},
 203	{0x37ab, 0x14},
 204	{0x37c2, 0x04},
 205	{0x37cb, 0x09},
 206	{0x37cc, 0x13},
 207	{0x37cd, 0x1f},
 208	{0x37ce, 0x1f},
 209	{0x3800, 0x00},
 210	{0x3801, 0x00},
 211	{0x3802, 0x00},
 212	{0x3803, 0x00},
 213	{0x3804, 0x0a},
 214	{0x3805, 0x3f},
 215	{0x3806, 0x07},
 216	{0x3807, 0xaf},
 217	{0x3808, 0x05},
 218	{0x3809, 0x10},
 219	{0x380a, 0x03},
 220	{0x380b, 0xcc},
 221	{0x380c, 0x02},
 222	{0x380d, 0xa0},
 223	{0x380e, 0x08},
 224	{0x380f, 0xb8},
 225	{0x3810, 0x00},
 226	{0x3811, 0x06},
 227	{0x3812, 0x00},
 228	{0x3813, 0x06},
 229	{0x3814, 0x03},
 230	{0x3815, 0x01},
 231	{0x3816, 0x03},
 232	{0x3817, 0x01},
 233	{0x3818, 0x00},
 234	{0x3819, 0x00},
 235	{0x381a, 0x00},
 236	{0x381b, 0x01},
 237	{0x3820, 0x8b},
 238	{0x3821, 0x01},
 239	{0x3c80, 0x08},
 240	{0x3c82, 0x00},
 241	{0x3c83, 0x00},
 242	{0x3c88, 0x00},
 243	{0x3d85, 0x14},
 244	{0x3f02, 0x08},
 245	{0x3f03, 0x10},
 246	{0x4008, 0x02},
 247	{0x4009, 0x09},
 248	{0x404e, 0x20},
 249	{0x4501, 0x00},
 250	{0x4502, 0x10},
 251	{0x4800, 0x00},
 252	{0x481f, 0x2a},
 253	{0x4837, 0x13},
 254	{0x5000, 0x17},
 255	{0x5780, 0x3e},
 256	{0x5781, 0x0f},
 257	{0x5782, 0x44},
 258	{0x5783, 0x02},
 259	{0x5784, 0x01},
 260	{0x5785, 0x01},
 261	{0x5786, 0x00},
 262	{0x5787, 0x04},
 263	{0x5788, 0x02},
 264	{0x5789, 0x0f},
 265	{0x578a, 0xfd},
 266	{0x578b, 0xf5},
 267	{0x578c, 0xf5},
 268	{0x578d, 0x03},
 269	{0x578e, 0x08},
 270	{0x578f, 0x0c},
 271	{0x5790, 0x08},
 272	{0x5791, 0x06},
 273	{0x5792, 0x00},
 274	{0x5793, 0x52},
 275	{0x5794, 0xa3},
 276	{0x5b00, 0x00},
 277	{0x5b01, 0x1c},
 278	{0x5b02, 0x00},
 279	{0x5b03, 0x7f},
 280	{0x5b05, 0x6c},
 281	{0x5e10, 0xfc},
 282	{0x4010, 0xf1},
 283	{0x3503, 0x08},
 284	{0x3505, 0x8c},
 285	{0x3507, 0x03},
 286	{0x3508, 0x00},
 287	{0x3509, 0xf8},
 288	{REG_NULL, 0x00},
 289};
 290
 291/*
 292 * Xclk 24Mhz
 293 * Pclk 45Mhz
 294 * linelength 740(0x2e4)
 295 * framelength 2024(0x7e8)
 296 * grabwindow_width 2592
 297 * grabwindow_height 1944
 298 * max_framerate 30fps
 299 * mipi_datarate per lane 840Mbps
 300 */
 301static const struct regval ov5695_2592x1944_regs[] = {
 302	{0x3501, 0x7e},
 303	{0x366e, 0x18},
 304	{0x3800, 0x00},
 305	{0x3801, 0x00},
 306	{0x3802, 0x00},
 307	{0x3803, 0x04},
 308	{0x3804, 0x0a},
 309	{0x3805, 0x3f},
 310	{0x3806, 0x07},
 311	{0x3807, 0xab},
 312	{0x3808, 0x0a},
 313	{0x3809, 0x20},
 314	{0x380a, 0x07},
 315	{0x380b, 0x98},
 316	{0x380c, 0x02},
 317	{0x380d, 0xe4},
 318	{0x380e, 0x07},
 319	{0x380f, 0xe8},
 320	{0x3811, 0x06},
 321	{0x3813, 0x08},
 322	{0x3814, 0x01},
 323	{0x3816, 0x01},
 324	{0x3817, 0x01},
 325	{0x3820, 0x88},
 326	{0x3821, 0x00},
 327	{0x4501, 0x00},
 328	{0x4008, 0x04},
 329	{0x4009, 0x13},
 330	{REG_NULL, 0x00},
 331};
 332
 333/*
 334 * Xclk 24Mhz
 335 * Pclk 45Mhz
 336 * linelength 672(0x2a0)
 337 * framelength 2232(0x8b8)
 338 * grabwindow_width 1920
 339 * grabwindow_height 1080
 340 * max_framerate 30fps
 341 * mipi_datarate per lane 840Mbps
 342 */
 343static const struct regval ov5695_1920x1080_regs[] = {
 344	{0x3501, 0x45},
 345	{0x366e, 0x18},
 346	{0x3800, 0x01},
 347	{0x3801, 0x50},
 348	{0x3802, 0x01},
 349	{0x3803, 0xb8},
 350	{0x3804, 0x08},
 351	{0x3805, 0xef},
 352	{0x3806, 0x05},
 353	{0x3807, 0xf7},
 354	{0x3808, 0x07},
 355	{0x3809, 0x80},
 356	{0x380a, 0x04},
 357	{0x380b, 0x38},
 358	{0x380c, 0x02},
 359	{0x380d, 0xa0},
 360	{0x380e, 0x08},
 361	{0x380f, 0xb8},
 362	{0x3811, 0x06},
 363	{0x3813, 0x04},
 364	{0x3814, 0x01},
 365	{0x3816, 0x01},
 366	{0x3817, 0x01},
 367	{0x3820, 0x88},
 368	{0x3821, 0x00},
 369	{0x4501, 0x00},
 370	{0x4008, 0x04},
 371	{0x4009, 0x13},
 372	{REG_NULL, 0x00}
 373};
 374
 375/*
 376 * Xclk 24Mhz
 377 * Pclk 45Mhz
 378 * linelength 740(0x02e4)
 379 * framelength 1012(0x03f4)
 380 * grabwindow_width 1296
 381 * grabwindow_height 972
 382 * max_framerate 60fps
 383 * mipi_datarate per lane 840Mbps
 384 */
 385static const struct regval ov5695_1296x972_regs[] = {
 386	{0x0103, 0x01},
 387	{0x0100, 0x00},
 388	{0x0300, 0x04},
 389	{0x0301, 0x00},
 390	{0x0302, 0x69},
 391	{0x0303, 0x00},
 392	{0x0304, 0x00},
 393	{0x0305, 0x01},
 394	{0x0307, 0x00},
 395	{0x030b, 0x00},
 396	{0x030c, 0x00},
 397	{0x030d, 0x1e},
 398	{0x030e, 0x04},
 399	{0x030f, 0x03},
 400	{0x0312, 0x01},
 401	{0x3000, 0x00},
 402	{0x3002, 0xa1},
 403	{0x3008, 0x00},
 404	{0x3010, 0x00},
 405	{0x3016, 0x32},
 406	{0x3022, 0x51},
 407	{0x3106, 0x15},
 408	{0x3107, 0x01},
 409	{0x3108, 0x05},
 410	{0x3500, 0x00},
 411	{0x3501, 0x3e},
 412	{0x3502, 0x00},
 413	{0x3503, 0x08},
 414	{0x3504, 0x03},
 415	{0x3505, 0x8c},
 416	{0x3507, 0x03},
 417	{0x3508, 0x00},
 418	{0x3509, 0x10},
 419	{0x350c, 0x00},
 420	{0x350d, 0x80},
 421	{0x3510, 0x00},
 422	{0x3511, 0x02},
 423	{0x3512, 0x00},
 424	{0x3601, 0x55},
 425	{0x3602, 0x58},
 426	{0x3611, 0x58},
 427	{0x3614, 0x30},
 428	{0x3615, 0x77},
 429	{0x3621, 0x08},
 430	{0x3624, 0x40},
 431	{0x3633, 0x0c},
 432	{0x3634, 0x0c},
 433	{0x3635, 0x0c},
 434	{0x3636, 0x0c},
 435	{0x3638, 0x00},
 436	{0x3639, 0x00},
 437	{0x363a, 0x00},
 438	{0x363b, 0x00},
 439	{0x363c, 0xff},
 440	{0x363d, 0xfa},
 441	{0x3650, 0x44},
 442	{0x3651, 0x44},
 443	{0x3652, 0x44},
 444	{0x3653, 0x44},
 445	{0x3654, 0x44},
 446	{0x3655, 0x44},
 447	{0x3656, 0x44},
 448	{0x3657, 0x44},
 449	{0x3660, 0x00},
 450	{0x3661, 0x00},
 451	{0x3662, 0x00},
 452	{0x366a, 0x00},
 453	{0x366e, 0x0c},
 454	{0x3673, 0x04},
 455	{0x3700, 0x14},
 456	{0x3703, 0x0c},
 457	{0x3706, 0x24},
 458	{0x3714, 0x27},
 459	{0x3715, 0x01},
 460	{0x3716, 0x00},
 461	{0x3717, 0x02},
 462	{0x3733, 0x10},
 463	{0x3734, 0x40},
 464	{0x373f, 0xa0},
 465	{0x3765, 0x20},
 466	{0x37a1, 0x1d},
 467	{0x37a8, 0x26},
 468	{0x37ab, 0x14},
 469	{0x37c2, 0x04},
 470	{0x37c3, 0xf0},
 471	{0x37cb, 0x09},
 472	{0x37cc, 0x13},
 473	{0x37cd, 0x1f},
 474	{0x37ce, 0x1f},
 475	{0x3800, 0x00},
 476	{0x3801, 0x00},
 477	{0x3802, 0x00},
 478	{0x3803, 0x00},
 479	{0x3804, 0x0a},
 480	{0x3805, 0x3f},
 481	{0x3806, 0x07},
 482	{0x3807, 0xaf},
 483	{0x3808, 0x05},
 484	{0x3809, 0x10},
 485	{0x380a, 0x03},
 486	{0x380b, 0xcc},
 487	{0x380c, 0x02},
 488	{0x380d, 0xe4},
 489	{0x380e, 0x03},
 490	{0x380f, 0xf4},
 491	{0x3810, 0x00},
 492	{0x3811, 0x00},
 493	{0x3812, 0x00},
 494	{0x3813, 0x06},
 495	{0x3814, 0x03},
 496	{0x3815, 0x01},
 497	{0x3816, 0x03},
 498	{0x3817, 0x01},
 499	{0x3818, 0x00},
 500	{0x3819, 0x00},
 501	{0x381a, 0x00},
 502	{0x381b, 0x01},
 503	{0x3820, 0x8b},
 504	{0x3821, 0x01},
 505	{0x3c80, 0x08},
 506	{0x3c82, 0x00},
 507	{0x3c83, 0x00},
 508	{0x3c88, 0x00},
 509	{0x3d85, 0x14},
 510	{0x3f02, 0x08},
 511	{0x3f03, 0x10},
 512	{0x4008, 0x02},
 513	{0x4009, 0x09},
 514	{0x404e, 0x20},
 515	{0x4501, 0x00},
 516	{0x4502, 0x10},
 517	{0x4800, 0x00},
 518	{0x481f, 0x2a},
 519	{0x4837, 0x13},
 520	{0x5000, 0x13},
 521	{0x5780, 0x3e},
 522	{0x5781, 0x0f},
 523	{0x5782, 0x44},
 524	{0x5783, 0x02},
 525	{0x5784, 0x01},
 526	{0x5785, 0x01},
 527	{0x5786, 0x00},
 528	{0x5787, 0x04},
 529	{0x5788, 0x02},
 530	{0x5789, 0x0f},
 531	{0x578a, 0xfd},
 532	{0x578b, 0xf5},
 533	{0x578c, 0xf5},
 534	{0x578d, 0x03},
 535	{0x578e, 0x08},
 536	{0x578f, 0x0c},
 537	{0x5790, 0x08},
 538	{0x5791, 0x06},
 539	{0x5792, 0x00},
 540	{0x5793, 0x52},
 541	{0x5794, 0xa3},
 542	{0x5b00, 0x00},
 543	{0x5b01, 0x1c},
 544	{0x5b02, 0x00},
 545	{0x5b03, 0x7f},
 546	{0x5b05, 0x6c},
 547	{0x5e10, 0xfc},
 548	{0x4010, 0xf1},
 549	{0x3503, 0x08},
 550	{0x3505, 0x8c},
 551	{0x3507, 0x03},
 552	{0x3508, 0x00},
 553	{0x3509, 0xf8},
 554	{0x0100, 0x01},
 555	{REG_NULL, 0x00}
 556};
 557
 558/*
 559 * Xclk 24Mhz
 560 * Pclk 45Mhz
 561 * linelength 672(0x2a0)
 562 * framelength 2232(0x8b8)
 563 * grabwindow_width 1280
 564 * grabwindow_height 720
 565 * max_framerate 30fps
 566 * mipi_datarate per lane 840Mbps
 567 */
 568static const struct regval ov5695_1280x720_regs[] = {
 569	{0x3501, 0x45},
 570	{0x366e, 0x0c},
 571	{0x3800, 0x00},
 572	{0x3801, 0x00},
 573	{0x3802, 0x01},
 574	{0x3803, 0x00},
 575	{0x3804, 0x0a},
 576	{0x3805, 0x3f},
 577	{0x3806, 0x06},
 578	{0x3807, 0xaf},
 579	{0x3808, 0x05},
 580	{0x3809, 0x00},
 581	{0x380a, 0x02},
 582	{0x380b, 0xd0},
 583	{0x380c, 0x02},
 584	{0x380d, 0xa0},
 585	{0x380e, 0x08},
 586	{0x380f, 0xb8},
 587	{0x3811, 0x06},
 588	{0x3813, 0x02},
 589	{0x3814, 0x03},
 590	{0x3816, 0x03},
 591	{0x3817, 0x01},
 592	{0x3820, 0x8b},
 593	{0x3821, 0x01},
 594	{0x4501, 0x00},
 595	{0x4008, 0x02},
 596	{0x4009, 0x09},
 597	{REG_NULL, 0x00}
 598};
 599
 600/*
 601 * Xclk 24Mhz
 602 * Pclk 45Mhz
 603 * linelength 672(0x2a0)
 604 * framelength 558(0x22e)
 605 * grabwindow_width 640
 606 * grabwindow_height 480
 607 * max_framerate 120fps
 608 * mipi_datarate per lane 840Mbps
 609 */
 610static const struct regval ov5695_640x480_regs[] = {
 611	{0x3501, 0x22},
 612	{0x366e, 0x0c},
 613	{0x3800, 0x00},
 614	{0x3801, 0x00},
 615	{0x3802, 0x00},
 616	{0x3803, 0x08},
 617	{0x3804, 0x0a},
 618	{0x3805, 0x3f},
 619	{0x3806, 0x07},
 620	{0x3807, 0xa7},
 621	{0x3808, 0x02},
 622	{0x3809, 0x80},
 623	{0x380a, 0x01},
 624	{0x380b, 0xe0},
 625	{0x380c, 0x02},
 626	{0x380d, 0xa0},
 627	{0x380e, 0x02},
 628	{0x380f, 0x2e},
 629	{0x3811, 0x06},
 630	{0x3813, 0x04},
 631	{0x3814, 0x07},
 632	{0x3816, 0x05},
 633	{0x3817, 0x03},
 634	{0x3820, 0x8d},
 635	{0x3821, 0x01},
 636	{0x4501, 0x00},
 637	{0x4008, 0x02},
 638	{0x4009, 0x09},
 639	{REG_NULL, 0x00}
 640};
 641
 642static const struct ov5695_mode supported_modes[] = {
 643	{
 644		.width = 2592,
 645		.height = 1944,
 646		.max_fps = 30,
 647		.exp_def = 0x0450,
 648		.hts_def = 0x02e4 * 4,
 649		.vts_def = 0x07e8,
 650		.reg_list = ov5695_2592x1944_regs,
 651	},
 652	{
 653		.width = 1920,
 654		.height = 1080,
 655		.max_fps = 30,
 656		.exp_def = 0x0450,
 657		.hts_def = 0x02a0 * 4,
 658		.vts_def = 0x08b8,
 659		.reg_list = ov5695_1920x1080_regs,
 660	},
 661	{
 662		.width = 1296,
 663		.height = 972,
 664		.max_fps = 60,
 665		.exp_def = 0x03e0,
 666		.hts_def = 0x02e4 * 4,
 667		.vts_def = 0x03f4,
 668		.reg_list = ov5695_1296x972_regs,
 669	},
 670	{
 671		.width = 1280,
 672		.height = 720,
 673		.max_fps = 30,
 674		.exp_def = 0x0450,
 675		.hts_def = 0x02a0 * 4,
 676		.vts_def = 0x08b8,
 677		.reg_list = ov5695_1280x720_regs,
 678	},
 679	{
 680		.width = 640,
 681		.height = 480,
 682		.max_fps = 120,
 683		.exp_def = 0x0450,
 684		.hts_def = 0x02a0 * 4,
 685		.vts_def = 0x022e,
 686		.reg_list = ov5695_640x480_regs,
 687	},
 688};
 689
 690#define OV5695_LINK_FREQ_420MHZ		420000000
 691static const s64 link_freq_menu_items[] = {
 692	OV5695_LINK_FREQ_420MHZ
 693};
 694
 695static const char * const ov5695_test_pattern_menu[] = {
 696	"Disabled",
 697	"Vertical Color Bar Type 1",
 698	"Vertical Color Bar Type 2",
 699	"Vertical Color Bar Type 3",
 700	"Vertical Color Bar Type 4"
 701};
 702
 703/* Write registers up to 4 at a time */
 704static int ov5695_write_reg(struct i2c_client *client, u16 reg,
 705			    u32 len, u32 val)
 706{
 707	u32 buf_i, val_i;
 708	u8 buf[6];
 709	u8 *val_p;
 710	__be32 val_be;
 711
 712	if (len > 4)
 713		return -EINVAL;
 714
 715	buf[0] = reg >> 8;
 716	buf[1] = reg & 0xff;
 717
 718	val_be = cpu_to_be32(val);
 719	val_p = (u8 *)&val_be;
 720	buf_i = 2;
 721	val_i = 4 - len;
 722
 723	while (val_i < 4)
 724		buf[buf_i++] = val_p[val_i++];
 725
 726	if (i2c_master_send(client, buf, len + 2) != len + 2)
 727		return -EIO;
 728
 729	return 0;
 730}
 731
 732static int ov5695_write_array(struct i2c_client *client,
 733			      const struct regval *regs)
 734{
 735	u32 i;
 736	int ret = 0;
 737
 738	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
 739		ret = ov5695_write_reg(client, regs[i].addr,
 740				       OV5695_REG_VALUE_08BIT, regs[i].val);
 741
 742	return ret;
 743}
 744
 745/* Read registers up to 4 at a time */
 746static int ov5695_read_reg(struct i2c_client *client, u16 reg, unsigned int len,
 747			   u32 *val)
 748{
 749	struct i2c_msg msgs[2];
 750	u8 *data_be_p;
 751	__be32 data_be = 0;
 752	__be16 reg_addr_be = cpu_to_be16(reg);
 753	int ret;
 754
 755	if (len > 4)
 756		return -EINVAL;
 757
 758	data_be_p = (u8 *)&data_be;
 759	/* Write register address */
 760	msgs[0].addr = client->addr;
 761	msgs[0].flags = 0;
 762	msgs[0].len = 2;
 763	msgs[0].buf = (u8 *)&reg_addr_be;
 764
 765	/* Read data from register */
 766	msgs[1].addr = client->addr;
 767	msgs[1].flags = I2C_M_RD;
 768	msgs[1].len = len;
 769	msgs[1].buf = &data_be_p[4 - len];
 770
 771	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 772	if (ret != ARRAY_SIZE(msgs))
 773		return -EIO;
 774
 775	*val = be32_to_cpu(data_be);
 776
 777	return 0;
 778}
 779
 780static int ov5695_get_reso_dist(const struct ov5695_mode *mode,
 781				struct v4l2_mbus_framefmt *framefmt)
 782{
 783	return abs(mode->width - framefmt->width) +
 784	       abs(mode->height - framefmt->height);
 785}
 786
 787static const struct ov5695_mode *
 788ov5695_find_best_fit(struct v4l2_subdev_format *fmt)
 789{
 790	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
 791	int dist;
 792	int cur_best_fit = 0;
 793	int cur_best_fit_dist = -1;
 794	int i;
 795
 796	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
 797		dist = ov5695_get_reso_dist(&supported_modes[i], framefmt);
 798		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
 799			cur_best_fit_dist = dist;
 800			cur_best_fit = i;
 801		}
 802	}
 803
 804	return &supported_modes[cur_best_fit];
 805}
 806
 807static int ov5695_set_fmt(struct v4l2_subdev *sd,
 808			  struct v4l2_subdev_state *sd_state,
 809			  struct v4l2_subdev_format *fmt)
 810{
 811	struct ov5695 *ov5695 = to_ov5695(sd);
 812	const struct ov5695_mode *mode;
 813	s64 h_blank, vblank_def;
 814
 815	mutex_lock(&ov5695->mutex);
 816
 817	mode = ov5695_find_best_fit(fmt);
 818	fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 819	fmt->format.width = mode->width;
 820	fmt->format.height = mode->height;
 821	fmt->format.field = V4L2_FIELD_NONE;
 822	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 823		*v4l2_subdev_state_get_format(sd_state, fmt->pad) = fmt->format;
 824	} else {
 825		ov5695->cur_mode = mode;
 826		h_blank = mode->hts_def - mode->width;
 827		__v4l2_ctrl_modify_range(ov5695->hblank, h_blank,
 828					 h_blank, 1, h_blank);
 829		vblank_def = mode->vts_def - mode->height;
 830		__v4l2_ctrl_modify_range(ov5695->vblank, vblank_def,
 831					 OV5695_VTS_MAX - mode->height,
 832					 1, vblank_def);
 833	}
 834
 835	mutex_unlock(&ov5695->mutex);
 836
 837	return 0;
 838}
 839
 840static int ov5695_get_fmt(struct v4l2_subdev *sd,
 841			  struct v4l2_subdev_state *sd_state,
 842			  struct v4l2_subdev_format *fmt)
 843{
 844	struct ov5695 *ov5695 = to_ov5695(sd);
 845	const struct ov5695_mode *mode = ov5695->cur_mode;
 846
 847	mutex_lock(&ov5695->mutex);
 848	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 849		fmt->format = *v4l2_subdev_state_get_format(sd_state,
 850							    fmt->pad);
 851	} else {
 852		fmt->format.width = mode->width;
 853		fmt->format.height = mode->height;
 854		fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 855		fmt->format.field = V4L2_FIELD_NONE;
 856	}
 857	mutex_unlock(&ov5695->mutex);
 858
 859	return 0;
 860}
 861
 862static int ov5695_enum_mbus_code(struct v4l2_subdev *sd,
 863				 struct v4l2_subdev_state *sd_state,
 864				 struct v4l2_subdev_mbus_code_enum *code)
 865{
 866	if (code->index != 0)
 867		return -EINVAL;
 868	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 869
 870	return 0;
 871}
 872
 873static int ov5695_enum_frame_sizes(struct v4l2_subdev *sd,
 874				   struct v4l2_subdev_state *sd_state,
 875				   struct v4l2_subdev_frame_size_enum *fse)
 876{
 877	if (fse->index >= ARRAY_SIZE(supported_modes))
 878		return -EINVAL;
 879
 880	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
 881		return -EINVAL;
 882
 883	fse->min_width  = supported_modes[fse->index].width;
 884	fse->max_width  = supported_modes[fse->index].width;
 885	fse->max_height = supported_modes[fse->index].height;
 886	fse->min_height = supported_modes[fse->index].height;
 887
 888	return 0;
 889}
 890
 891static int ov5695_enable_test_pattern(struct ov5695 *ov5695, u32 pattern)
 892{
 893	u32 val;
 894
 895	if (pattern)
 896		val = (pattern - 1) | OV5695_TEST_PATTERN_ENABLE;
 897	else
 898		val = OV5695_TEST_PATTERN_DISABLE;
 899
 900	return ov5695_write_reg(ov5695->client, OV5695_REG_TEST_PATTERN,
 901				OV5695_REG_VALUE_08BIT, val);
 902}
 903
 904static int __ov5695_start_stream(struct ov5695 *ov5695)
 905{
 906	int ret;
 907
 908	ret = ov5695_write_array(ov5695->client, ov5695_global_regs);
 909	if (ret)
 910		return ret;
 911	ret = ov5695_write_array(ov5695->client, ov5695->cur_mode->reg_list);
 912	if (ret)
 913		return ret;
 914
 915	/* In case these controls are set before streaming */
 916	ret = __v4l2_ctrl_handler_setup(&ov5695->ctrl_handler);
 917	if (ret)
 918		return ret;
 919
 920	return ov5695_write_reg(ov5695->client, OV5695_REG_CTRL_MODE,
 921				OV5695_REG_VALUE_08BIT, OV5695_MODE_STREAMING);
 922}
 923
 924static int __ov5695_stop_stream(struct ov5695 *ov5695)
 925{
 926	return ov5695_write_reg(ov5695->client, OV5695_REG_CTRL_MODE,
 927				OV5695_REG_VALUE_08BIT, OV5695_MODE_SW_STANDBY);
 928}
 929
 930static int ov5695_s_stream(struct v4l2_subdev *sd, int on)
 931{
 932	struct ov5695 *ov5695 = to_ov5695(sd);
 933	struct i2c_client *client = ov5695->client;
 934	int ret = 0;
 935
 936	mutex_lock(&ov5695->mutex);
 937
 938	if (on) {
 939		ret = pm_runtime_resume_and_get(&client->dev);
 940		if (ret < 0)
 941			goto unlock_and_return;
 942
 943		ret = __ov5695_start_stream(ov5695);
 944		if (ret) {
 945			v4l2_err(sd, "start stream failed while write regs\n");
 946			pm_runtime_put(&client->dev);
 947			goto unlock_and_return;
 948		}
 949	} else {
 950		__ov5695_stop_stream(ov5695);
 951		pm_runtime_put(&client->dev);
 952	}
 953
 954unlock_and_return:
 955	mutex_unlock(&ov5695->mutex);
 956
 957	return ret;
 958}
 959
 960static int __ov5695_power_on(struct ov5695 *ov5695)
 961{
 962	int i, ret;
 963	struct device *dev = &ov5695->client->dev;
 964
 965	ret = clk_prepare_enable(ov5695->xvclk);
 966	if (ret < 0) {
 967		dev_err(dev, "Failed to enable xvclk\n");
 968		return ret;
 969	}
 970
 971	gpiod_set_value_cansleep(ov5695->reset_gpio, 1);
 972
 973	/*
 974	 * The hardware requires the regulators to be powered on in order,
 975	 * so enable them one by one.
 976	 */
 977	for (i = 0; i < OV5695_NUM_SUPPLIES; i++) {
 978		ret = regulator_enable(ov5695->supplies[i].consumer);
 979		if (ret) {
 980			dev_err(dev, "Failed to enable %s: %d\n",
 981				ov5695->supplies[i].supply, ret);
 982			goto disable_reg_clk;
 983		}
 984	}
 985
 986	gpiod_set_value_cansleep(ov5695->reset_gpio, 0);
 987
 988	usleep_range(1000, 1200);
 989
 990	return 0;
 991
 992disable_reg_clk:
 993	for (--i; i >= 0; i--)
 994		regulator_disable(ov5695->supplies[i].consumer);
 995	clk_disable_unprepare(ov5695->xvclk);
 996
 997	return ret;
 998}
 999
1000static void __ov5695_power_off(struct ov5695 *ov5695)
1001{
1002	struct device *dev = &ov5695->client->dev;
1003	int i, ret;
1004
1005	clk_disable_unprepare(ov5695->xvclk);
1006	gpiod_set_value_cansleep(ov5695->reset_gpio, 1);
1007
1008	/*
1009	 * The hardware requires the regulators to be powered off in order,
1010	 * so disable them one by one.
1011	 */
1012	for (i = OV5695_NUM_SUPPLIES - 1; i >= 0; i--) {
1013		ret = regulator_disable(ov5695->supplies[i].consumer);
1014		if (ret)
1015			dev_err(dev, "Failed to disable %s: %d\n",
1016				ov5695->supplies[i].supply, ret);
1017	}
1018}
1019
1020static int __maybe_unused ov5695_runtime_resume(struct device *dev)
1021{
1022	struct v4l2_subdev *sd = dev_get_drvdata(dev);
1023	struct ov5695 *ov5695 = to_ov5695(sd);
1024
1025	return __ov5695_power_on(ov5695);
1026}
1027
1028static int __maybe_unused ov5695_runtime_suspend(struct device *dev)
1029{
1030	struct v4l2_subdev *sd = dev_get_drvdata(dev);
1031	struct ov5695 *ov5695 = to_ov5695(sd);
1032
1033	__ov5695_power_off(ov5695);
1034
1035	return 0;
1036}
1037
1038static int ov5695_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1039{
1040	struct ov5695 *ov5695 = to_ov5695(sd);
1041	struct v4l2_mbus_framefmt *try_fmt =
1042				v4l2_subdev_state_get_format(fh->state, 0);
1043	const struct ov5695_mode *def_mode = &supported_modes[0];
1044
1045	mutex_lock(&ov5695->mutex);
1046	/* Initialize try_fmt */
1047	try_fmt->width = def_mode->width;
1048	try_fmt->height = def_mode->height;
1049	try_fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
1050	try_fmt->field = V4L2_FIELD_NONE;
1051
1052	mutex_unlock(&ov5695->mutex);
1053	/* No crop or compose */
1054
1055	return 0;
1056}
1057
1058static const struct dev_pm_ops ov5695_pm_ops = {
1059	SET_RUNTIME_PM_OPS(ov5695_runtime_suspend,
1060			   ov5695_runtime_resume, NULL)
1061};
1062
1063static const struct v4l2_subdev_internal_ops ov5695_internal_ops = {
1064	.open = ov5695_open,
1065};
1066
1067static const struct v4l2_subdev_video_ops ov5695_video_ops = {
1068	.s_stream = ov5695_s_stream,
1069};
1070
1071static const struct v4l2_subdev_pad_ops ov5695_pad_ops = {
1072	.enum_mbus_code = ov5695_enum_mbus_code,
1073	.enum_frame_size = ov5695_enum_frame_sizes,
1074	.get_fmt = ov5695_get_fmt,
1075	.set_fmt = ov5695_set_fmt,
1076};
1077
1078static const struct v4l2_subdev_ops ov5695_subdev_ops = {
1079	.video	= &ov5695_video_ops,
1080	.pad	= &ov5695_pad_ops,
1081};
1082
1083static int ov5695_set_ctrl(struct v4l2_ctrl *ctrl)
1084{
1085	struct ov5695 *ov5695 = container_of(ctrl->handler,
1086					     struct ov5695, ctrl_handler);
1087	struct i2c_client *client = ov5695->client;
1088	s64 max;
1089	int ret = 0;
1090
1091	/* Propagate change of current control to all related controls */
1092	switch (ctrl->id) {
1093	case V4L2_CID_VBLANK:
1094		/* Update max exposure while meeting expected vblanking */
1095		max = ov5695->cur_mode->height + ctrl->val - 4;
1096		__v4l2_ctrl_modify_range(ov5695->exposure,
1097					 ov5695->exposure->minimum, max,
1098					 ov5695->exposure->step,
1099					 ov5695->exposure->default_value);
1100		break;
1101	}
1102
1103	if (!pm_runtime_get_if_in_use(&client->dev))
1104		return 0;
1105
1106	switch (ctrl->id) {
1107	case V4L2_CID_EXPOSURE:
1108		/* 4 least significant bits of exposure are fractional part */
1109		ret = ov5695_write_reg(ov5695->client, OV5695_REG_EXPOSURE,
1110				       OV5695_REG_VALUE_24BIT, ctrl->val << 4);
1111		break;
1112	case V4L2_CID_ANALOGUE_GAIN:
1113		ret = ov5695_write_reg(ov5695->client, OV5695_REG_ANALOG_GAIN,
1114				       OV5695_REG_VALUE_08BIT, ctrl->val);
1115		break;
1116	case V4L2_CID_DIGITAL_GAIN:
1117		ret = ov5695_write_reg(ov5695->client, OV5695_REG_DIGI_GAIN_L,
1118				       OV5695_REG_VALUE_08BIT,
1119				       ctrl->val & OV5695_DIGI_GAIN_L_MASK);
1120		ret = ov5695_write_reg(ov5695->client, OV5695_REG_DIGI_GAIN_H,
1121				       OV5695_REG_VALUE_08BIT,
1122				       ctrl->val >> OV5695_DIGI_GAIN_H_SHIFT);
1123		break;
1124	case V4L2_CID_VBLANK:
1125		ret = ov5695_write_reg(ov5695->client, OV5695_REG_VTS,
1126				       OV5695_REG_VALUE_16BIT,
1127				       ctrl->val + ov5695->cur_mode->height);
1128		break;
1129	case V4L2_CID_TEST_PATTERN:
1130		ret = ov5695_enable_test_pattern(ov5695, ctrl->val);
1131		break;
1132	default:
1133		dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
1134			 __func__, ctrl->id, ctrl->val);
1135		break;
1136	}
1137
1138	pm_runtime_put(&client->dev);
1139
1140	return ret;
1141}
1142
1143static const struct v4l2_ctrl_ops ov5695_ctrl_ops = {
1144	.s_ctrl = ov5695_set_ctrl,
1145};
1146
1147static int ov5695_initialize_controls(struct ov5695 *ov5695)
1148{
1149	const struct ov5695_mode *mode;
1150	struct v4l2_ctrl_handler *handler;
1151	struct v4l2_ctrl *ctrl;
1152	s64 exposure_max, vblank_def;
1153	u32 h_blank;
1154	int ret;
1155
1156	handler = &ov5695->ctrl_handler;
1157	mode = ov5695->cur_mode;
1158	ret = v4l2_ctrl_handler_init(handler, 8);
1159	if (ret)
1160		return ret;
1161	handler->lock = &ov5695->mutex;
1162
1163	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
1164				      0, 0, link_freq_menu_items);
1165	if (ctrl)
1166		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1167
1168	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
1169			  0, OV5695_PIXEL_RATE, 1, OV5695_PIXEL_RATE);
1170
1171	h_blank = mode->hts_def - mode->width;
1172	ov5695->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
1173				h_blank, h_blank, 1, h_blank);
1174	if (ov5695->hblank)
1175		ov5695->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1176
1177	vblank_def = mode->vts_def - mode->height;
1178	ov5695->vblank = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
1179				V4L2_CID_VBLANK, vblank_def,
1180				OV5695_VTS_MAX - mode->height,
1181				1, vblank_def);
1182
1183	exposure_max = mode->vts_def - 4;
1184	ov5695->exposure = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
1185				V4L2_CID_EXPOSURE, OV5695_EXPOSURE_MIN,
1186				exposure_max, OV5695_EXPOSURE_STEP,
1187				mode->exp_def);
1188
1189	ov5695->anal_gain = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
1190				V4L2_CID_ANALOGUE_GAIN, ANALOG_GAIN_MIN,
1191				ANALOG_GAIN_MAX, ANALOG_GAIN_STEP,
1192				ANALOG_GAIN_DEFAULT);
1193
1194	/* Digital gain */
1195	ov5695->digi_gain = v4l2_ctrl_new_std(handler, &ov5695_ctrl_ops,
1196				V4L2_CID_DIGITAL_GAIN, OV5695_DIGI_GAIN_MIN,
1197				OV5695_DIGI_GAIN_MAX, OV5695_DIGI_GAIN_STEP,
1198				OV5695_DIGI_GAIN_DEFAULT);
1199
1200	ov5695->test_pattern = v4l2_ctrl_new_std_menu_items(handler,
1201				&ov5695_ctrl_ops, V4L2_CID_TEST_PATTERN,
1202				ARRAY_SIZE(ov5695_test_pattern_menu) - 1,
1203				0, 0, ov5695_test_pattern_menu);
1204
1205	if (handler->error) {
1206		ret = handler->error;
1207		dev_err(&ov5695->client->dev,
1208			"Failed to init controls(%d)\n", ret);
1209		goto err_free_handler;
1210	}
1211
1212	ov5695->subdev.ctrl_handler = handler;
1213
1214	return 0;
1215
1216err_free_handler:
1217	v4l2_ctrl_handler_free(handler);
1218
1219	return ret;
1220}
1221
1222static int ov5695_check_sensor_id(struct ov5695 *ov5695,
1223				  struct i2c_client *client)
1224{
1225	struct device *dev = &ov5695->client->dev;
1226	u32 id = 0;
1227	int ret;
1228
1229	ret = ov5695_read_reg(client, OV5695_REG_CHIP_ID,
1230			      OV5695_REG_VALUE_24BIT, &id);
1231	if (id != CHIP_ID) {
1232		dev_err(dev, "Unexpected sensor id(%06x), ret(%d)\n", id, ret);
1233		return ret;
1234	}
1235
1236	dev_info(dev, "Detected OV%06x sensor\n", CHIP_ID);
1237
1238	return 0;
1239}
1240
1241static int ov5695_configure_regulators(struct ov5695 *ov5695)
1242{
1243	int i;
1244
1245	for (i = 0; i < OV5695_NUM_SUPPLIES; i++)
1246		ov5695->supplies[i].supply = ov5695_supply_names[i];
1247
1248	return devm_regulator_bulk_get(&ov5695->client->dev,
1249				       OV5695_NUM_SUPPLIES,
1250				       ov5695->supplies);
1251}
1252
1253static int ov5695_probe(struct i2c_client *client)
1254{
1255	struct device *dev = &client->dev;
1256	struct ov5695 *ov5695;
1257	struct v4l2_subdev *sd;
1258	int ret;
1259
1260	ov5695 = devm_kzalloc(dev, sizeof(*ov5695), GFP_KERNEL);
1261	if (!ov5695)
1262		return -ENOMEM;
1263
1264	ov5695->client = client;
1265	ov5695->cur_mode = &supported_modes[0];
1266
1267	ov5695->xvclk = devm_v4l2_sensor_clk_get_legacy(dev, "xvclk", true,
1268							OV5695_XVCLK_FREQ);
1269	if (IS_ERR(ov5695->xvclk))
1270		return dev_err_probe(dev, PTR_ERR(ov5695->xvclk),
1271				     "Failed to get xvclk\n");
1272
1273	if (clk_get_rate(ov5695->xvclk) != OV5695_XVCLK_FREQ)
1274		dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
1275
1276	ov5695->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
1277	if (IS_ERR(ov5695->reset_gpio)) {
1278		dev_err(dev, "Failed to get reset-gpios\n");
1279		return -EINVAL;
1280	}
1281
1282	ret = ov5695_configure_regulators(ov5695);
1283	if (ret) {
1284		dev_err(dev, "Failed to get power regulators\n");
1285		return ret;
1286	}
1287
1288	mutex_init(&ov5695->mutex);
1289
1290	sd = &ov5695->subdev;
1291	v4l2_i2c_subdev_init(sd, client, &ov5695_subdev_ops);
1292	ret = ov5695_initialize_controls(ov5695);
1293	if (ret)
1294		goto err_destroy_mutex;
1295
1296	ret = __ov5695_power_on(ov5695);
1297	if (ret)
1298		goto err_free_handler;
1299
1300	ret = ov5695_check_sensor_id(ov5695, client);
1301	if (ret)
1302		goto err_power_off;
1303
1304	sd->internal_ops = &ov5695_internal_ops;
1305	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1306	ov5695->pad.flags = MEDIA_PAD_FL_SOURCE;
1307	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1308	ret = media_entity_pads_init(&sd->entity, 1, &ov5695->pad);
1309	if (ret < 0)
1310		goto err_power_off;
1311
1312	ret = v4l2_async_register_subdev_sensor(sd);
1313	if (ret) {
1314		dev_err(dev, "v4l2 async register subdev failed\n");
1315		goto err_clean_entity;
1316	}
1317
1318	pm_runtime_set_active(dev);
1319	pm_runtime_enable(dev);
1320	pm_runtime_idle(dev);
1321
1322	return 0;
1323
1324err_clean_entity:
1325	media_entity_cleanup(&sd->entity);
1326err_power_off:
1327	__ov5695_power_off(ov5695);
1328err_free_handler:
1329	v4l2_ctrl_handler_free(&ov5695->ctrl_handler);
1330err_destroy_mutex:
1331	mutex_destroy(&ov5695->mutex);
1332
1333	return ret;
1334}
1335
1336static void ov5695_remove(struct i2c_client *client)
1337{
1338	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1339	struct ov5695 *ov5695 = to_ov5695(sd);
1340
1341	v4l2_async_unregister_subdev(sd);
1342	media_entity_cleanup(&sd->entity);
1343	v4l2_ctrl_handler_free(&ov5695->ctrl_handler);
1344	mutex_destroy(&ov5695->mutex);
1345
1346	pm_runtime_disable(&client->dev);
1347	if (!pm_runtime_status_suspended(&client->dev))
1348		__ov5695_power_off(ov5695);
1349	pm_runtime_set_suspended(&client->dev);
1350}
1351
1352#if IS_ENABLED(CONFIG_OF)
1353static const struct of_device_id ov5695_of_match[] = {
1354	{ .compatible = "ovti,ov5695" },
1355	{},
1356};
1357MODULE_DEVICE_TABLE(of, ov5695_of_match);
1358#endif
1359
1360static struct i2c_driver ov5695_i2c_driver = {
1361	.driver = {
1362		.name = "ov5695",
1363		.pm = &ov5695_pm_ops,
1364		.of_match_table = of_match_ptr(ov5695_of_match),
1365	},
1366	.probe		= ov5695_probe,
1367	.remove		= ov5695_remove,
1368};
1369
1370module_i2c_driver(ov5695_i2c_driver);
1371
1372MODULE_DESCRIPTION("OmniVision ov5695 sensor driver");
1373MODULE_LICENSE("GPL v2");