disable FB

hc

2024-01-04 1543e317f1da31b75942316931e8f491a8920811

 kernel/drivers/media/i2c/ov2680.c
..
..
@@ -1,965 +1,747 @@
1
1
 // SPDX-License-Identifier: GPL-2.0
2
2
 /*
3
- * ov2680 driver
3
+ * Omnivision OV2680 CMOS Image Sensor driver
4
4
  *
5
- * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
6
- * V0.0X01.0X02 fix mclk issue when probe multiple camera.
7
- * V0.0X01.0X03 add enum_frame_interval function.
8
- * V0.0X01.0X04 add quick stream on/off
9
- * V0.0X01.0X05 add function g_mbus_config
5
+ * Copyright (C) 2018 Linaro Ltd
6
+ *
7
+ * Based on OV5640 Sensor Driver
8
+ * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
9
+ * Copyright (C) 2014-2017 Mentor Graphics Inc.
10
+ *
10
11
  */
11
12
 
13
+#include <asm/unaligned.h>
12
14
 #include <linux/clk.h>
13
-#include <linux/device.h>
14
15
 #include <linux/delay.h>
15
-#include <linux/gpio/consumer.h>
16
+#include <linux/err.h>
16
17
 #include <linux/i2c.h>
18
+#include <linux/init.h>
17
19
 #include <linux/module.h>
18
-#include <linux/pm_runtime.h>
19
-#include <linux/of.h>
20
-#include <linux/of_graph.h>
20
+#include <linux/of_device.h>
21
+#include <linux/gpio/consumer.h>
21
22
 #include <linux/regulator/consumer.h>
22
-#include <linux/sysfs.h>
23
-#include <linux/pinctrl/consumer.h>
24
-#include <linux/version.h>
25
-#include <media/v4l2-async.h>
26
-#include <media/media-entity.h>
23
+
27
24
 #include <media/v4l2-common.h>
28
25
 #include <media/v4l2-ctrls.h>
29
-#include <media/v4l2-device.h>
30
-#include <media/v4l2-event.h>
31
-#include <media/v4l2-fwnode.h>
32
-#include <media/v4l2-image-sizes.h>
33
-#include <media/v4l2-mediabus.h>
34
26
 #include <media/v4l2-subdev.h>
35
27
 
36
-#include <linux/rk-camera-module.h>
28
+#define OV2680_XVCLK_VALUE	24000000
37
29
 
38
-/* verify default register values */
39
-//#define CHECK_REG_VALUE
30
+#define OV2680_CHIP_ID		0x2680
40
31
 
41
-#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x5)
32
+#define OV2680_REG_STREAM_CTRL		0x0100
33
+#define OV2680_REG_SOFT_RESET		0x0103
42
34
 
43
-#ifndef V4L2_CID_DIGITAL_GAIN
44
-#define V4L2_CID_DIGITAL_GAIN		V4L2_CID_GAIN
45
-#endif
35
+#define OV2680_REG_CHIP_ID_HIGH		0x300a
36
+#define OV2680_REG_CHIP_ID_LOW		0x300b
46
37
 
47
-/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
48
-#define MIPI_FREQ	330000000U
49
-#define OV2680_PIXEL_RATE		(330000000LL * 1LL * 2LL / 10)
50
-#define OV2680_XVCLK_FREQ		24000000
38
+#define OV2680_REG_R_MANUAL		0x3503
39
+#define OV2680_REG_GAIN_PK		0x350a
40
+#define OV2680_REG_EXPOSURE_PK_HIGH	0x3500
41
+#define OV2680_REG_TIMING_HTS		0x380c
42
+#define OV2680_REG_TIMING_VTS		0x380e
43
+#define OV2680_REG_FORMAT1		0x3820
44
+#define OV2680_REG_FORMAT2		0x3821
51
45
 
52
-#define CHIP_ID				0x2680
53
-#define OV2680_REG_CHIP_ID		0x300a
46
+#define OV2680_REG_ISP_CTRL00		0x5080
54
47
 
55
-#define OV2680_REG_CTRL_MODE		0x0100
56
-#define OV2680_MODE_SW_STANDBY		0x00
57
-#define OV2680_MODE_STREAMING		0x01
48
+#define OV2680_FRAME_RATE		30
58
49
 
59
-#define OV2680_REG_EXPOSURE		0x3500
60
-#define	OV2680_EXPOSURE_MIN		4
61
-#define	OV2680_EXPOSURE_STEP		1
62
-#define OV2680_VTS_MAX			0x7fff
63
-
64
-#define OV2680_REG_ANALOG_GAIN		0x350a
65
-#define	ANALOG_GAIN_MIN			0x10
66
-#define	ANALOG_GAIN_MAX			0xf8
67
-#define	ANALOG_GAIN_STEP		1
68
-#define	ANALOG_GAIN_DEFAULT		0xf8
69
-#define OV2680_REG_GAIN_H		0x350a
70
-#define OV2680_REG_GAIN_L		0x350b
71
-#define OV2680_GAIN_L_MASK		0xff
72
-#define OV2680_GAIN_H_MASK		0x07
73
-#define OV2680_DIGI_GAIN_H_SHIFT	8
74
-
75
-#define OV2680_DIGI_GAIN_MIN		0
76
-#define OV2680_DIGI_GAIN_MAX		(0x4000 - 1)
77
-#define OV2680_DIGI_GAIN_STEP		1
78
-#define OV2680_DIGI_GAIN_DEFAULT	1024
79
-
80
-#define OV2680_REG_TEST_PATTERN		0x5080
81
-#define	OV2680_TEST_PATTERN_ENABLE	0x80
82
-#define	OV2680_TEST_PATTERN_DISABLE	0x0
83
-
84
-#define OV2680_REG_VTS			0x380e
85
-
86
-#define REG_NULL			0xFFFF
87
-
88
-#define OV2680_REG_VALUE_08BIT		1
50
+#define OV2680_REG_VALUE_8BIT		1
89
51
 #define OV2680_REG_VALUE_16BIT		2
90
52
 #define OV2680_REG_VALUE_24BIT		3
91
53
 
92
-#define OV2680_LANES			1
93
-#define OV2680_BITS_PER_SAMPLE		10
54
+#define OV2680_WIDTH_MAX		1600
55
+#define OV2680_HEIGHT_MAX		1200
94
56
 
95
-#define OF_CAMERA_PINCTRL_STATE_DEFAULT	"rockchip,camera_default"
96
-#define OF_CAMERA_PINCTRL_STATE_SLEEP	"rockchip,camera_sleep"
97
-
98
-#define OV2680_NAME			"ov2680"
99
-
100
-static const char * const ov2680_supply_names[] = {
101
-	"avdd",		/* Analog power */
102
-	"dovdd",	/* Digital I/O power */
103
-	"dvdd",		/* Digital core power */
57
+enum ov2680_mode_id {
58
+	OV2680_MODE_QUXGA_800_600,
59
+	OV2680_MODE_720P_1280_720,
60
+	OV2680_MODE_UXGA_1600_1200,
61
+	OV2680_MODE_MAX,
104
62
 };
105
63
 
106
-#define OV2680_NUM_SUPPLIES ARRAY_SIZE(ov2680_supply_names)
107
-
108
-struct regval {
109
-	u16 addr;
64
+struct reg_value {
65
+	u16 reg_addr;
110
66
 	u8 val;
111
67
 };
112
68
 
113
-struct ov2680_mode {
69
+static const char * const ov2680_supply_name[] = {
70
+	"DOVDD",
71
+	"DVDD",
72
+	"AVDD",
73
+};
74
+
75
+#define OV2680_NUM_SUPPLIES ARRAY_SIZE(ov2680_supply_name)
76
+
77
+struct ov2680_mode_info {
78
+	const char *name;
79
+	enum ov2680_mode_id id;
114
80
 	u32 width;
115
81
 	u32 height;
116
-	struct v4l2_fract max_fps;
117
-	u32 hts_def;
118
-	u32 vts_def;
119
-	u32 exp_def;
120
-	const struct regval *reg_list;
82
+	const struct reg_value *reg_data;
83
+	u32 reg_data_size;
121
84
 };
122
85
 
123
-struct ov2680 {
124
-	struct i2c_client	*client;
125
-	struct clk		*xvclk;
126
-	struct gpio_desc	*power_gpio;
127
-	struct gpio_desc	*reset_gpio;
128
-	struct gpio_desc	*pwdn_gpio;
129
-	struct regulator_bulk_data supplies[OV2680_NUM_SUPPLIES];
130
-
131
-	struct pinctrl		*pinctrl;
132
-	struct pinctrl_state	*pins_default;
133
-	struct pinctrl_state	*pins_sleep;
134
-
135
-	struct v4l2_subdev	subdev;
136
-	struct media_pad	pad;
137
-	struct v4l2_ctrl_handler ctrl_handler;
138
-	struct v4l2_ctrl	*exposure;
139
-	struct v4l2_ctrl	*anal_gain;
140
-	struct v4l2_ctrl	*digi_gain;
141
-	struct v4l2_ctrl	*hblank;
142
-	struct v4l2_ctrl	*vblank;
143
-	struct v4l2_ctrl	*test_pattern;
144
-	struct mutex		mutex;
145
-	bool			streaming;
146
-	const struct ov2680_mode *cur_mode;
147
-	unsigned int lane_num;
148
-	unsigned int cfg_num;
149
-	unsigned int pixel_rate;
150
-	u32			module_index;
151
-	const char		*module_facing;
152
-	const char		*module_name;
153
-	const char		*len_name;
86
+struct ov2680_ctrls {
87
+	struct v4l2_ctrl_handler handler;
88
+	struct v4l2_ctrl *exposure;
89
+	struct v4l2_ctrl *gain;
90
+	struct v4l2_ctrl *hflip;
91
+	struct v4l2_ctrl *vflip;
92
+	struct v4l2_ctrl *test_pattern;
154
93
 };
155
94
 
156
-#define to_ov2680(sd) container_of(sd, struct ov2680, subdev)
95
+struct ov2680_dev {
96
+	struct i2c_client		*i2c_client;
97
+	struct v4l2_subdev		sd;
157
98
 
158
-/*
159
- * Xclk 24Mhz
160
- * Pclk 66Mhz
161
- * linelength 1700(0x6a4)
162
- * framelength 1294(0x50e)
163
- * grabwindow_width 1600
164
- * grabwindow_height 1200
165
- * max_framerate 30fps
166
- * mipi_datarate per lane 660Mbps
167
- */
168
-static const struct regval ov2680_global_regs[] = {
169
-	{0x0100, 0x00},
170
-	{0x0103, 0x01},
171
-	{0x3002, 0x00},
172
-	{0x3016, 0x1c},
173
-	{0x3018, 0x44},
174
-	{0x3020, 0x00},
175
-	{0x3080, 0x02},
176
-	{0x3082, 0x37},
177
-	{0x3084, 0x09},
178
-	{0x3085, 0x04},
179
-	{0x3086, 0x01},
180
-	{0x3501, 0x26},
181
-	{0x3502, 0x40},
182
-	{0x3503, 0x03},
183
-	{0x350b, 0x36},
184
-	{0x3600, 0xb4},
185
-	{0x3603, 0x35},
186
-	{0x3604, 0x24},
187
-	{0x3605, 0x00},
188
-	{0x3620, 0x26},
189
-	{0x3621, 0x37},
190
-	{0x3622, 0x04},
191
-	{0x3628, 0x00},
192
-	{0x3705, 0x3c},
193
-	{0x370c, 0x50},
194
-	{0x370d, 0xc0},
195
-	{0x3718, 0x88},
196
-	{0x3720, 0x00},
197
-	{0x3721, 0x00},
198
-	{0x3722, 0x00},
199
-	{0x3723, 0x00},
200
-	{0x3738, 0x00},
201
-	{0x370a, 0x23},
202
-	{0x3717, 0x58},
203
-	{0x3781, 0x80},
204
-	{0x3784, 0x0c},
205
-	{0x3789, 0x60},
206
-	{0x3800, 0x00},
207
-	{0x3801, 0x00},
208
-	{0x3802, 0x00},
209
-	{0x3803, 0x00},
210
-	{0x3804, 0x06},
211
-	{0x3805, 0x4f},
212
-	{0x3806, 0x04},
213
-	{0x3807, 0xbf},
214
-	{0x3808, 0x03},
215
-	{0x3809, 0x20},
216
-	{0x380a, 0x02},
217
-	{0x380b, 0x58},
218
-	{0x380c, 0x06},
219
-	{0x380d, 0xac},
220
-	{0x380e, 0x02},
221
-	{0x380f, 0x84},
222
-	{0x3810, 0x00},
223
-	{0x3811, 0x04},
224
-	{0x3812, 0x00},
225
-	{0x3813, 0x04},
226
-	{0x3814, 0x31},
227
-	{0x3815, 0x31},
228
-	{0x3819, 0x04},
229
-	{0x3820, 0xc2},
230
-	{0x3821, 0x01},
231
-	{0x4000, 0x81},
232
-	{0x4001, 0x40},
233
-	{0x4008, 0x00},
234
-	{0x4009, 0x03},
235
-	{0x4602, 0x02},
236
-	{0x481f, 0x36},
237
-	{0x4825, 0x36},
238
-	{0x4837, 0x30},
239
-	{0x5002, 0x30},
240
-	{0x5080, 0x00},
241
-	{0x5081, 0x41},
242
-	{REG_NULL, 0x00},
243
-	//{0x0100, 0x01},
99
+	struct media_pad		pad;
100
+	struct clk			*xvclk;
101
+	u32				xvclk_freq;
102
+	struct regulator_bulk_data	supplies[OV2680_NUM_SUPPLIES];
244
103
 
245
-	{REG_NULL, 0x00},
104
+	struct gpio_desc		*reset_gpio;
105
+	struct mutex			lock; /* protect members */
106
+
107
+	bool				mode_pending_changes;
108
+	bool				is_enabled;
109
+	bool				is_streaming;
110
+
111
+	struct ov2680_ctrls		ctrls;
112
+	struct v4l2_mbus_framefmt	fmt;
113
+	struct v4l2_fract		frame_interval;
114
+
115
+	const struct ov2680_mode_info	*current_mode;
246
116
 };
247
117
 
248
-/*
249
- * Xclk 24Mhz
250
- * Pclk 66Mhz
251
- * linelength 1700(0x6a4)
252
- * framelength 1294(0x50e)
253
- * grabwindow_width 1600
254
- * grabwindow_height 1200
255
- * max_framerate 30fps
256
- * mipi_datarate per lane 660Mbps
257
- */
258
-static const struct regval ov2680_1600x1200_regs[] = {
259
-	// 1600x1200 30fps 1 lane MIPI 660Mbps/lane
260
-	{0x3086, 0x00},
261
-	{0x3620, 0x26},
262
-	{0x3621, 0x37},
263
-	{0x3622, 0x04},
264
-	{0x370a, 0x21},
265
-	{0x370d, 0xc0},
266
-	{0x3718, 0x88},
267
-	{0x3721, 0x00},
268
-	{0x3722, 0x00},
269
-	{0x3723, 0x00},
270
-	{0x3738, 0x00},
271
-	{0x3803, 0x00},
272
-	{0x3807, 0xbf},
273
-	{0x3808, 0x06},
274
-	{0x3809, 0x40},
275
-	{0x380a, 0x04},
276
-	{0x380b, 0xb0},
277
-	{0x380c, 0x06},
278
-	{0x380d, 0xa4},
279
-	{0x380e, 0x05},
280
-	{0x380f, 0x0e},
281
-	{0x3811, 0x08},
282
-	{0x3813, 0x08},
283
-	{0x3814, 0x11},
284
-	{0x3815, 0x11},
285
-	{0x3820, 0xc0},
286
-	{0x3821, 0x00},
287
-	{0x4008, 0x02},
288
-	{0x4009, 0x09},
289
-	{0x4837, 0x18},
290
-	//{0x0100, 0x01},
291
-
292
-	{REG_NULL, 0x00},
293
-};
294
-
295
-static const struct ov2680_mode supported_modes_1lane[] = {
296
-	{
297
-		.width = 1600,
298
-		.height = 1200,
299
-		.max_fps = {
300
-			.numerator = 10000,
301
-			.denominator = 300000,
302
-		},
303
-		.exp_def = 0x0500,
304
-		.hts_def = 0x06a4,
305
-		.vts_def = 0x050e,
306
-		.reg_list = ov2680_1600x1200_regs,
307
-	},
308
-};
309
-
310
-static const struct ov2680_mode *supported_modes;
311
-
312
-static const s64 link_freq_menu_items[] = {
313
-	MIPI_FREQ
314
-};
315
-
316
-static const char * const ov2680_test_pattern_menu[] = {
118
+static const char * const test_pattern_menu[] = {
317
119
 	"Disabled",
318
-	"Vertical Color Bar Type 1",
319
-	"Vertical Color Bar Type 2",
320
-	"Vertical Color Bar Type 3",
321
-	"Vertical Color Bar Type 4"
120
+	"Color Bars",
121
+	"Random Data",
122
+	"Square",
123
+	"Black Image",
322
124
 };
323
125
 
324
-/* Write registers up to 4 at a time */
325
-static int ov2680_write_reg(struct i2c_client *client, u16 reg,
326
-			    u32 len, u32 val)
327
-{
328
-	u32 buf_i, val_i;
329
-	u8 buf[6];
330
-	u8 *val_p;
331
-	__be32 val_be;
126
+static const int ov2680_hv_flip_bayer_order[] = {
127
+	MEDIA_BUS_FMT_SBGGR10_1X10,
128
+	MEDIA_BUS_FMT_SGRBG10_1X10,
129
+	MEDIA_BUS_FMT_SGBRG10_1X10,
130
+	MEDIA_BUS_FMT_SRGGB10_1X10,
131
+};
332
132
 
333
-	dev_dbg(&client->dev, "write reg(0x%x val:0x%06x)!\n", reg, val);
133
+static const struct reg_value ov2680_setting_30fps_QUXGA_800_600[] = {
134
+	{0x3086, 0x01}, {0x370a, 0x23}, {0x3808, 0x03}, {0x3809, 0x20},
135
+	{0x380a, 0x02}, {0x380b, 0x58}, {0x380c, 0x06}, {0x380d, 0xac},
136
+	{0x380e, 0x02}, {0x380f, 0x84}, {0x3811, 0x04}, {0x3813, 0x04},
137
+	{0x3814, 0x31}, {0x3815, 0x31}, {0x3820, 0xc0}, {0x4008, 0x00},
138
+	{0x4009, 0x03}, {0x4837, 0x1e}, {0x3501, 0x4e}, {0x3502, 0xe0},
139
+	{0x3503, 0x03},
140
+};
141
+
142
+static const struct reg_value ov2680_setting_30fps_720P_1280_720[] = {
143
+	{0x3086, 0x00}, {0x3808, 0x05}, {0x3809, 0x00}, {0x380a, 0x02},
144
+	{0x380b, 0xd0}, {0x380c, 0x06}, {0x380d, 0xa8}, {0x380e, 0x05},
145
+	{0x380f, 0x0e}, {0x3811, 0x08}, {0x3813, 0x06}, {0x3814, 0x11},
146
+	{0x3815, 0x11}, {0x3820, 0xc0}, {0x4008, 0x00},
147
+};
148
+
149
+static const struct reg_value ov2680_setting_30fps_UXGA_1600_1200[] = {
150
+	{0x3086, 0x00}, {0x3501, 0x4e}, {0x3502, 0xe0}, {0x3808, 0x06},
151
+	{0x3809, 0x40}, {0x380a, 0x04}, {0x380b, 0xb0}, {0x380c, 0x06},
152
+	{0x380d, 0xa8}, {0x380e, 0x05}, {0x380f, 0x0e}, {0x3811, 0x00},
153
+	{0x3813, 0x00}, {0x3814, 0x11}, {0x3815, 0x11}, {0x3820, 0xc0},
154
+	{0x4008, 0x00}, {0x4837, 0x18}
155
+};
156
+
157
+static const struct ov2680_mode_info ov2680_mode_init_data = {
158
+	"mode_quxga_800_600", OV2680_MODE_QUXGA_800_600, 800, 600,
159
+	ov2680_setting_30fps_QUXGA_800_600,
160
+	ARRAY_SIZE(ov2680_setting_30fps_QUXGA_800_600),
161
+};
162
+
163
+static const struct ov2680_mode_info ov2680_mode_data[OV2680_MODE_MAX] = {
164
+	{"mode_quxga_800_600", OV2680_MODE_QUXGA_800_600,
165
+	 800, 600, ov2680_setting_30fps_QUXGA_800_600,
166
+	 ARRAY_SIZE(ov2680_setting_30fps_QUXGA_800_600)},
167
+	{"mode_720p_1280_720", OV2680_MODE_720P_1280_720,
168
+	 1280, 720, ov2680_setting_30fps_720P_1280_720,
169
+	 ARRAY_SIZE(ov2680_setting_30fps_720P_1280_720)},
170
+	{"mode_uxga_1600_1200", OV2680_MODE_UXGA_1600_1200,
171
+	 1600, 1200, ov2680_setting_30fps_UXGA_1600_1200,
172
+	 ARRAY_SIZE(ov2680_setting_30fps_UXGA_1600_1200)},
173
+};
174
+
175
+static struct ov2680_dev *to_ov2680_dev(struct v4l2_subdev *sd)
176
+{
177
+	return container_of(sd, struct ov2680_dev, sd);
178
+}
179
+
180
+static struct device *ov2680_to_dev(struct ov2680_dev *sensor)
181
+{
182
+	return &sensor->i2c_client->dev;
183
+}
184
+
185
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
186
+{
187
+	return &container_of(ctrl->handler, struct ov2680_dev,
188
+			     ctrls.handler)->sd;
189
+}
190
+
191
+static int __ov2680_write_reg(struct ov2680_dev *sensor, u16 reg,
192
+			      unsigned int len, u32 val)
193
+{
194
+	struct i2c_client *client = sensor->i2c_client;
195
+	u8 buf[6];
196
+	int ret;
334
197
 
335
198
 	if (len > 4)
336
199
 		return -EINVAL;
337
200
 
338
-	buf[0] = reg >> 8;
339
-	buf[1] = reg & 0xff;
340
-
341
-	val_be = cpu_to_be32(val);
342
-	val_p = (u8 *)&val_be;
343
-	buf_i = 2;
344
-	val_i = 4 - len;
345
-
346
-	while (val_i < 4)
347
-		buf[buf_i++] = val_p[val_i++];
348
-
349
-	if (i2c_master_send(client, buf, len + 2) != len + 2) {
350
-		dev_err(&client->dev,
351
-			   "write reg(0x%x val:0x%x)failed !\n", reg, val);
201
+	put_unaligned_be16(reg, buf);
202
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
203
+	ret = i2c_master_send(client, buf, len + 2);
204
+	if (ret != len + 2) {
205
+		dev_err(&client->dev, "write error: reg=0x%4x: %d\n", reg, ret);
352
206
 		return -EIO;
353
207
 	}
208
+
354
209
 	return 0;
355
210
 }
356
211
 
357
-static int ov2680_write_array(struct i2c_client *client,
358
-			      const struct regval *regs)
212
+#define ov2680_write_reg(s, r, v) \
213
+	__ov2680_write_reg(s, r, OV2680_REG_VALUE_8BIT, v)
214
+
215
+#define ov2680_write_reg16(s, r, v) \
216
+	__ov2680_write_reg(s, r, OV2680_REG_VALUE_16BIT, v)
217
+
218
+#define ov2680_write_reg24(s, r, v) \
219
+	__ov2680_write_reg(s, r, OV2680_REG_VALUE_24BIT, v)
220
+
221
+static int __ov2680_read_reg(struct ov2680_dev *sensor, u16 reg,
222
+			     unsigned int len, u32 *val)
359
223
 {
360
-	u32 i;
361
-	int ret = 0;
362
-
363
-	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
364
-		ret = ov2680_write_reg(client, regs[i].addr,
365
-				       OV2680_REG_VALUE_08BIT, regs[i].val);
366
-
367
-	return ret;
368
-}
369
-
370
-/* Read registers up to 4 at a time */
371
-static int ov2680_read_reg(struct i2c_client *client, u16 reg,
372
-					unsigned int len, u32 *val)
373
-{
224
+	struct i2c_client *client = sensor->i2c_client;
374
225
 	struct i2c_msg msgs[2];
375
-	u8 *data_be_p;
376
-	__be32 data_be = 0;
377
-	__be16 reg_addr_be = cpu_to_be16(reg);
226
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
227
+	u8 data_buf[4] = { 0, };
378
228
 	int ret;
379
229
 
380
-	if (len > 4 || !len)
230
+	if (len > 4)
381
231
 		return -EINVAL;
382
232
 
383
-	data_be_p = (u8 *)&data_be;
384
-	/* Write register address */
385
233
 	msgs[0].addr = client->addr;
386
234
 	msgs[0].flags = 0;
387
-	msgs[0].len = 2;
388
-	msgs[0].buf = (u8 *)&reg_addr_be;
235
+	msgs[0].len = ARRAY_SIZE(addr_buf);
236
+	msgs[0].buf = addr_buf;
389
237
 
390
-	/* Read data from register */
391
238
 	msgs[1].addr = client->addr;
392
239
 	msgs[1].flags = I2C_M_RD;
393
240
 	msgs[1].len = len;
394
-	msgs[1].buf = &data_be_p[4 - len];
241
+	msgs[1].buf = &data_buf[4 - len];
395
242
 
396
243
 	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
397
-	if (ret != ARRAY_SIZE(msgs))
244
+	if (ret != ARRAY_SIZE(msgs)) {
245
+		dev_err(&client->dev, "read error: reg=0x%4x: %d\n", reg, ret);
398
246
 		return -EIO;
247
+	}
399
248
 
400
-	*val = be32_to_cpu(data_be);
249
+	*val = get_unaligned_be32(data_buf);
401
250
 
402
251
 	return 0;
403
252
 }
404
253
 
405
-/* Check Register value */
406
-#ifdef CHECK_REG_VALUE
407
-static int ov2680_reg_verify(struct i2c_client *client,
408
-				const struct regval *regs)
409
-{
410
-	u32 i;
411
-	int ret = 0;
412
-	u32 value;
254
+#define ov2680_read_reg(s, r, v) \
255
+	__ov2680_read_reg(s, r, OV2680_REG_VALUE_8BIT, v)
413
256
 
414
-	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++) {
415
-		ret = ov2680_read_reg(client, regs[i].addr,
416
-			  OV2680_REG_VALUE_08BIT, &value);
417
-		if (value != regs[i].val) {
418
-			dev_info(&client->dev, "%s:0x%04x is 0x%08x instead of 0x%08x\n",
419
-				 __func__, regs[i].addr, value, regs[i].val);
420
-		}
257
+#define ov2680_read_reg16(s, r, v) \
258
+	__ov2680_read_reg(s, r, OV2680_REG_VALUE_16BIT, v)
259
+
260
+#define ov2680_read_reg24(s, r, v) \
261
+	__ov2680_read_reg(s, r, OV2680_REG_VALUE_24BIT, v)
262
+
263
+static int ov2680_mod_reg(struct ov2680_dev *sensor, u16 reg, u8 mask, u8 val)
264
+{
265
+	u32 readval;
266
+	int ret;
267
+
268
+	ret = ov2680_read_reg(sensor, reg, &readval);
269
+	if (ret < 0)
270
+		return ret;
271
+
272
+	readval &= ~mask;
273
+	val &= mask;
274
+	val |= readval;
275
+
276
+	return ov2680_write_reg(sensor, reg, val);
277
+}
278
+
279
+static int ov2680_load_regs(struct ov2680_dev *sensor,
280
+			    const struct ov2680_mode_info *mode)
281
+{
282
+	const struct reg_value *regs = mode->reg_data;
283
+	unsigned int i;
284
+	int ret = 0;
285
+	u16 reg_addr;
286
+	u8 val;
287
+
288
+	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
289
+		reg_addr = regs->reg_addr;
290
+		val = regs->val;
291
+
292
+		ret = ov2680_write_reg(sensor, reg_addr, val);
293
+		if (ret)
294
+			break;
421
295
 	}
296
+
422
297
 	return ret;
423
298
 }
424
-#endif
425
299
 
426
-static int ov2680_get_reso_dist(const struct ov2680_mode *mode,
427
-				struct v4l2_mbus_framefmt *framefmt)
300
+static void ov2680_power_up(struct ov2680_dev *sensor)
428
301
 {
429
-	return abs(mode->width - framefmt->width) +
430
-	       abs(mode->height - framefmt->height);
302
+	if (!sensor->reset_gpio)
303
+		return;
304
+
305
+	gpiod_set_value(sensor->reset_gpio, 0);
306
+	usleep_range(5000, 10000);
431
307
 }
432
308
 
433
-static const struct ov2680_mode *
434
-ov2680_find_best_fit(struct ov2680 *ov2680,
435
-			struct v4l2_subdev_format *fmt)
309
+static void ov2680_power_down(struct ov2680_dev *sensor)
436
310
 {
437
-	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
438
-	int dist;
439
-	int cur_best_fit = 0;
440
-	int cur_best_fit_dist = -1;
441
-	unsigned int i;
311
+	if (!sensor->reset_gpio)
312
+		return;
442
313
 
443
-	for (i = 0; i < ov2680->cfg_num; i++) {
444
-		dist = ov2680_get_reso_dist(&supported_modes[i], framefmt);
445
-		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
446
-			cur_best_fit_dist = dist;
447
-			cur_best_fit = i;
314
+	gpiod_set_value(sensor->reset_gpio, 1);
315
+	usleep_range(5000, 10000);
316
+}
317
+
318
+static void ov2680_set_bayer_order(struct ov2680_dev *sensor)
319
+{
320
+	int hv_flip = 0;
321
+
322
+	if (sensor->ctrls.vflip && sensor->ctrls.vflip->val)
323
+		hv_flip += 1;
324
+
325
+	if (sensor->ctrls.hflip && sensor->ctrls.hflip->val)
326
+		hv_flip += 2;
327
+
328
+	sensor->fmt.code = ov2680_hv_flip_bayer_order[hv_flip];
329
+}
330
+
331
+static int ov2680_set_vflip(struct ov2680_dev *sensor, s32 val)
332
+{
333
+	int ret;
334
+
335
+	if (sensor->is_streaming)
336
+		return -EBUSY;
337
+
338
+	ret = ov2680_mod_reg(sensor, OV2680_REG_FORMAT1,
339
+			     BIT(2), val ? BIT(2) : 0);
340
+	if (ret < 0)
341
+		return ret;
342
+
343
+	ov2680_set_bayer_order(sensor);
344
+	return 0;
345
+}
346
+
347
+static int ov2680_set_hflip(struct ov2680_dev *sensor, s32 val)
348
+{
349
+	int ret;
350
+
351
+	if (sensor->is_streaming)
352
+		return -EBUSY;
353
+
354
+	ret = ov2680_mod_reg(sensor, OV2680_REG_FORMAT2,
355
+			     BIT(2), val ? BIT(2) : 0);
356
+	if (ret < 0)
357
+		return ret;
358
+
359
+	ov2680_set_bayer_order(sensor);
360
+	return 0;
361
+}
362
+
363
+static int ov2680_test_pattern_set(struct ov2680_dev *sensor, int value)
364
+{
365
+	int ret;
366
+
367
+	if (!value)
368
+		return ov2680_mod_reg(sensor, OV2680_REG_ISP_CTRL00, BIT(7), 0);
369
+
370
+	ret = ov2680_mod_reg(sensor, OV2680_REG_ISP_CTRL00, 0x03, value - 1);
371
+	if (ret < 0)
372
+		return ret;
373
+
374
+	ret = ov2680_mod_reg(sensor, OV2680_REG_ISP_CTRL00, BIT(7), BIT(7));
375
+	if (ret < 0)
376
+		return ret;
377
+
378
+	return 0;
379
+}
380
+
381
+static int ov2680_gain_set(struct ov2680_dev *sensor, u32 gain)
382
+{
383
+	return ov2680_write_reg16(sensor, OV2680_REG_GAIN_PK, gain);
384
+}
385
+
386
+static int ov2680_exposure_set(struct ov2680_dev *sensor, u32 exp)
387
+{
388
+	return ov2680_write_reg24(sensor, OV2680_REG_EXPOSURE_PK_HIGH,
389
+				  exp << 4);
390
+}
391
+
392
+static int ov2680_stream_enable(struct ov2680_dev *sensor)
393
+{
394
+	return ov2680_write_reg(sensor, OV2680_REG_STREAM_CTRL, 1);
395
+}
396
+
397
+static int ov2680_stream_disable(struct ov2680_dev *sensor)
398
+{
399
+	return ov2680_write_reg(sensor, OV2680_REG_STREAM_CTRL, 0);
400
+}
401
+
402
+static int ov2680_mode_set(struct ov2680_dev *sensor)
403
+{
404
+	int ret;
405
+
406
+	ret = ov2680_load_regs(sensor, sensor->current_mode);
407
+	if (ret < 0)
408
+		return ret;
409
+
410
+	/* Restore value of all ctrls */
411
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
412
+	if (ret < 0)
413
+		return ret;
414
+
415
+	sensor->mode_pending_changes = false;
416
+
417
+	return 0;
418
+}
419
+
420
+static int ov2680_mode_restore(struct ov2680_dev *sensor)
421
+{
422
+	int ret;
423
+
424
+	ret = ov2680_load_regs(sensor, &ov2680_mode_init_data);
425
+	if (ret < 0)
426
+		return ret;
427
+
428
+	return ov2680_mode_set(sensor);
429
+}
430
+
431
+static int ov2680_power_off(struct ov2680_dev *sensor)
432
+{
433
+	if (!sensor->is_enabled)
434
+		return 0;
435
+
436
+	clk_disable_unprepare(sensor->xvclk);
437
+	ov2680_power_down(sensor);
438
+	regulator_bulk_disable(OV2680_NUM_SUPPLIES, sensor->supplies);
439
+	sensor->is_enabled = false;
440
+
441
+	return 0;
442
+}
443
+
444
+static int ov2680_power_on(struct ov2680_dev *sensor)
445
+{
446
+	struct device *dev = ov2680_to_dev(sensor);
447
+	int ret;
448
+
449
+	if (sensor->is_enabled)
450
+		return 0;
451
+
452
+	ret = regulator_bulk_enable(OV2680_NUM_SUPPLIES, sensor->supplies);
453
+	if (ret < 0) {
454
+		dev_err(dev, "failed to enable regulators: %d\n", ret);
455
+		return ret;
456
+	}
457
+
458
+	if (!sensor->reset_gpio) {
459
+		ret = ov2680_write_reg(sensor, OV2680_REG_SOFT_RESET, 0x01);
460
+		if (ret != 0) {
461
+			dev_err(dev, "sensor soft reset failed\n");
462
+			goto err_disable_regulators;
448
463
 		}
464
+		usleep_range(1000, 2000);
465
+	} else {
466
+		ov2680_power_down(sensor);
467
+		ov2680_power_up(sensor);
449
468
 	}
450
469
 
451
-	return &supported_modes[cur_best_fit];
470
+	ret = clk_prepare_enable(sensor->xvclk);
471
+	if (ret < 0)
472
+		goto err_disable_regulators;
473
+
474
+	sensor->is_enabled = true;
475
+
476
+	/* Set clock lane into LP-11 state */
477
+	ov2680_stream_enable(sensor);
478
+	usleep_range(1000, 2000);
479
+	ov2680_stream_disable(sensor);
480
+
481
+	return 0;
482
+
483
+err_disable_regulators:
484
+	regulator_bulk_disable(OV2680_NUM_SUPPLIES, sensor->supplies);
485
+	return ret;
452
486
 }
453
487
 
454
-static int ov2680_set_fmt(struct v4l2_subdev *sd,
455
-			  struct v4l2_subdev_pad_config *cfg,
456
-			  struct v4l2_subdev_format *fmt)
488
+static int ov2680_s_power(struct v4l2_subdev *sd, int on)
457
489
 {
458
-	struct ov2680 *ov2680 = to_ov2680(sd);
459
-	const struct ov2680_mode *mode;
460
-	s64 h_blank, vblank_def;
490
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
491
+	int ret = 0;
461
492
 
462
-	mutex_lock(&ov2680->mutex);
493
+	mutex_lock(&sensor->lock);
463
494
 
464
-	mode = ov2680_find_best_fit(ov2680, fmt);
465
-	fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
466
-	fmt->format.width = mode->width;
467
-	fmt->format.height = mode->height;
468
-	fmt->format.field = V4L2_FIELD_NONE;
469
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
470
-#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
471
-		*v4l2_subdev_get_try_format(sd, cfg, fmt->pad) = fmt->format;
472
-#else
473
-		mutex_unlock(&ov2680->mutex);
474
-		return -ENOTTY;
475
-#endif
476
-	} else {
477
-		ov2680->cur_mode = mode;
478
-		h_blank = mode->hts_def - mode->width;
479
-		__v4l2_ctrl_modify_range(ov2680->hblank, h_blank,
480
-					 h_blank, 1, h_blank);
481
-		vblank_def = mode->vts_def - mode->height;
482
-		__v4l2_ctrl_modify_range(ov2680->vblank, vblank_def,
483
-					 OV2680_VTS_MAX - mode->height,
484
-					 1, vblank_def);
485
-	}
495
+	if (on)
496
+		ret = ov2680_power_on(sensor);
497
+	else
498
+		ret = ov2680_power_off(sensor);
486
499
 
487
-	mutex_unlock(&ov2680->mutex);
500
+	if (on && ret == 0)
501
+		ret = ov2680_mode_restore(sensor);
502
+
503
+	mutex_unlock(&sensor->lock);
504
+
505
+	return ret;
506
+}
507
+
508
+static int ov2680_s_g_frame_interval(struct v4l2_subdev *sd,
509
+				     struct v4l2_subdev_frame_interval *fi)
510
+{
511
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
512
+
513
+	mutex_lock(&sensor->lock);
514
+	fi->interval = sensor->frame_interval;
515
+	mutex_unlock(&sensor->lock);
488
516
 
489
517
 	return 0;
490
518
 }
491
519
 
492
-static int ov2680_get_fmt(struct v4l2_subdev *sd,
493
-			  struct v4l2_subdev_pad_config *cfg,
494
-			  struct v4l2_subdev_format *fmt)
520
+static int ov2680_s_stream(struct v4l2_subdev *sd, int enable)
495
521
 {
496
-	struct ov2680 *ov2680 = to_ov2680(sd);
497
-	const struct ov2680_mode *mode = ov2680->cur_mode;
522
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
523
+	int ret = 0;
498
524
 
499
-	mutex_lock(&ov2680->mutex);
500
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
501
-#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
502
-		fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
503
-#else
504
-		mutex_unlock(&ov2680->mutex);
505
-		return -ENOTTY;
506
-#endif
507
-	} else {
508
-		fmt->format.width = mode->width;
509
-		fmt->format.height = mode->height;
510
-		fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
511
-		fmt->format.field = V4L2_FIELD_NONE;
525
+	mutex_lock(&sensor->lock);
526
+
527
+	if (sensor->is_streaming == !!enable)
528
+		goto unlock;
529
+
530
+	if (enable && sensor->mode_pending_changes) {
531
+		ret = ov2680_mode_set(sensor);
532
+		if (ret < 0)
533
+			goto unlock;
512
534
 	}
513
-	mutex_unlock(&ov2680->mutex);
514
535
 
515
-	return 0;
536
+	if (enable)
537
+		ret = ov2680_stream_enable(sensor);
538
+	else
539
+		ret = ov2680_stream_disable(sensor);
540
+
541
+	sensor->is_streaming = !!enable;
542
+
543
+unlock:
544
+	mutex_unlock(&sensor->lock);
545
+
546
+	return ret;
516
547
 }
517
548
 
518
549
 static int ov2680_enum_mbus_code(struct v4l2_subdev *sd,
519
550
 				 struct v4l2_subdev_pad_config *cfg,
520
551
 				 struct v4l2_subdev_mbus_code_enum *code)
521
552
 {
522
-	if (code->index != 0)
553
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
554
+
555
+	if (code->pad != 0 || code->index != 0)
523
556
 		return -EINVAL;
524
-	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
557
+
558
+	code->code = sensor->fmt.code;
525
559
 
526
560
 	return 0;
527
561
 }
528
562
 
529
-static int ov2680_enum_frame_sizes(struct v4l2_subdev *sd,
530
-				   struct v4l2_subdev_pad_config *cfg,
531
-				   struct v4l2_subdev_frame_size_enum *fse)
563
+static int ov2680_get_fmt(struct v4l2_subdev *sd,
564
+			  struct v4l2_subdev_pad_config *cfg,
565
+			  struct v4l2_subdev_format *format)
532
566
 {
533
-	struct ov2680 *ov2680 = to_ov2680(sd);
534
-
535
-	if (fse->index >= ov2680->cfg_num)
536
-		return -EINVAL;
537
-
538
-	if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
539
-		return -EINVAL;
540
-
541
-	fse->min_width  = supported_modes[fse->index].width;
542
-	fse->max_width  = supported_modes[fse->index].width;
543
-	fse->max_height = supported_modes[fse->index].height;
544
-	fse->min_height = supported_modes[fse->index].height;
545
-
546
-	return 0;
547
-}
548
-
549
-static int ov2680_enable_test_pattern(struct ov2680 *ov2680, u32 pattern)
550
-{
551
-	u32 val;
552
-
553
-	if (pattern)
554
-		val = (pattern - 1) | OV2680_TEST_PATTERN_ENABLE;
555
-	else
556
-		val = OV2680_TEST_PATTERN_DISABLE;
557
-
558
-	return ov2680_write_reg(ov2680->client, OV2680_REG_TEST_PATTERN,
559
-				OV2680_REG_VALUE_08BIT, val);
560
-}
561
-
562
-static int ov2680_g_frame_interval(struct v4l2_subdev *sd,
563
-				   struct v4l2_subdev_frame_interval *fi)
564
-{
565
-	struct ov2680 *ov2680 = to_ov2680(sd);
566
-	const struct ov2680_mode *mode = ov2680->cur_mode;
567
-
568
-	mutex_lock(&ov2680->mutex);
569
-	fi->interval = mode->max_fps;
570
-	mutex_unlock(&ov2680->mutex);
571
-
572
-	return 0;
573
-}
574
-
575
-static void ov2680_get_module_inf(struct ov2680 *ov2680,
576
-				  struct rkmodule_inf *inf)
577
-{
578
-	memset(inf, 0, sizeof(*inf));
579
-	strlcpy(inf->base.sensor, OV2680_NAME, sizeof(inf->base.sensor));
580
-	strlcpy(inf->base.module, ov2680->module_name,
581
-		sizeof(inf->base.module));
582
-	strlcpy(inf->base.lens, ov2680->len_name, sizeof(inf->base.lens));
583
-}
584
-
585
-static long ov2680_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
586
-{
587
-	struct ov2680 *ov2680 = to_ov2680(sd);
588
-	long ret = 0;
589
-	u32 stream = 0;
590
-
591
-	switch (cmd) {
592
-	case RKMODULE_GET_MODULE_INFO:
593
-		ov2680_get_module_inf(ov2680, (struct rkmodule_inf *)arg);
594
-		break;
595
-	case RKMODULE_SET_QUICK_STREAM:
596
-
597
-		stream = *((u32 *)arg);
598
-
599
-		if (stream)
600
-			ret = ov2680_write_reg(ov2680->client, OV2680_REG_CTRL_MODE,
601
-				OV2680_REG_VALUE_08BIT, OV2680_MODE_STREAMING);
602
-		else
603
-			ret = ov2680_write_reg(ov2680->client, OV2680_REG_CTRL_MODE,
604
-				OV2680_REG_VALUE_08BIT, OV2680_MODE_SW_STANDBY);
605
-		break;
606
-	default:
607
-		ret = -ENOIOCTLCMD;
608
-		break;
609
-	}
610
-
611
-	return ret;
612
-}
613
-
614
-#ifdef CONFIG_COMPAT
615
-static long ov2680_compat_ioctl32(struct v4l2_subdev *sd,
616
-				  unsigned int cmd, unsigned long arg)
617
-{
618
-	void __user *up = compat_ptr(arg);
619
-	struct rkmodule_inf *inf;
620
-	struct rkmodule_awb_cfg *cfg;
621
-	long ret;
622
-	u32 stream = 0;
623
-
624
-	switch (cmd) {
625
-	case RKMODULE_GET_MODULE_INFO:
626
-		inf = kzalloc(sizeof(*inf), GFP_KERNEL);
627
-		if (!inf) {
628
-			ret = -ENOMEM;
629
-			return ret;
630
-		}
631
-
632
-		ret = ov2680_ioctl(sd, cmd, inf);
633
-		if (!ret)
634
-			ret = copy_to_user(up, inf, sizeof(*inf));
635
-		kfree(inf);
636
-		break;
637
-	case RKMODULE_AWB_CFG:
638
-		cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
639
-		if (!cfg) {
640
-			ret = -ENOMEM;
641
-			return ret;
642
-		}
643
-
644
-		ret = copy_from_user(cfg, up, sizeof(*cfg));
645
-		if (!ret)
646
-			ret = ov2680_ioctl(sd, cmd, cfg);
647
-		kfree(cfg);
648
-		break;
649
-	case RKMODULE_SET_QUICK_STREAM:
650
-		ret = copy_from_user(&stream, up, sizeof(u32));
651
-		if (!ret)
652
-			ret = ov2680_ioctl(sd, cmd, &stream);
653
-		break;
654
-	default:
655
-		ret = -ENOIOCTLCMD;
656
-		break;
657
-	}
658
-
659
-	return ret;
660
-}
661
-#endif
662
-
663
-static int __ov2680_start_stream(struct ov2680 *ov2680)
664
-{
665
-	int ret;
666
-
667
-	ret = ov2680_write_array(ov2680->client, ov2680->cur_mode->reg_list);
668
-	if (ret)
669
-		return ret;
670
-
671
-#ifdef CHECK_REG_VALUE
672
-	usleep_range(10000, 20000);
673
-	/*  verify default values to make sure everything has */
674
-	/*  been written correctly as expected */
675
-	dev_info(&ov2680->client->dev, "%s:Check register value!\n",
676
-				__func__);
677
-	ret = ov2680_reg_verify(ov2680->client, ov2680_global_regs);
678
-	if (ret)
679
-		return ret;
680
-
681
-	ret = ov2680_reg_verify(ov2680->client, ov2680->cur_mode->reg_list);
682
-	if (ret)
683
-		return ret;
684
-#endif
685
-
686
-	/* In case these controls are set before streaming */
687
-	mutex_unlock(&ov2680->mutex);
688
-	ret = v4l2_ctrl_handler_setup(&ov2680->ctrl_handler);
689
-	mutex_lock(&ov2680->mutex);
690
-	if (ret)
691
-		return ret;
692
-	ret = ov2680_write_reg(ov2680->client, OV2680_REG_CTRL_MODE,
693
-				OV2680_REG_VALUE_08BIT, OV2680_MODE_STREAMING);
694
-	return ret;
695
-}
696
-
697
-static int __ov2680_stop_stream(struct ov2680 *ov2680)
698
-{
699
-	return ov2680_write_reg(ov2680->client, OV2680_REG_CTRL_MODE,
700
-				OV2680_REG_VALUE_08BIT, OV2680_MODE_SW_STANDBY);
701
-}
702
-
703
-static int ov2680_s_stream(struct v4l2_subdev *sd, int on)
704
-{
705
-	struct ov2680 *ov2680 = to_ov2680(sd);
706
-	struct i2c_client *client = ov2680->client;
567
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
568
+	struct v4l2_mbus_framefmt *fmt = NULL;
707
569
 	int ret = 0;
708
570
 
709
-	dev_info(&client->dev, "%s: on: %d, %dx%d@%d\n", __func__, on,
710
-				ov2680->cur_mode->width,
711
-				ov2680->cur_mode->height,
712
-		DIV_ROUND_CLOSEST(ov2680->cur_mode->max_fps.denominator,
713
-		ov2680->cur_mode->max_fps.numerator));
571
+	if (format->pad != 0)
572
+		return -EINVAL;
714
573
 
715
-	mutex_lock(&ov2680->mutex);
716
-	on = !!on;
717
-	if (on == ov2680->streaming)
718
-		goto unlock_and_return;
574
+	mutex_lock(&sensor->lock);
719
575
 
720
-	if (on) {
721
-		dev_info(&client->dev, "stream on!!!\n");
722
-		ret = __ov2680_start_stream(ov2680);
723
-		if (ret) {
724
-			v4l2_err(sd, "start stream failed while write regs\n");
725
-			pm_runtime_put(&client->dev);
726
-			goto unlock_and_return;
727
-		}
728
-	} else {
729
-		dev_info(&client->dev, "stream off!!!\n");
730
-		__ov2680_stop_stream(ov2680);
731
-	}
732
-
733
-	ov2680->streaming = on;
734
-
735
-unlock_and_return:
736
-	mutex_unlock(&ov2680->mutex);
737
-
738
-	return ret;
739
-}
740
-
741
-/* Calculate the delay in us by clock rate and clock cycles */
742
-static inline u32 ov2680_cal_delay(u32 cycles)
743
-{
744
-	return DIV_ROUND_UP(cycles, OV2680_XVCLK_FREQ / 1000 / 1000);
745
-}
746
-
747
-static int __ov2680_power_on(struct ov2680 *ov2680)
748
-{
749
-	int ret;
750
-	u32 delay_us;
751
-	struct device *dev = &ov2680->client->dev;
752
-
753
-	if (!IS_ERR(ov2680->power_gpio))
754
-		gpiod_set_value_cansleep(ov2680->power_gpio, 1);
755
-
756
-	usleep_range(1000, 2000);
757
-
758
-	if (!IS_ERR_OR_NULL(ov2680->pins_default)) {
759
-		ret = pinctrl_select_state(ov2680->pinctrl,
760
-					   ov2680->pins_default);
761
-		if (ret < 0)
762
-			dev_err(dev, "could not set pins\n");
763
-	}
764
-	ret = clk_set_rate(ov2680->xvclk, OV2680_XVCLK_FREQ);
765
-	if (ret < 0)
766
-		dev_warn(dev, "Failed to set xvclk rate (24MHz)\n");
767
-	if (clk_get_rate(ov2680->xvclk) != OV2680_XVCLK_FREQ)
768
-		dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
769
-	ret = clk_prepare_enable(ov2680->xvclk);
770
-	if (ret < 0) {
771
-		dev_err(dev, "Failed to enable xvclk\n");
772
-		return ret;
773
-	}
774
-	if (!IS_ERR(ov2680->reset_gpio))
775
-		gpiod_set_value_cansleep(ov2680->reset_gpio, 1);
776
-
777
-	ret = regulator_bulk_enable(OV2680_NUM_SUPPLIES, ov2680->supplies);
778
-	if (ret < 0) {
779
-		dev_err(dev, "Failed to enable regulators\n");
780
-		goto disable_clk;
781
-	}
782
-
783
-	if (!IS_ERR(ov2680->reset_gpio))
784
-		gpiod_set_value_cansleep(ov2680->reset_gpio, 0);
785
-
786
-	if (!IS_ERR(ov2680->pwdn_gpio))
787
-		gpiod_set_value_cansleep(ov2680->pwdn_gpio, 1);
788
-
789
-	/* 8192 cycles prior to first SCCB transaction */
790
-	delay_us = ov2680_cal_delay(8192);
791
-	usleep_range(delay_us, delay_us * 2);
792
-
793
-	return 0;
794
-
795
-disable_clk:
796
-	clk_disable_unprepare(ov2680->xvclk);
797
-
798
-	return ret;
799
-}
800
-
801
-static void __ov2680_power_off(struct ov2680 *ov2680)
802
-{
803
-	int ret;
804
-	struct device *dev = &ov2680->client->dev;
805
-
806
-	if (!IS_ERR(ov2680->pwdn_gpio))
807
-		gpiod_set_value_cansleep(ov2680->pwdn_gpio, 0);
808
-	clk_disable_unprepare(ov2680->xvclk);
809
-	if (!IS_ERR(ov2680->reset_gpio))
810
-		gpiod_set_value_cansleep(ov2680->reset_gpio, 1);
811
-	if (!IS_ERR_OR_NULL(ov2680->pins_sleep)) {
812
-		ret = pinctrl_select_state(ov2680->pinctrl,
813
-					   ov2680->pins_sleep);
814
-		if (ret < 0)
815
-			dev_dbg(dev, "could not set pins\n");
816
-	}
817
-	if (!IS_ERR(ov2680->power_gpio))
818
-		gpiod_set_value_cansleep(ov2680->power_gpio, 0);
819
-
820
-	regulator_bulk_disable(OV2680_NUM_SUPPLIES, ov2680->supplies);
821
-}
822
-
823
-static int ov2680_runtime_resume(struct device *dev)
824
-{
825
-	struct i2c_client *client = to_i2c_client(dev);
826
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
827
-	struct ov2680 *ov2680 = to_ov2680(sd);
828
-
829
-	return __ov2680_power_on(ov2680);
830
-}
831
-
832
-static int ov2680_runtime_suspend(struct device *dev)
833
-{
834
-	struct i2c_client *client = to_i2c_client(dev);
835
-	struct v4l2_subdev *sd = i2c_get_clientdata(client);
836
-	struct ov2680 *ov2680 = to_ov2680(sd);
837
-
838
-	__ov2680_power_off(ov2680);
839
-
840
-	return 0;
841
-}
842
-
576
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
843
577
 #ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
844
-static int ov2680_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
845
-{
846
-	struct ov2680 *ov2680 = to_ov2680(sd);
847
-	struct v4l2_mbus_framefmt *try_fmt =
848
-				v4l2_subdev_get_try_format(sd, fh->pad, 0);
849
-	const struct ov2680_mode *def_mode = &supported_modes[0];
850
-
851
-	mutex_lock(&ov2680->mutex);
852
-	/* Initialize try_fmt */
853
-	try_fmt->width = def_mode->width;
854
-	try_fmt->height = def_mode->height;
855
-	try_fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
856
-	try_fmt->field = V4L2_FIELD_NONE;
857
-
858
-	mutex_unlock(&ov2680->mutex);
859
-	/* No crop or compose */
860
-
861
-	return 0;
862
-}
578
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg, format->pad);
579
+#else
580
+		ret = -EINVAL;
863
581
 #endif
582
+	} else {
583
+		fmt = &sensor->fmt;
584
+	}
864
585
 
865
-static int ov2680_init(struct v4l2_subdev *sd)
866
-{
867
-	int ret;
868
-	struct ov2680 *ov2680 = to_ov2680(sd);
869
-	struct i2c_client *client = ov2680->client;
586
+	if (fmt)
587
+		format->format = *fmt;
870
588
 
871
-	dev_info(&client->dev, "%s(%d)\n", __func__, __LINE__);
872
-	ret = ov2680_write_array(ov2680->client, ov2680_global_regs);
589
+	mutex_unlock(&sensor->lock);
590
+
873
591
 	return ret;
874
592
 }
875
593
 
876
-static int ov2680_power(struct v4l2_subdev *sd, int on)
594
+static int ov2680_set_fmt(struct v4l2_subdev *sd,
595
+			  struct v4l2_subdev_pad_config *cfg,
596
+			  struct v4l2_subdev_format *format)
877
597
 {
878
-	int ret;
879
-	struct ov2680 *ov2680 = to_ov2680(sd);
880
-	struct i2c_client *client = ov2680->client;
881
-	struct device *dev = &ov2680->client->dev;
598
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
599
+	struct v4l2_mbus_framefmt *fmt = &format->format;
600
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
601
+	struct v4l2_mbus_framefmt *try_fmt;
602
+#endif
603
+	const struct ov2680_mode_info *mode;
604
+	int ret = 0;
882
605
 
883
-	dev_info(&client->dev, "%s(%d) on(%d)\n", __func__, __LINE__, on);
884
-	if (on) {
885
-		ret = pm_runtime_get_sync(&client->dev);
886
-		if (ret < 0) {
887
-			pm_runtime_put_noidle(&client->dev);
888
-			return ret;
889
-		}
890
-		ret = ov2680_init(sd);
891
-		usleep_range(5000, 10000);
892
-		if (ret)
893
-			dev_err(dev, "init error\n");
894
-	} else {
895
-		pm_runtime_put(&client->dev);
606
+	if (format->pad != 0)
607
+		return -EINVAL;
608
+
609
+	mutex_lock(&sensor->lock);
610
+
611
+	if (sensor->is_streaming) {
612
+		ret = -EBUSY;
613
+		goto unlock;
896
614
 	}
615
+
616
+	mode = v4l2_find_nearest_size(ov2680_mode_data,
617
+				      ARRAY_SIZE(ov2680_mode_data), width,
618
+				      height, fmt->width, fmt->height);
619
+	if (!mode) {
620
+		ret = -EINVAL;
621
+		goto unlock;
622
+	}
623
+
624
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
625
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
626
+		try_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
627
+		format->format = *try_fmt;
628
+#endif
629
+		goto unlock;
630
+	}
631
+
632
+	fmt->width = mode->width;
633
+	fmt->height = mode->height;
634
+	fmt->code = sensor->fmt.code;
635
+	fmt->colorspace = sensor->fmt.colorspace;
636
+
637
+	sensor->current_mode = mode;
638
+	sensor->fmt = format->format;
639
+	sensor->mode_pending_changes = true;
640
+
641
+unlock:
642
+	mutex_unlock(&sensor->lock);
643
+
644
+	return ret;
645
+}
646
+
647
+static int ov2680_init_cfg(struct v4l2_subdev *sd,
648
+			   struct v4l2_subdev_pad_config *cfg)
649
+{
650
+	struct v4l2_subdev_format fmt = {
651
+		.which = cfg ? V4L2_SUBDEV_FORMAT_TRY
652
+				: V4L2_SUBDEV_FORMAT_ACTIVE,
653
+		.format = {
654
+			.width = 800,
655
+			.height = 600,
656
+		}
657
+	};
658
+
659
+	return ov2680_set_fmt(sd, cfg, &fmt);
660
+}
661
+
662
+static int ov2680_enum_frame_size(struct v4l2_subdev *sd,
663
+				  struct v4l2_subdev_pad_config *cfg,
664
+				  struct v4l2_subdev_frame_size_enum *fse)
665
+{
666
+	int index = fse->index;
667
+
668
+	if (index >= OV2680_MODE_MAX || index < 0)
669
+		return -EINVAL;
670
+
671
+	fse->min_width = ov2680_mode_data[index].width;
672
+	fse->min_height = ov2680_mode_data[index].height;
673
+	fse->max_width = ov2680_mode_data[index].width;
674
+	fse->max_height = ov2680_mode_data[index].height;
675
+
897
676
 	return 0;
898
677
 }
899
678
 
900
679
 static int ov2680_enum_frame_interval(struct v4l2_subdev *sd,
901
-				      struct v4l2_subdev_pad_config *cfg,
902
-				      struct v4l2_subdev_frame_interval_enum *fie)
680
+			      struct v4l2_subdev_pad_config *cfg,
681
+			      struct v4l2_subdev_frame_interval_enum *fie)
903
682
 {
904
-	struct ov2680 *ov2680 = to_ov2680(sd);
683
+	struct v4l2_fract tpf;
905
684
 
906
-	if (fie->index >= ov2680->cfg_num)
685
+	if (fie->index >= OV2680_MODE_MAX || fie->width > OV2680_WIDTH_MAX ||
686
+	    fie->height > OV2680_HEIGHT_MAX ||
687
+	    fie->which > V4L2_SUBDEV_FORMAT_ACTIVE)
907
688
 		return -EINVAL;
908
689
 
909
-	if (fie->code != MEDIA_BUS_FMT_SBGGR10_1X10)
910
-		return -EINVAL;
690
+	tpf.denominator = OV2680_FRAME_RATE;
691
+	tpf.numerator = 1;
911
692
 
912
-	fie->width = supported_modes[fie->index].width;
913
-	fie->height = supported_modes[fie->index].height;
914
-	fie->interval = supported_modes[fie->index].max_fps;
915
-	return 0;
916
-}
917
-
918
-static int ov2680_g_mbus_config(struct v4l2_subdev *sd,
919
-				struct v4l2_mbus_config *config)
920
-{
921
-	u32 val = 0;
922
-
923
-	val = 1 << (OV2680_LANES - 1) |
924
-	      V4L2_MBUS_CSI2_CHANNEL_0 |
925
-	      V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
926
-	config->type = V4L2_MBUS_CSI2;
927
-	config->flags = val;
693
+	fie->interval = tpf;
928
694
 
929
695
 	return 0;
930
696
 }
931
697
 
932
-static const struct dev_pm_ops ov2680_pm_ops = {
933
-	SET_RUNTIME_PM_OPS(ov2680_runtime_suspend,
934
-			   ov2680_runtime_resume, NULL)
935
-};
698
+static int ov2680_s_ctrl(struct v4l2_ctrl *ctrl)
699
+{
700
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
701
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
936
702
 
937
-#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
938
-static const struct v4l2_subdev_internal_ops ov2680_internal_ops = {
939
-	.open = ov2680_open,
703
+	if (!sensor->is_enabled)
704
+		return 0;
705
+
706
+	switch (ctrl->id) {
707
+	case V4L2_CID_GAIN:
708
+		return ov2680_gain_set(sensor, ctrl->val);
709
+	case V4L2_CID_EXPOSURE:
710
+		return ov2680_exposure_set(sensor, ctrl->val);
711
+	case V4L2_CID_VFLIP:
712
+		return ov2680_set_vflip(sensor, ctrl->val);
713
+	case V4L2_CID_HFLIP:
714
+		return ov2680_set_hflip(sensor, ctrl->val);
715
+	case V4L2_CID_TEST_PATTERN:
716
+		return ov2680_test_pattern_set(sensor, ctrl->val);
717
+	default:
718
+		break;
719
+	}
720
+
721
+	return -EINVAL;
722
+}
723
+
724
+static const struct v4l2_ctrl_ops ov2680_ctrl_ops = {
725
+	.s_ctrl = ov2680_s_ctrl,
940
726
 };
941
-#endif
942
727
 
943
728
 static const struct v4l2_subdev_core_ops ov2680_core_ops = {
944
-	.ioctl = ov2680_ioctl,
945
-#ifdef CONFIG_COMPAT
946
-	.compat_ioctl32 = ov2680_compat_ioctl32,
947
-#endif
948
-	.s_power = ov2680_power,
729
+	.s_power = ov2680_s_power,
949
730
 };
950
731
 
951
732
 static const struct v4l2_subdev_video_ops ov2680_video_ops = {
952
-	.s_stream = ov2680_s_stream,
953
-	.g_frame_interval = ov2680_g_frame_interval,
954
-	.g_mbus_config = ov2680_g_mbus_config,
733
+	.g_frame_interval	= ov2680_s_g_frame_interval,
734
+	.s_frame_interval	= ov2680_s_g_frame_interval,
735
+	.s_stream		= ov2680_s_stream,
955
736
 };
956
737
 
957
738
 static const struct v4l2_subdev_pad_ops ov2680_pad_ops = {
958
-	.enum_mbus_code = ov2680_enum_mbus_code,
959
-	.enum_frame_size = ov2680_enum_frame_sizes,
960
-	.enum_frame_interval = ov2680_enum_frame_interval,
961
-	.get_fmt = ov2680_get_fmt,
962
-	.set_fmt = ov2680_set_fmt,
739
+	.init_cfg		= ov2680_init_cfg,
740
+	.enum_mbus_code		= ov2680_enum_mbus_code,
741
+	.get_fmt		= ov2680_get_fmt,
742
+	.set_fmt		= ov2680_set_fmt,
743
+	.enum_frame_size	= ov2680_enum_frame_size,
744
+	.enum_frame_interval	= ov2680_enum_frame_interval,
963
745
 };
964
746
 
965
747
 static const struct v4l2_subdev_ops ov2680_subdev_ops = {
..
..
@@ -968,359 +750,196 @@
968
750
 	.pad	= &ov2680_pad_ops,
969
751
 };
970
752
 
971
-static int ov2680_set_ctrl(struct v4l2_ctrl *ctrl)
753
+static int ov2680_mode_init(struct ov2680_dev *sensor)
972
754
 {
973
-	struct ov2680 *ov2680 = container_of(ctrl->handler,
974
-					     struct ov2680, ctrl_handler);
975
-	struct i2c_client *client = ov2680->client;
976
-	s64 max;
977
-	int ret = 0;
755
+	const struct ov2680_mode_info *init_mode;
978
756
 
979
-	/* Propagate change of current control to all related controls */
980
-	switch (ctrl->id) {
981
-	case V4L2_CID_VBLANK:
982
-		/* Update max exposure while meeting expected vblanking */
983
-		max = ov2680->cur_mode->height + ctrl->val - 4;
984
-		__v4l2_ctrl_modify_range(ov2680->exposure,
985
-					 ov2680->exposure->minimum, max,
986
-					 ov2680->exposure->step,
987
-					 ov2680->exposure->default_value);
988
-		break;
989
-	}
757
+	/* set initial mode */
758
+	sensor->fmt.code = MEDIA_BUS_FMT_SBGGR10_1X10;
759
+	sensor->fmt.width = 800;
760
+	sensor->fmt.height = 600;
761
+	sensor->fmt.field = V4L2_FIELD_NONE;
762
+	sensor->fmt.colorspace = V4L2_COLORSPACE_SRGB;
990
763
 
991
-	if (!pm_runtime_get_if_in_use(&client->dev))
992
-		return 0;
764
+	sensor->frame_interval.denominator = OV2680_FRAME_RATE;
765
+	sensor->frame_interval.numerator = 1;
993
766
 
994
-	switch (ctrl->id) {
995
-	case V4L2_CID_EXPOSURE:
996
-		/* 4 least significant bits of expsoure are fractional part */
767
+	init_mode = &ov2680_mode_init_data;
997
768
 
998
-		ret = ov2680_write_reg(ov2680->client, OV2680_REG_EXPOSURE,
999
-				       OV2680_REG_VALUE_24BIT, ctrl->val << 4);
769
+	sensor->current_mode = init_mode;
1000
770
 
1001
-		break;
1002
-	case V4L2_CID_ANALOGUE_GAIN:
1003
-		ret = ov2680_write_reg(ov2680->client, OV2680_REG_GAIN_L,
1004
-				       OV2680_REG_VALUE_08BIT,
1005
-				       ctrl->val & OV2680_GAIN_L_MASK);
1006
-		ret |= ov2680_write_reg(ov2680->client, OV2680_REG_GAIN_H,
1007
-				       OV2680_REG_VALUE_08BIT,
1008
-				       (ctrl->val >> OV2680_DIGI_GAIN_H_SHIFT) &
1009
-				       OV2680_GAIN_H_MASK);
1010
-		break;
1011
-	case V4L2_CID_VBLANK:
771
+	sensor->mode_pending_changes = true;
1012
772
 
1013
-		ret = ov2680_write_reg(ov2680->client, OV2680_REG_VTS,
1014
-				       OV2680_REG_VALUE_16BIT,
1015
-				       ctrl->val + ov2680->cur_mode->height);
1016
-		break;
1017
-	case V4L2_CID_TEST_PATTERN:
1018
-		ret = ov2680_enable_test_pattern(ov2680, ctrl->val);
1019
-		break;
1020
-	default:
1021
-		dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
1022
-			 __func__, ctrl->id, ctrl->val);
1023
-		break;
1024
-	}
1025
-
1026
-	pm_runtime_put(&client->dev);
1027
-
1028
-	return ret;
773
+	return 0;
1029
774
 }
1030
775
 
1031
-static const struct v4l2_ctrl_ops ov2680_ctrl_ops = {
1032
-	.s_ctrl = ov2680_set_ctrl,
1033
-};
1034
-
1035
-static int ov2680_initialize_controls(struct ov2680 *ov2680)
776
+static int ov2680_v4l2_register(struct ov2680_dev *sensor)
1036
777
 {
1037
-	const struct ov2680_mode *mode;
1038
-	struct v4l2_ctrl_handler *handler;
1039
-	struct v4l2_ctrl *ctrl;
1040
-	s64 exposure_max, vblank_def;
1041
-	u32 h_blank;
1042
-	int ret;
778
+	const struct v4l2_ctrl_ops *ops = &ov2680_ctrl_ops;
779
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
780
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
781
+	int ret = 0;
1043
782
 
1044
-	handler = &ov2680->ctrl_handler;
1045
-	mode = ov2680->cur_mode;
1046
-	ret = v4l2_ctrl_handler_init(handler, 8);
1047
-	if (ret)
783
+	v4l2_i2c_subdev_init(&sensor->sd, sensor->i2c_client,
784
+			     &ov2680_subdev_ops);
785
+
786
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
787
+	sensor->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
788
+#endif
789
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
790
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
791
+
792
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
793
+	if (ret < 0)
1048
794
 		return ret;
1049
-	handler->lock = &ov2680->mutex;
1050
795
 
1051
-	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
1052
-				      0, 0, link_freq_menu_items);
1053
-	if (ctrl)
1054
-		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
796
+	v4l2_ctrl_handler_init(hdl, 5);
1055
797
 
1056
-	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
1057
-			  0, ov2680->pixel_rate, 1, ov2680->pixel_rate);
798
+	hdl->lock = &sensor->lock;
1058
799
 
1059
-	h_blank = mode->hts_def - mode->width;
1060
-	ov2680->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
1061
-				h_blank, h_blank, 1, h_blank);
1062
-	if (ov2680->hblank)
1063
-		ov2680->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
800
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
801
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1064
802
 
1065
-	vblank_def = mode->vts_def - mode->height;
1066
-	ov2680->vblank = v4l2_ctrl_new_std(handler, &ov2680_ctrl_ops,
1067
-				V4L2_CID_VBLANK, vblank_def,
1068
-				OV2680_VTS_MAX - mode->height,
1069
-				1, vblank_def);
803
+	ctrls->test_pattern = v4l2_ctrl_new_std_menu_items(hdl,
804
+					&ov2680_ctrl_ops, V4L2_CID_TEST_PATTERN,
805
+					ARRAY_SIZE(test_pattern_menu) - 1,
806
+					0, 0, test_pattern_menu);
1070
807
 
1071
-	exposure_max = mode->vts_def - 4;
1072
-	ov2680->exposure = v4l2_ctrl_new_std(handler, &ov2680_ctrl_ops,
1073
-				V4L2_CID_EXPOSURE, OV2680_EXPOSURE_MIN,
1074
-				exposure_max, OV2680_EXPOSURE_STEP,
1075
-				mode->exp_def);
808
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
809
+					    0, 32767, 1, 0);
1076
810
 
1077
-	ov2680->anal_gain = v4l2_ctrl_new_std(handler, &ov2680_ctrl_ops,
1078
-				V4L2_CID_ANALOGUE_GAIN, ANALOG_GAIN_MIN,
1079
-				ANALOG_GAIN_MAX, ANALOG_GAIN_STEP,
1080
-				ANALOG_GAIN_DEFAULT);
811
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN, 0, 2047, 1, 0);
1081
812
 
1082
-	/* Digital gain */
1083
-	ov2680->digi_gain = v4l2_ctrl_new_std(handler, &ov2680_ctrl_ops,
1084
-				V4L2_CID_DIGITAL_GAIN, OV2680_DIGI_GAIN_MIN,
1085
-				OV2680_DIGI_GAIN_MAX, OV2680_DIGI_GAIN_STEP,
1086
-				OV2680_DIGI_GAIN_DEFAULT);
1087
-
1088
-	ov2680->test_pattern = v4l2_ctrl_new_std_menu_items(handler,
1089
-				&ov2680_ctrl_ops, V4L2_CID_TEST_PATTERN,
1090
-				ARRAY_SIZE(ov2680_test_pattern_menu) - 1,
1091
-				0, 0, ov2680_test_pattern_menu);
1092
-
1093
-	if (handler->error) {
1094
-		ret = handler->error;
1095
-		dev_err(&ov2680->client->dev,
1096
-			"Failed to init controls(%d)\n", ret);
1097
-		goto err_free_handler;
813
+	if (hdl->error) {
814
+		ret = hdl->error;
815
+		goto cleanup_entity;
1098
816
 	}
1099
817
 
1100
-	ov2680->subdev.ctrl_handler = handler;
818
+	ctrls->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
819
+	ctrls->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
820
+
821
+	sensor->sd.ctrl_handler = hdl;
822
+
823
+	ret = v4l2_async_register_subdev(&sensor->sd);
824
+	if (ret < 0)
825
+		goto cleanup_entity;
1101
826
 
1102
827
 	return 0;
1103
828
 
1104
-err_free_handler:
1105
-	v4l2_ctrl_handler_free(handler);
829
+cleanup_entity:
830
+	media_entity_cleanup(&sensor->sd.entity);
831
+	v4l2_ctrl_handler_free(hdl);
1106
832
 
1107
833
 	return ret;
1108
834
 }
1109
835
 
1110
-static int ov2680_check_sensor_id(struct ov2680 *ov2680,
1111
-				  struct i2c_client *client)
836
+static int ov2680_get_regulators(struct ov2680_dev *sensor)
1112
837
 {
1113
-	struct device *dev = &ov2680->client->dev;
1114
-	u32 id = 0;
838
+	int i;
839
+
840
+	for (i = 0; i < OV2680_NUM_SUPPLIES; i++)
841
+		sensor->supplies[i].supply = ov2680_supply_name[i];
842
+
843
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
844
+				       OV2680_NUM_SUPPLIES,
845
+				       sensor->supplies);
846
+}
847
+
848
+static int ov2680_check_id(struct ov2680_dev *sensor)
849
+{
850
+	struct device *dev = ov2680_to_dev(sensor);
851
+	u32 chip_id;
1115
852
 	int ret;
1116
853
 
1117
-	ret = ov2680_read_reg(client, OV2680_REG_CHIP_ID,
1118
-			      OV2680_REG_VALUE_16BIT, &id);
1119
-	if (id != CHIP_ID) {
1120
-		dev_err(dev, "Unexpected sensor id(%06x), ret(%d)\n", id, ret);
854
+	ov2680_power_on(sensor);
855
+
856
+	ret = ov2680_read_reg16(sensor, OV2680_REG_CHIP_ID_HIGH, &chip_id);
857
+	if (ret < 0) {
858
+		dev_err(dev, "failed to read chip id high\n");
1121
859
 		return -ENODEV;
1122
860
 	}
1123
861
 
1124
-	dev_info(dev, "Detected OV%06x sensor\n", CHIP_ID);
862
+	if (chip_id != OV2680_CHIP_ID) {
863
+		dev_err(dev, "chip id: 0x%04x does not match expected 0x%04x\n",
864
+			chip_id, OV2680_CHIP_ID);
865
+		return -ENODEV;
866
+	}
1125
867
 
1126
868
 	return 0;
1127
869
 }
1128
870
 
1129
-static int ov2680_configure_regulators(struct ov2680 *ov2680)
871
+static int ov2680_parse_dt(struct ov2680_dev *sensor)
1130
872
 {
1131
-	unsigned int i;
1132
-
1133
-	for (i = 0; i < OV2680_NUM_SUPPLIES; i++)
1134
-		ov2680->supplies[i].supply = ov2680_supply_names[i];
1135
-
1136
-	return devm_regulator_bulk_get(&ov2680->client->dev,
1137
-				       OV2680_NUM_SUPPLIES,
1138
-				       ov2680->supplies);
1139
-}
1140
-
1141
-static int ov2680_parse_of(struct ov2680 *ov2680)
1142
-{
1143
-	struct device *dev = &ov2680->client->dev;
1144
-	struct device_node *endpoint;
1145
-	struct fwnode_handle *fwnode;
1146
-	int rval;
1147
-
1148
-	endpoint = of_graph_get_next_endpoint(dev->of_node, NULL);
1149
-	if (!endpoint) {
1150
-		dev_err(dev, "Failed to get endpoint\n");
1151
-		return -EINVAL;
1152
-	}
1153
-	fwnode = of_fwnode_handle(endpoint);
1154
-	rval = fwnode_property_read_u32_array(fwnode, "data-lanes", NULL, 0);
1155
-	if (rval <= 0) {
1156
-		dev_warn(dev, " Get mipi lane num failed!\n");
1157
-		return -1;
1158
-	}
1159
-
1160
-	ov2680->lane_num = rval;
1161
-	if (1 == ov2680->lane_num) {
1162
-		ov2680->cur_mode = &supported_modes_1lane[0];
1163
-		supported_modes = supported_modes_1lane;
1164
-		ov2680->cfg_num = ARRAY_SIZE(supported_modes_1lane);
1165
-
1166
-		/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
1167
-		ov2680->pixel_rate = MIPI_FREQ * 2U * ov2680->lane_num / 10U;
1168
-		dev_info(dev, "lane_num(%d)  pixel_rate(%u)\n",
1169
-				 ov2680->lane_num, ov2680->pixel_rate);
1170
-	} else {
1171
-		dev_err(dev, "unsupported lane_num(%d)\n", ov2680->lane_num);
1172
-		return -1;
1173
-	}
1174
-	return 0;
1175
-}
1176
-
1177
-static int ov2680_probe(struct i2c_client *client,
1178
-			const struct i2c_device_id *id)
1179
-{
1180
-	struct device *dev = &client->dev;
1181
-	struct device_node *node = dev->of_node;
1182
-	struct ov2680 *ov2680;
1183
-	struct v4l2_subdev *sd;
1184
-	char facing[2] = "b";
873
+	struct device *dev = ov2680_to_dev(sensor);
1185
874
 	int ret;
1186
875
 
1187
-	dev_info(dev, "driver version: %02x.%02x.%02x",
1188
-		DRIVER_VERSION >> 16,
1189
-		(DRIVER_VERSION & 0xff00) >> 8,
1190
-		DRIVER_VERSION & 0x00ff);
1191
-
1192
-	ov2680 = devm_kzalloc(dev, sizeof(*ov2680), GFP_KERNEL);
1193
-	if (!ov2680)
1194
-		return -ENOMEM;
1195
-
1196
-	ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
1197
-				   &ov2680->module_index);
1198
-	if (ret) {
1199
-		dev_warn(dev, "could not get module index!\n");
1200
-		ov2680->module_index = 0;
1201
-	}
1202
-
1203
-	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
1204
-				       &ov2680->module_facing);
1205
-	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME,
1206
-				       &ov2680->module_name);
1207
-	ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME,
1208
-				       &ov2680->len_name);
1209
-	if (ret) {
1210
-		dev_err(dev, "could not get module information!\n");
1211
-		return -EINVAL;
1212
-	}
1213
-
1214
-	ov2680->client = client;
1215
-
1216
-	ov2680->xvclk = devm_clk_get(dev, "xvclk");
1217
-	if (IS_ERR(ov2680->xvclk)) {
1218
-		dev_err(dev, "Failed to get xvclk\n");
1219
-		return -EINVAL;
1220
-	}
1221
-
1222
-	ov2680->power_gpio = devm_gpiod_get(dev, "power", GPIOD_OUT_LOW);
1223
-	if (IS_ERR(ov2680->power_gpio))
1224
-		dev_warn(dev, "Failed to get power-gpios, maybe no use\n");
1225
-
1226
-	ov2680->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
1227
-	if (IS_ERR(ov2680->reset_gpio))
1228
-		dev_warn(dev, "Failed to get reset-gpios, maybe no use\n");
1229
-
1230
-	ov2680->pwdn_gpio = devm_gpiod_get(dev, "pwdn", GPIOD_OUT_LOW);
1231
-	if (IS_ERR(ov2680->pwdn_gpio))
1232
-		dev_warn(dev, "Failed to get pwdn-gpios\n");
1233
-
1234
-	ret = ov2680_configure_regulators(ov2680);
1235
-	if (ret) {
1236
-		dev_err(dev, "Failed to get power regulators\n");
876
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
877
+						     GPIOD_OUT_HIGH);
878
+	ret = PTR_ERR_OR_ZERO(sensor->reset_gpio);
879
+	if (ret < 0) {
880
+		dev_dbg(dev, "error while getting reset gpio: %d\n", ret);
1237
881
 		return ret;
1238
882
 	}
1239
-	ret = ov2680_parse_of(ov2680);
1240
-	if (ret != 0)
883
+
884
+	sensor->xvclk = devm_clk_get(dev, "xvclk");
885
+	if (IS_ERR(sensor->xvclk)) {
886
+		dev_err(dev, "xvclk clock missing or invalid\n");
887
+		return PTR_ERR(sensor->xvclk);
888
+	}
889
+
890
+	sensor->xvclk_freq = clk_get_rate(sensor->xvclk);
891
+	if (sensor->xvclk_freq != OV2680_XVCLK_VALUE) {
892
+		dev_err(dev, "wrong xvclk frequency %d HZ, expected: %d Hz\n",
893
+			sensor->xvclk_freq, OV2680_XVCLK_VALUE);
894
+		return -EINVAL;
895
+	}
896
+
897
+	return 0;
898
+}
899
+
900
+static int ov2680_probe(struct i2c_client *client)
901
+{
902
+	struct device *dev = &client->dev;
903
+	struct ov2680_dev *sensor;
904
+	int ret;
905
+
906
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
907
+	if (!sensor)
908
+		return -ENOMEM;
909
+
910
+	sensor->i2c_client = client;
911
+
912
+	ret = ov2680_parse_dt(sensor);
913
+	if (ret < 0)
1241
914
 		return -EINVAL;
1242
915
 
1243
-	ov2680->pinctrl = devm_pinctrl_get(dev);
1244
-	if (!IS_ERR(ov2680->pinctrl)) {
1245
-		ov2680->pins_default =
1246
-			pinctrl_lookup_state(ov2680->pinctrl,
1247
-					     OF_CAMERA_PINCTRL_STATE_DEFAULT);
1248
-		if (IS_ERR(ov2680->pins_default))
1249
-			dev_err(dev, "could not get default pinstate\n");
1250
-
1251
-		ov2680->pins_sleep =
1252
-			pinctrl_lookup_state(ov2680->pinctrl,
1253
-					     OF_CAMERA_PINCTRL_STATE_SLEEP);
1254
-		if (IS_ERR(ov2680->pins_sleep))
1255
-			dev_err(dev, "could not get sleep pinstate\n");
1256
-	}
1257
-
1258
-	mutex_init(&ov2680->mutex);
1259
-
1260
-	sd = &ov2680->subdev;
1261
-	v4l2_i2c_subdev_init(sd, client, &ov2680_subdev_ops);
1262
-	ret = ov2680_initialize_controls(ov2680);
1263
-	if (ret)
1264
-		goto err_destroy_mutex;
1265
-
1266
-	ret = __ov2680_power_on(ov2680);
1267
-	if (ret)
1268
-		goto err_free_handler;
1269
-
1270
-	ret = ov2680_check_sensor_id(ov2680, client);
1271
-	if (ret < 0) {
1272
-		dev_info(&client->dev, "%s(%d) Check id  failed\n"
1273
-				  "check following information:\n"
1274
-				  "Power/PowerDown/Reset/Mclk/I2cBus !!\n",
1275
-				  __func__, __LINE__);
1276
-		goto err_power_off;
1277
-	}
1278
-
1279
-#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
1280
-	sd->internal_ops = &ov2680_internal_ops;
1281
-	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1282
-		     V4L2_SUBDEV_FL_HAS_EVENTS;
1283
-#endif
1284
-#if defined(CONFIG_MEDIA_CONTROLLER)
1285
-	ov2680->pad.flags = MEDIA_PAD_FL_SOURCE;
1286
-	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1287
-	ret = media_entity_pads_init(&sd->entity, 1, &ov2680->pad);
916
+	ret = ov2680_mode_init(sensor);
1288
917
 	if (ret < 0)
1289
-		goto err_power_off;
1290
-#endif
918
+		return ret;
1291
919
 
1292
-	memset(facing, 0, sizeof(facing));
1293
-	if (strcmp(ov2680->module_facing, "back") == 0)
1294
-		facing[0] = 'b';
1295
-	else
1296
-		facing[0] = 'f';
1297
-
1298
-	snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
1299
-		 ov2680->module_index, facing,
1300
-		 OV2680_NAME, dev_name(sd->dev));
1301
-
1302
-	ret = v4l2_async_register_subdev_sensor_common(sd);
1303
-	if (ret) {
1304
-		dev_err(dev, "v4l2 async register subdev failed\n");
1305
-		goto err_clean_entity;
920
+	ret = ov2680_get_regulators(sensor);
921
+	if (ret < 0) {
922
+		dev_err(dev, "failed to get regulators\n");
923
+		return ret;
1306
924
 	}
1307
925
 
1308
-	pm_runtime_set_active(dev);
1309
-	pm_runtime_enable(dev);
1310
-	pm_runtime_idle(dev);
926
+	mutex_init(&sensor->lock);
927
+
928
+	ret = ov2680_check_id(sensor);
929
+	if (ret < 0)
930
+		goto lock_destroy;
931
+
932
+	ret = ov2680_v4l2_register(sensor);
933
+	if (ret < 0)
934
+		goto lock_destroy;
935
+
936
+	dev_info(dev, "ov2680 init correctly\n");
1311
937
 
1312
938
 	return 0;
1313
939
 
1314
-err_clean_entity:
1315
-#if defined(CONFIG_MEDIA_CONTROLLER)
1316
-	media_entity_cleanup(&sd->entity);
1317
-#endif
1318
-err_power_off:
1319
-	__ov2680_power_off(ov2680);
1320
-err_free_handler:
1321
-	v4l2_ctrl_handler_free(&ov2680->ctrl_handler);
1322
-err_destroy_mutex:
1323
-	mutex_destroy(&ov2680->mutex);
940
+lock_destroy:
941
+	dev_err(dev, "ov2680 init fail: %d\n", ret);
942
+	mutex_destroy(&sensor->lock);
1324
943
 
1325
944
 	return ret;
1326
945
 }
..
..
@@ -1328,59 +947,71 @@
1328
947
 static int ov2680_remove(struct i2c_client *client)
1329
948
 {
1330
949
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1331
-	struct ov2680 *ov2680 = to_ov2680(sd);
950
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
1332
951
 
1333
-	v4l2_async_unregister_subdev(sd);
1334
-#if defined(CONFIG_MEDIA_CONTROLLER)
1335
-	media_entity_cleanup(&sd->entity);
1336
-#endif
1337
-	v4l2_ctrl_handler_free(&ov2680->ctrl_handler);
1338
-	mutex_destroy(&ov2680->mutex);
1339
-
1340
-	pm_runtime_disable(&client->dev);
1341
-	if (!pm_runtime_status_suspended(&client->dev))
1342
-		__ov2680_power_off(ov2680);
1343
-	pm_runtime_set_suspended(&client->dev);
952
+	v4l2_async_unregister_subdev(&sensor->sd);
953
+	mutex_destroy(&sensor->lock);
954
+	media_entity_cleanup(&sensor->sd.entity);
955
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
1344
956
 
1345
957
 	return 0;
1346
958
 }
1347
959
 
1348
-#if IS_ENABLED(CONFIG_OF)
1349
-static const struct of_device_id ov2680_of_match[] = {
1350
-	{ .compatible = "ovti,ov2680" },
1351
-	{},
1352
-};
1353
-MODULE_DEVICE_TABLE(of, ov2680_of_match);
1354
-#endif
960
+static int __maybe_unused ov2680_suspend(struct device *dev)
961
+{
962
+	struct i2c_client *client = to_i2c_client(dev);
963
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
964
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
1355
965
 
1356
-static const struct i2c_device_id ov2680_match_id[] = {
1357
-	{ "ovti,ov2680", 0 },
1358
-	{ },
966
+	if (sensor->is_streaming)
967
+		ov2680_stream_disable(sensor);
968
+
969
+	return 0;
970
+}
971
+
972
+static int __maybe_unused ov2680_resume(struct device *dev)
973
+{
974
+	struct i2c_client *client = to_i2c_client(dev);
975
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
976
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
977
+	int ret;
978
+
979
+	if (sensor->is_streaming) {
980
+		ret = ov2680_stream_enable(sensor);
981
+		if (ret < 0)
982
+			goto stream_disable;
983
+	}
984
+
985
+	return 0;
986
+
987
+stream_disable:
988
+	ov2680_stream_disable(sensor);
989
+	sensor->is_streaming = false;
990
+
991
+	return ret;
992
+}
993
+
994
+static const struct dev_pm_ops ov2680_pm_ops = {
995
+	SET_SYSTEM_SLEEP_PM_OPS(ov2680_suspend, ov2680_resume)
1359
996
 };
997
+
998
+static const struct of_device_id ov2680_dt_ids[] = {
999
+	{ .compatible = "ovti,ov2680" },
1000
+	{ /* sentinel */ },
1001
+};
1002
+MODULE_DEVICE_TABLE(of, ov2680_dt_ids);
1360
1003
 
1361
1004
 static struct i2c_driver ov2680_i2c_driver = {
1362
1005
 	.driver = {
1363
-		.name = OV2680_NAME,
1006
+		.name  = "ov2680",
1364
1007
 		.pm = &ov2680_pm_ops,
1365
-		.of_match_table = of_match_ptr(ov2680_of_match),
1008
+		.of_match_table	= of_match_ptr(ov2680_dt_ids),
1366
1009
 	},
1367
-	.probe		= &ov2680_probe,
1368
-	.remove		= &ov2680_remove,
1369
-	.id_table	= ov2680_match_id,
1010
+	.probe_new	= ov2680_probe,
1011
+	.remove		= ov2680_remove,
1370
1012
 };
1013
+module_i2c_driver(ov2680_i2c_driver);
1371
1014
 
1372
-static int __init sensor_mod_init(void)
1373
-{
1374
-	return i2c_add_driver(&ov2680_i2c_driver);
1375
-}
1376
-
1377
-static void __exit sensor_mod_exit(void)
1378
-{
1379
-	i2c_del_driver(&ov2680_i2c_driver);
1380
-}
1381
-
1382
-device_initcall_sync(sensor_mod_init);
1383
-module_exit(sensor_mod_exit);
1384
-
1385
-MODULE_DESCRIPTION("OmniVision ov2680 sensor driver");
1015
+MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
1016
+MODULE_DESCRIPTION("OV2680 CMOS Image Sensor driver");
1386
1017
 MODULE_LICENSE("GPL v2");