add boot partition size

hc

2023-12-11 d2ccde1c8e90d38cee87a1b0309ad2827f3fd30d

 kernel/drivers/media/i2c/ov2680.c
..
..
@@ -1,965 +1,889 @@
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];
86
+struct ov2680_ctrls {
87
+	struct v4l2_ctrl_handler handler;
88
+	struct {
89
+		struct v4l2_ctrl *auto_exp;
90
+		struct v4l2_ctrl *exposure;
91
+	};
92
+	struct {
93
+		struct v4l2_ctrl *auto_gain;
94
+		struct v4l2_ctrl *gain;
95
+	};
130
96
 
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;
97
+	struct v4l2_ctrl *hflip;
98
+	struct v4l2_ctrl *vflip;
99
+	struct v4l2_ctrl *test_pattern;
154
100
 };
155
101
 
156
-#define to_ov2680(sd) container_of(sd, struct ov2680, subdev)
102
+struct ov2680_dev {
103
+	struct i2c_client		*i2c_client;
104
+	struct v4l2_subdev		sd;
157
105
 
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},
106
+	struct media_pad		pad;
107
+	struct clk			*xvclk;
108
+	u32				xvclk_freq;
109
+	struct regulator_bulk_data	supplies[OV2680_NUM_SUPPLIES];
244
110
 
245
-	{REG_NULL, 0x00},
111
+	struct gpio_desc		*reset_gpio;
112
+	struct mutex			lock; /* protect members */
113
+
114
+	bool				mode_pending_changes;
115
+	bool				is_enabled;
116
+	bool				is_streaming;
117
+
118
+	struct ov2680_ctrls		ctrls;
119
+	struct v4l2_mbus_framefmt	fmt;
120
+	struct v4l2_fract		frame_interval;
121
+
122
+	const struct ov2680_mode_info	*current_mode;
246
123
 };
247
124
 
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[] = {
125
+static const char * const test_pattern_menu[] = {
317
126
 	"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"
127
+	"Color Bars",
128
+	"Random Data",
129
+	"Square",
130
+	"Black Image",
322
131
 };
323
132
 
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;
133
+static const int ov2680_hv_flip_bayer_order[] = {
134
+	MEDIA_BUS_FMT_SBGGR10_1X10,
135
+	MEDIA_BUS_FMT_SGRBG10_1X10,
136
+	MEDIA_BUS_FMT_SGBRG10_1X10,
137
+	MEDIA_BUS_FMT_SRGGB10_1X10,
138
+};
332
139
 
333
-	dev_dbg(&client->dev, "write reg(0x%x val:0x%06x)!\n", reg, val);
140
+static const struct reg_value ov2680_setting_30fps_QUXGA_800_600[] = {
141
+	{0x3086, 0x01}, {0x370a, 0x23}, {0x3808, 0x03}, {0x3809, 0x20},
142
+	{0x380a, 0x02}, {0x380b, 0x58}, {0x380c, 0x06}, {0x380d, 0xac},
143
+	{0x380e, 0x02}, {0x380f, 0x84}, {0x3811, 0x04}, {0x3813, 0x04},
144
+	{0x3814, 0x31}, {0x3815, 0x31}, {0x3820, 0xc0}, {0x4008, 0x00},
145
+	{0x4009, 0x03}, {0x4837, 0x1e}, {0x3501, 0x4e}, {0x3502, 0xe0},
146
+};
147
+
148
+static const struct reg_value ov2680_setting_30fps_720P_1280_720[] = {
149
+	{0x3086, 0x00}, {0x3808, 0x05}, {0x3809, 0x00}, {0x380a, 0x02},
150
+	{0x380b, 0xd0}, {0x380c, 0x06}, {0x380d, 0xa8}, {0x380e, 0x05},
151
+	{0x380f, 0x0e}, {0x3811, 0x08}, {0x3813, 0x06}, {0x3814, 0x11},
152
+	{0x3815, 0x11}, {0x3820, 0xc0}, {0x4008, 0x00},
153
+};
154
+
155
+static const struct reg_value ov2680_setting_30fps_UXGA_1600_1200[] = {
156
+	{0x3086, 0x00}, {0x3501, 0x4e}, {0x3502, 0xe0}, {0x3808, 0x06},
157
+	{0x3809, 0x40}, {0x380a, 0x04}, {0x380b, 0xb0}, {0x380c, 0x06},
158
+	{0x380d, 0xa8}, {0x380e, 0x05}, {0x380f, 0x0e}, {0x3811, 0x00},
159
+	{0x3813, 0x00}, {0x3814, 0x11}, {0x3815, 0x11}, {0x3820, 0xc0},
160
+	{0x4008, 0x00}, {0x4837, 0x18}
161
+};
162
+
163
+static const struct ov2680_mode_info ov2680_mode_init_data = {
164
+	"mode_quxga_800_600", OV2680_MODE_QUXGA_800_600, 800, 600,
165
+	ov2680_setting_30fps_QUXGA_800_600,
166
+	ARRAY_SIZE(ov2680_setting_30fps_QUXGA_800_600),
167
+};
168
+
169
+static const struct ov2680_mode_info ov2680_mode_data[OV2680_MODE_MAX] = {
170
+	{"mode_quxga_800_600", OV2680_MODE_QUXGA_800_600,
171
+	 800, 600, ov2680_setting_30fps_QUXGA_800_600,
172
+	 ARRAY_SIZE(ov2680_setting_30fps_QUXGA_800_600)},
173
+	{"mode_720p_1280_720", OV2680_MODE_720P_1280_720,
174
+	 1280, 720, ov2680_setting_30fps_720P_1280_720,
175
+	 ARRAY_SIZE(ov2680_setting_30fps_720P_1280_720)},
176
+	{"mode_uxga_1600_1200", OV2680_MODE_UXGA_1600_1200,
177
+	 1600, 1200, ov2680_setting_30fps_UXGA_1600_1200,
178
+	 ARRAY_SIZE(ov2680_setting_30fps_UXGA_1600_1200)},
179
+};
180
+
181
+static struct ov2680_dev *to_ov2680_dev(struct v4l2_subdev *sd)
182
+{
183
+	return container_of(sd, struct ov2680_dev, sd);
184
+}
185
+
186
+static struct device *ov2680_to_dev(struct ov2680_dev *sensor)
187
+{
188
+	return &sensor->i2c_client->dev;
189
+}
190
+
191
+static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
192
+{
193
+	return &container_of(ctrl->handler, struct ov2680_dev,
194
+			     ctrls.handler)->sd;
195
+}
196
+
197
+static int __ov2680_write_reg(struct ov2680_dev *sensor, u16 reg,
198
+			      unsigned int len, u32 val)
199
+{
200
+	struct i2c_client *client = sensor->i2c_client;
201
+	u8 buf[6];
202
+	int ret;
334
203
 
335
204
 	if (len > 4)
336
205
 		return -EINVAL;
337
206
 
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);
207
+	put_unaligned_be16(reg, buf);
208
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
209
+	ret = i2c_master_send(client, buf, len + 2);
210
+	if (ret != len + 2) {
211
+		dev_err(&client->dev, "write error: reg=0x%4x: %d\n", reg, ret);
352
212
 		return -EIO;
353
213
 	}
214
+
354
215
 	return 0;
355
216
 }
356
217
 
357
-static int ov2680_write_array(struct i2c_client *client,
358
-			      const struct regval *regs)
218
+#define ov2680_write_reg(s, r, v) \
219
+	__ov2680_write_reg(s, r, OV2680_REG_VALUE_8BIT, v)
220
+
221
+#define ov2680_write_reg16(s, r, v) \
222
+	__ov2680_write_reg(s, r, OV2680_REG_VALUE_16BIT, v)
223
+
224
+#define ov2680_write_reg24(s, r, v) \
225
+	__ov2680_write_reg(s, r, OV2680_REG_VALUE_24BIT, v)
226
+
227
+static int __ov2680_read_reg(struct ov2680_dev *sensor, u16 reg,
228
+			     unsigned int len, u32 *val)
359
229
 {
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
-{
230
+	struct i2c_client *client = sensor->i2c_client;
374
231
 	struct i2c_msg msgs[2];
375
-	u8 *data_be_p;
376
-	__be32 data_be = 0;
377
-	__be16 reg_addr_be = cpu_to_be16(reg);
232
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
233
+	u8 data_buf[4] = { 0, };
378
234
 	int ret;
379
235
 
380
-	if (len > 4 || !len)
236
+	if (len > 4)
381
237
 		return -EINVAL;
382
238
 
383
-	data_be_p = (u8 *)&data_be;
384
-	/* Write register address */
385
239
 	msgs[0].addr = client->addr;
386
240
 	msgs[0].flags = 0;
387
-	msgs[0].len = 2;
388
-	msgs[0].buf = (u8 *)&reg_addr_be;
241
+	msgs[0].len = ARRAY_SIZE(addr_buf);
242
+	msgs[0].buf = addr_buf;
389
243
 
390
-	/* Read data from register */
391
244
 	msgs[1].addr = client->addr;
392
245
 	msgs[1].flags = I2C_M_RD;
393
246
 	msgs[1].len = len;
394
-	msgs[1].buf = &data_be_p[4 - len];
247
+	msgs[1].buf = &data_buf[4 - len];
395
248
 
396
249
 	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
397
-	if (ret != ARRAY_SIZE(msgs))
250
+	if (ret != ARRAY_SIZE(msgs)) {
251
+		dev_err(&client->dev, "read error: reg=0x%4x: %d\n", reg, ret);
398
252
 		return -EIO;
253
+	}
399
254
 
400
-	*val = be32_to_cpu(data_be);
255
+	*val = get_unaligned_be32(data_buf);
401
256
 
402
257
 	return 0;
403
258
 }
404
259
 
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;
260
+#define ov2680_read_reg(s, r, v) \
261
+	__ov2680_read_reg(s, r, OV2680_REG_VALUE_8BIT, v)
413
262
 
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
-		}
263
+#define ov2680_read_reg16(s, r, v) \
264
+	__ov2680_read_reg(s, r, OV2680_REG_VALUE_16BIT, v)
265
+
266
+#define ov2680_read_reg24(s, r, v) \
267
+	__ov2680_read_reg(s, r, OV2680_REG_VALUE_24BIT, v)
268
+
269
+static int ov2680_mod_reg(struct ov2680_dev *sensor, u16 reg, u8 mask, u8 val)
270
+{
271
+	u32 readval;
272
+	int ret;
273
+
274
+	ret = ov2680_read_reg(sensor, reg, &readval);
275
+	if (ret < 0)
276
+		return ret;
277
+
278
+	readval &= ~mask;
279
+	val &= mask;
280
+	val |= readval;
281
+
282
+	return ov2680_write_reg(sensor, reg, val);
283
+}
284
+
285
+static int ov2680_load_regs(struct ov2680_dev *sensor,
286
+			    const struct ov2680_mode_info *mode)
287
+{
288
+	const struct reg_value *regs = mode->reg_data;
289
+	unsigned int i;
290
+	int ret = 0;
291
+	u16 reg_addr;
292
+	u8 val;
293
+
294
+	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
295
+		reg_addr = regs->reg_addr;
296
+		val = regs->val;
297
+
298
+		ret = ov2680_write_reg(sensor, reg_addr, val);
299
+		if (ret)
300
+			break;
421
301
 	}
302
+
422
303
 	return ret;
423
304
 }
424
-#endif
425
305
 
426
-static int ov2680_get_reso_dist(const struct ov2680_mode *mode,
427
-				struct v4l2_mbus_framefmt *framefmt)
306
+static void ov2680_power_up(struct ov2680_dev *sensor)
428
307
 {
429
-	return abs(mode->width - framefmt->width) +
430
-	       abs(mode->height - framefmt->height);
308
+	if (!sensor->reset_gpio)
309
+		return;
310
+
311
+	gpiod_set_value(sensor->reset_gpio, 0);
312
+	usleep_range(5000, 10000);
431
313
 }
432
314
 
433
-static const struct ov2680_mode *
434
-ov2680_find_best_fit(struct ov2680 *ov2680,
435
-			struct v4l2_subdev_format *fmt)
315
+static void ov2680_power_down(struct ov2680_dev *sensor)
436
316
 {
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;
317
+	if (!sensor->reset_gpio)
318
+		return;
442
319
 
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;
320
+	gpiod_set_value(sensor->reset_gpio, 1);
321
+	usleep_range(5000, 10000);
322
+}
323
+
324
+static int ov2680_bayer_order(struct ov2680_dev *sensor)
325
+{
326
+	u32 format1;
327
+	u32 format2;
328
+	u32 hv_flip;
329
+	int ret;
330
+
331
+	ret = ov2680_read_reg(sensor, OV2680_REG_FORMAT1, &format1);
332
+	if (ret < 0)
333
+		return ret;
334
+
335
+	ret = ov2680_read_reg(sensor, OV2680_REG_FORMAT2, &format2);
336
+	if (ret < 0)
337
+		return ret;
338
+
339
+	hv_flip = (format2 & BIT(2)  << 1) | (format1 & BIT(2));
340
+
341
+	sensor->fmt.code = ov2680_hv_flip_bayer_order[hv_flip];
342
+
343
+	return 0;
344
+}
345
+
346
+static int ov2680_vflip_enable(struct ov2680_dev *sensor)
347
+{
348
+	int ret;
349
+
350
+	ret = ov2680_mod_reg(sensor, OV2680_REG_FORMAT1, BIT(2), BIT(2));
351
+	if (ret < 0)
352
+		return ret;
353
+
354
+	return ov2680_bayer_order(sensor);
355
+}
356
+
357
+static int ov2680_vflip_disable(struct ov2680_dev *sensor)
358
+{
359
+	int ret;
360
+
361
+	ret = ov2680_mod_reg(sensor, OV2680_REG_FORMAT1, BIT(2), BIT(0));
362
+	if (ret < 0)
363
+		return ret;
364
+
365
+	return ov2680_bayer_order(sensor);
366
+}
367
+
368
+static int ov2680_hflip_enable(struct ov2680_dev *sensor)
369
+{
370
+	int ret;
371
+
372
+	ret = ov2680_mod_reg(sensor, OV2680_REG_FORMAT2, BIT(2), BIT(2));
373
+	if (ret < 0)
374
+		return ret;
375
+
376
+	return ov2680_bayer_order(sensor);
377
+}
378
+
379
+static int ov2680_hflip_disable(struct ov2680_dev *sensor)
380
+{
381
+	int ret;
382
+
383
+	ret = ov2680_mod_reg(sensor, OV2680_REG_FORMAT2, BIT(2), BIT(0));
384
+	if (ret < 0)
385
+		return ret;
386
+
387
+	return ov2680_bayer_order(sensor);
388
+}
389
+
390
+static int ov2680_test_pattern_set(struct ov2680_dev *sensor, int value)
391
+{
392
+	int ret;
393
+
394
+	if (!value)
395
+		return ov2680_mod_reg(sensor, OV2680_REG_ISP_CTRL00, BIT(7), 0);
396
+
397
+	ret = ov2680_mod_reg(sensor, OV2680_REG_ISP_CTRL00, 0x03, value - 1);
398
+	if (ret < 0)
399
+		return ret;
400
+
401
+	ret = ov2680_mod_reg(sensor, OV2680_REG_ISP_CTRL00, BIT(7), BIT(7));
402
+	if (ret < 0)
403
+		return ret;
404
+
405
+	return 0;
406
+}
407
+
408
+static int ov2680_gain_set(struct ov2680_dev *sensor, bool auto_gain)
409
+{
410
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
411
+	u32 gain;
412
+	int ret;
413
+
414
+	ret = ov2680_mod_reg(sensor, OV2680_REG_R_MANUAL, BIT(1),
415
+			     auto_gain ? 0 : BIT(1));
416
+	if (ret < 0)
417
+		return ret;
418
+
419
+	if (auto_gain || !ctrls->gain->is_new)
420
+		return 0;
421
+
422
+	gain = ctrls->gain->val;
423
+
424
+	ret = ov2680_write_reg16(sensor, OV2680_REG_GAIN_PK, gain);
425
+
426
+	return 0;
427
+}
428
+
429
+static int ov2680_gain_get(struct ov2680_dev *sensor)
430
+{
431
+	u32 gain;
432
+	int ret;
433
+
434
+	ret = ov2680_read_reg16(sensor, OV2680_REG_GAIN_PK, &gain);
435
+	if (ret)
436
+		return ret;
437
+
438
+	return gain;
439
+}
440
+
441
+static int ov2680_exposure_set(struct ov2680_dev *sensor, bool auto_exp)
442
+{
443
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
444
+	u32 exp;
445
+	int ret;
446
+
447
+	ret = ov2680_mod_reg(sensor, OV2680_REG_R_MANUAL, BIT(0),
448
+			     auto_exp ? 0 : BIT(0));
449
+	if (ret < 0)
450
+		return ret;
451
+
452
+	if (auto_exp || !ctrls->exposure->is_new)
453
+		return 0;
454
+
455
+	exp = (u32)ctrls->exposure->val;
456
+	exp <<= 4;
457
+
458
+	return ov2680_write_reg24(sensor, OV2680_REG_EXPOSURE_PK_HIGH, exp);
459
+}
460
+
461
+static int ov2680_exposure_get(struct ov2680_dev *sensor)
462
+{
463
+	int ret;
464
+	u32 exp;
465
+
466
+	ret = ov2680_read_reg24(sensor, OV2680_REG_EXPOSURE_PK_HIGH, &exp);
467
+	if (ret)
468
+		return ret;
469
+
470
+	return exp >> 4;
471
+}
472
+
473
+static int ov2680_stream_enable(struct ov2680_dev *sensor)
474
+{
475
+	return ov2680_write_reg(sensor, OV2680_REG_STREAM_CTRL, 1);
476
+}
477
+
478
+static int ov2680_stream_disable(struct ov2680_dev *sensor)
479
+{
480
+	return ov2680_write_reg(sensor, OV2680_REG_STREAM_CTRL, 0);
481
+}
482
+
483
+static int ov2680_mode_set(struct ov2680_dev *sensor)
484
+{
485
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
486
+	int ret;
487
+
488
+	ret = ov2680_gain_set(sensor, false);
489
+	if (ret < 0)
490
+		return ret;
491
+
492
+	ret = ov2680_exposure_set(sensor, false);
493
+	if (ret < 0)
494
+		return ret;
495
+
496
+	ret = ov2680_load_regs(sensor, sensor->current_mode);
497
+	if (ret < 0)
498
+		return ret;
499
+
500
+	if (ctrls->auto_gain->val) {
501
+		ret = ov2680_gain_set(sensor, true);
502
+		if (ret < 0)
503
+			return ret;
504
+	}
505
+
506
+	if (ctrls->auto_exp->val == V4L2_EXPOSURE_AUTO) {
507
+		ret = ov2680_exposure_set(sensor, true);
508
+		if (ret < 0)
509
+			return ret;
510
+	}
511
+
512
+	sensor->mode_pending_changes = false;
513
+
514
+	return 0;
515
+}
516
+
517
+static int ov2680_mode_restore(struct ov2680_dev *sensor)
518
+{
519
+	int ret;
520
+
521
+	ret = ov2680_load_regs(sensor, &ov2680_mode_init_data);
522
+	if (ret < 0)
523
+		return ret;
524
+
525
+	return ov2680_mode_set(sensor);
526
+}
527
+
528
+static int ov2680_power_off(struct ov2680_dev *sensor)
529
+{
530
+	if (!sensor->is_enabled)
531
+		return 0;
532
+
533
+	clk_disable_unprepare(sensor->xvclk);
534
+	ov2680_power_down(sensor);
535
+	regulator_bulk_disable(OV2680_NUM_SUPPLIES, sensor->supplies);
536
+	sensor->is_enabled = false;
537
+
538
+	return 0;
539
+}
540
+
541
+static int ov2680_power_on(struct ov2680_dev *sensor)
542
+{
543
+	struct device *dev = ov2680_to_dev(sensor);
544
+	int ret;
545
+
546
+	if (sensor->is_enabled)
547
+		return 0;
548
+
549
+	ret = regulator_bulk_enable(OV2680_NUM_SUPPLIES, sensor->supplies);
550
+	if (ret < 0) {
551
+		dev_err(dev, "failed to enable regulators: %d\n", ret);
552
+		return ret;
553
+	}
554
+
555
+	if (!sensor->reset_gpio) {
556
+		ret = ov2680_write_reg(sensor, OV2680_REG_SOFT_RESET, 0x01);
557
+		if (ret != 0) {
558
+			dev_err(dev, "sensor soft reset failed\n");
559
+			return ret;
448
560
 		}
449
-	}
450
-
451
-	return &supported_modes[cur_best_fit];
452
-}
453
-
454
-static int ov2680_set_fmt(struct v4l2_subdev *sd,
455
-			  struct v4l2_subdev_pad_config *cfg,
456
-			  struct v4l2_subdev_format *fmt)
457
-{
458
-	struct ov2680 *ov2680 = to_ov2680(sd);
459
-	const struct ov2680_mode *mode;
460
-	s64 h_blank, vblank_def;
461
-
462
-	mutex_lock(&ov2680->mutex);
463
-
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
561
+		usleep_range(1000, 2000);
476
562
 	} 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);
563
+		ov2680_power_down(sensor);
564
+		ov2680_power_up(sensor);
485
565
 	}
486
566
 
487
-	mutex_unlock(&ov2680->mutex);
567
+	ret = clk_prepare_enable(sensor->xvclk);
568
+	if (ret < 0)
569
+		return ret;
570
+
571
+	sensor->is_enabled = true;
572
+
573
+	/* Set clock lane into LP-11 state */
574
+	ov2680_stream_enable(sensor);
575
+	usleep_range(1000, 2000);
576
+	ov2680_stream_disable(sensor);
488
577
 
489
578
 	return 0;
490
579
 }
491
580
 
492
-static int ov2680_get_fmt(struct v4l2_subdev *sd,
493
-			  struct v4l2_subdev_pad_config *cfg,
494
-			  struct v4l2_subdev_format *fmt)
581
+static int ov2680_s_power(struct v4l2_subdev *sd, int on)
495
582
 {
496
-	struct ov2680 *ov2680 = to_ov2680(sd);
497
-	const struct ov2680_mode *mode = ov2680->cur_mode;
583
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
584
+	int ret = 0;
498
585
 
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;
586
+	mutex_lock(&sensor->lock);
587
+
588
+	if (on)
589
+		ret = ov2680_power_on(sensor);
590
+	else
591
+		ret = ov2680_power_off(sensor);
592
+
593
+	mutex_unlock(&sensor->lock);
594
+
595
+	if (on && ret == 0) {
596
+		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
597
+		if (ret < 0)
598
+			return ret;
599
+
600
+		ret = ov2680_mode_restore(sensor);
512
601
 	}
513
-	mutex_unlock(&ov2680->mutex);
602
+
603
+	return ret;
604
+}
605
+
606
+static int ov2680_s_g_frame_interval(struct v4l2_subdev *sd,
607
+				     struct v4l2_subdev_frame_interval *fi)
608
+{
609
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
610
+
611
+	mutex_lock(&sensor->lock);
612
+	fi->interval = sensor->frame_interval;
613
+	mutex_unlock(&sensor->lock);
514
614
 
515
615
 	return 0;
616
+}
617
+
618
+static int ov2680_s_stream(struct v4l2_subdev *sd, int enable)
619
+{
620
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
621
+	int ret = 0;
622
+
623
+	mutex_lock(&sensor->lock);
624
+
625
+	if (sensor->is_streaming == !!enable)
626
+		goto unlock;
627
+
628
+	if (enable && sensor->mode_pending_changes) {
629
+		ret = ov2680_mode_set(sensor);
630
+		if (ret < 0)
631
+			goto unlock;
632
+	}
633
+
634
+	if (enable)
635
+		ret = ov2680_stream_enable(sensor);
636
+	else
637
+		ret = ov2680_stream_disable(sensor);
638
+
639
+	sensor->is_streaming = !!enable;
640
+
641
+unlock:
642
+	mutex_unlock(&sensor->lock);
643
+
644
+	return ret;
516
645
 }
517
646
 
518
647
 static int ov2680_enum_mbus_code(struct v4l2_subdev *sd,
519
648
 				 struct v4l2_subdev_pad_config *cfg,
520
649
 				 struct v4l2_subdev_mbus_code_enum *code)
521
650
 {
522
-	if (code->index != 0)
651
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
652
+
653
+	if (code->pad != 0 || code->index != 0)
523
654
 		return -EINVAL;
524
-	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
655
+
656
+	code->code = sensor->fmt.code;
525
657
 
526
658
 	return 0;
527
659
 }
528
660
 
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)
661
+static int ov2680_get_fmt(struct v4l2_subdev *sd,
662
+			  struct v4l2_subdev_pad_config *cfg,
663
+			  struct v4l2_subdev_format *format)
532
664
 {
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;
665
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
666
+	struct v4l2_mbus_framefmt *fmt = NULL;
707
667
 	int ret = 0;
708
668
 
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));
669
+	if (format->pad != 0)
670
+		return -EINVAL;
714
671
 
715
-	mutex_lock(&ov2680->mutex);
716
-	on = !!on;
717
-	if (on == ov2680->streaming)
718
-		goto unlock_and_return;
672
+	mutex_lock(&sensor->lock);
719
673
 
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
-
674
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
843
675
 #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
-}
676
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg, format->pad);
677
+#else
678
+		ret = -EINVAL;
863
679
 #endif
680
+	} else {
681
+		fmt = &sensor->fmt;
682
+	}
864
683
 
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;
684
+	if (fmt)
685
+		format->format = *fmt;
870
686
 
871
-	dev_info(&client->dev, "%s(%d)\n", __func__, __LINE__);
872
-	ret = ov2680_write_array(ov2680->client, ov2680_global_regs);
687
+	mutex_unlock(&sensor->lock);
688
+
873
689
 	return ret;
874
690
 }
875
691
 
876
-static int ov2680_power(struct v4l2_subdev *sd, int on)
692
+static int ov2680_set_fmt(struct v4l2_subdev *sd,
693
+			  struct v4l2_subdev_pad_config *cfg,
694
+			  struct v4l2_subdev_format *format)
877
695
 {
878
-	int ret;
879
-	struct ov2680 *ov2680 = to_ov2680(sd);
880
-	struct i2c_client *client = ov2680->client;
881
-	struct device *dev = &ov2680->client->dev;
696
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
697
+	struct v4l2_mbus_framefmt *fmt = &format->format;
698
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
699
+	struct v4l2_mbus_framefmt *try_fmt;
700
+#endif
701
+	const struct ov2680_mode_info *mode;
702
+	int ret = 0;
882
703
 
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);
704
+	if (format->pad != 0)
705
+		return -EINVAL;
706
+
707
+	mutex_lock(&sensor->lock);
708
+
709
+	if (sensor->is_streaming) {
710
+		ret = -EBUSY;
711
+		goto unlock;
896
712
 	}
713
+
714
+	mode = v4l2_find_nearest_size(ov2680_mode_data,
715
+				      ARRAY_SIZE(ov2680_mode_data), width,
716
+				      height, fmt->width, fmt->height);
717
+	if (!mode) {
718
+		ret = -EINVAL;
719
+		goto unlock;
720
+	}
721
+
722
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
723
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
724
+		try_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
725
+		format->format = *try_fmt;
726
+#endif
727
+		goto unlock;
728
+	}
729
+
730
+	fmt->width = mode->width;
731
+	fmt->height = mode->height;
732
+	fmt->code = sensor->fmt.code;
733
+	fmt->colorspace = sensor->fmt.colorspace;
734
+
735
+	sensor->current_mode = mode;
736
+	sensor->fmt = format->format;
737
+	sensor->mode_pending_changes = true;
738
+
739
+unlock:
740
+	mutex_unlock(&sensor->lock);
741
+
742
+	return ret;
743
+}
744
+
745
+static int ov2680_init_cfg(struct v4l2_subdev *sd,
746
+			   struct v4l2_subdev_pad_config *cfg)
747
+{
748
+	struct v4l2_subdev_format fmt = {
749
+		.which = cfg ? V4L2_SUBDEV_FORMAT_TRY
750
+				: V4L2_SUBDEV_FORMAT_ACTIVE,
751
+		.format = {
752
+			.width = 800,
753
+			.height = 600,
754
+		}
755
+	};
756
+
757
+	return ov2680_set_fmt(sd, cfg, &fmt);
758
+}
759
+
760
+static int ov2680_enum_frame_size(struct v4l2_subdev *sd,
761
+				  struct v4l2_subdev_pad_config *cfg,
762
+				  struct v4l2_subdev_frame_size_enum *fse)
763
+{
764
+	int index = fse->index;
765
+
766
+	if (index >= OV2680_MODE_MAX || index < 0)
767
+		return -EINVAL;
768
+
769
+	fse->min_width = ov2680_mode_data[index].width;
770
+	fse->min_height = ov2680_mode_data[index].height;
771
+	fse->max_width = ov2680_mode_data[index].width;
772
+	fse->max_height = ov2680_mode_data[index].height;
773
+
897
774
 	return 0;
898
775
 }
899
776
 
900
777
 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)
778
+			      struct v4l2_subdev_pad_config *cfg,
779
+			      struct v4l2_subdev_frame_interval_enum *fie)
903
780
 {
904
-	struct ov2680 *ov2680 = to_ov2680(sd);
781
+	struct v4l2_fract tpf;
905
782
 
906
-	if (fie->index >= ov2680->cfg_num)
783
+	if (fie->index >= OV2680_MODE_MAX || fie->width > OV2680_WIDTH_MAX ||
784
+	    fie->height > OV2680_HEIGHT_MAX ||
785
+	    fie->which > V4L2_SUBDEV_FORMAT_ACTIVE)
907
786
 		return -EINVAL;
908
787
 
909
-	if (fie->code != MEDIA_BUS_FMT_SBGGR10_1X10)
910
-		return -EINVAL;
788
+	tpf.denominator = OV2680_FRAME_RATE;
789
+	tpf.numerator = 1;
911
790
 
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;
791
+	fie->interval = tpf;
928
792
 
929
793
 	return 0;
930
794
 }
931
795
 
932
-static const struct dev_pm_ops ov2680_pm_ops = {
933
-	SET_RUNTIME_PM_OPS(ov2680_runtime_suspend,
934
-			   ov2680_runtime_resume, NULL)
935
-};
796
+static int ov2680_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
797
+{
798
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
799
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
800
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
801
+	int val;
936
802
 
937
-#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
938
-static const struct v4l2_subdev_internal_ops ov2680_internal_ops = {
939
-	.open = ov2680_open,
803
+	if (!sensor->is_enabled)
804
+		return 0;
805
+
806
+	switch (ctrl->id) {
807
+	case V4L2_CID_GAIN:
808
+		val = ov2680_gain_get(sensor);
809
+		if (val < 0)
810
+			return val;
811
+		ctrls->gain->val = val;
812
+		break;
813
+	case V4L2_CID_EXPOSURE:
814
+		val = ov2680_exposure_get(sensor);
815
+		if (val < 0)
816
+			return val;
817
+		ctrls->exposure->val = val;
818
+		break;
819
+	}
820
+
821
+	return 0;
822
+}
823
+
824
+static int ov2680_s_ctrl(struct v4l2_ctrl *ctrl)
825
+{
826
+	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
827
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
828
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
829
+
830
+	if (!sensor->is_enabled)
831
+		return 0;
832
+
833
+	switch (ctrl->id) {
834
+	case V4L2_CID_AUTOGAIN:
835
+		return ov2680_gain_set(sensor, !!ctrl->val);
836
+	case V4L2_CID_GAIN:
837
+		return ov2680_gain_set(sensor, !!ctrls->auto_gain->val);
838
+	case V4L2_CID_EXPOSURE_AUTO:
839
+		return ov2680_exposure_set(sensor, !!ctrl->val);
840
+	case V4L2_CID_EXPOSURE:
841
+		return ov2680_exposure_set(sensor, !!ctrls->auto_exp->val);
842
+	case V4L2_CID_VFLIP:
843
+		if (sensor->is_streaming)
844
+			return -EBUSY;
845
+		if (ctrl->val)
846
+			return ov2680_vflip_enable(sensor);
847
+		else
848
+			return ov2680_vflip_disable(sensor);
849
+	case V4L2_CID_HFLIP:
850
+		if (sensor->is_streaming)
851
+			return -EBUSY;
852
+		if (ctrl->val)
853
+			return ov2680_hflip_enable(sensor);
854
+		else
855
+			return ov2680_hflip_disable(sensor);
856
+	case V4L2_CID_TEST_PATTERN:
857
+		return ov2680_test_pattern_set(sensor, ctrl->val);
858
+	default:
859
+		break;
860
+	}
861
+
862
+	return -EINVAL;
863
+}
864
+
865
+static const struct v4l2_ctrl_ops ov2680_ctrl_ops = {
866
+	.g_volatile_ctrl = ov2680_g_volatile_ctrl,
867
+	.s_ctrl = ov2680_s_ctrl,
940
868
 };
941
-#endif
942
869
 
943
870
 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,
871
+	.s_power = ov2680_s_power,
949
872
 };
950
873
 
951
874
 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,
875
+	.g_frame_interval	= ov2680_s_g_frame_interval,
876
+	.s_frame_interval	= ov2680_s_g_frame_interval,
877
+	.s_stream		= ov2680_s_stream,
955
878
 };
956
879
 
957
880
 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,
881
+	.init_cfg		= ov2680_init_cfg,
882
+	.enum_mbus_code		= ov2680_enum_mbus_code,
883
+	.get_fmt		= ov2680_get_fmt,
884
+	.set_fmt		= ov2680_set_fmt,
885
+	.enum_frame_size	= ov2680_enum_frame_size,
886
+	.enum_frame_interval	= ov2680_enum_frame_interval,
963
887
 };
964
888
 
965
889
 static const struct v4l2_subdev_ops ov2680_subdev_ops = {
..
..
@@ -968,359 +892,206 @@
968
892
 	.pad	= &ov2680_pad_ops,
969
893
 };
970
894
 
971
-static int ov2680_set_ctrl(struct v4l2_ctrl *ctrl)
895
+static int ov2680_mode_init(struct ov2680_dev *sensor)
972
896
 {
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;
897
+	const struct ov2680_mode_info *init_mode;
978
898
 
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
-	}
899
+	/* set initial mode */
900
+	sensor->fmt.code = MEDIA_BUS_FMT_SBGGR10_1X10;
901
+	sensor->fmt.width = 800;
902
+	sensor->fmt.height = 600;
903
+	sensor->fmt.field = V4L2_FIELD_NONE;
904
+	sensor->fmt.colorspace = V4L2_COLORSPACE_SRGB;
990
905
 
991
-	if (!pm_runtime_get_if_in_use(&client->dev))
992
-		return 0;
906
+	sensor->frame_interval.denominator = OV2680_FRAME_RATE;
907
+	sensor->frame_interval.numerator = 1;
993
908
 
994
-	switch (ctrl->id) {
995
-	case V4L2_CID_EXPOSURE:
996
-		/* 4 least significant bits of expsoure are fractional part */
909
+	init_mode = &ov2680_mode_init_data;
997
910
 
998
-		ret = ov2680_write_reg(ov2680->client, OV2680_REG_EXPOSURE,
999
-				       OV2680_REG_VALUE_24BIT, ctrl->val << 4);
911
+	sensor->current_mode = init_mode;
1000
912
 
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:
913
+	sensor->mode_pending_changes = true;
1012
914
 
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;
915
+	return 0;
1029
916
 }
1030
917
 
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)
918
+static int ov2680_v4l2_register(struct ov2680_dev *sensor)
1036
919
 {
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;
920
+	const struct v4l2_ctrl_ops *ops = &ov2680_ctrl_ops;
921
+	struct ov2680_ctrls *ctrls = &sensor->ctrls;
922
+	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
923
+	int ret = 0;
1043
924
 
1044
-	handler = &ov2680->ctrl_handler;
1045
-	mode = ov2680->cur_mode;
1046
-	ret = v4l2_ctrl_handler_init(handler, 8);
1047
-	if (ret)
925
+	v4l2_i2c_subdev_init(&sensor->sd, sensor->i2c_client,
926
+			     &ov2680_subdev_ops);
927
+
928
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
929
+	sensor->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
930
+#endif
931
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
932
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
933
+
934
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
935
+	if (ret < 0)
1048
936
 		return ret;
1049
-	handler->lock = &ov2680->mutex;
1050
937
 
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;
938
+	v4l2_ctrl_handler_init(hdl, 7);
1055
939
 
1056
-	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
1057
-			  0, ov2680->pixel_rate, 1, ov2680->pixel_rate);
940
+	hdl->lock = &sensor->lock;
1058
941
 
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;
942
+	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
943
+	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1064
944
 
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);
945
+	ctrls->test_pattern = v4l2_ctrl_new_std_menu_items(hdl,
946
+					&ov2680_ctrl_ops, V4L2_CID_TEST_PATTERN,
947
+					ARRAY_SIZE(test_pattern_menu) - 1,
948
+					0, 0, test_pattern_menu);
1070
949
 
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);
950
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
951
+						 V4L2_CID_EXPOSURE_AUTO,
952
+						 V4L2_EXPOSURE_MANUAL, 0,
953
+						 V4L2_EXPOSURE_AUTO);
1076
954
 
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);
955
+	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
956
+					    0, 32767, 1, 0);
1081
957
 
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);
958
+	ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
959
+					     0, 1, 1, 1);
960
+	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN, 0, 2047, 1, 0);
1087
961
 
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;
962
+	if (hdl->error) {
963
+		ret = hdl->error;
964
+		goto cleanup_entity;
1098
965
 	}
1099
966
 
1100
-	ov2680->subdev.ctrl_handler = handler;
967
+	ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
968
+	ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
969
+
970
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
971
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
972
+
973
+	sensor->sd.ctrl_handler = hdl;
974
+
975
+	ret = v4l2_async_register_subdev(&sensor->sd);
976
+	if (ret < 0)
977
+		goto cleanup_entity;
1101
978
 
1102
979
 	return 0;
1103
980
 
1104
-err_free_handler:
1105
-	v4l2_ctrl_handler_free(handler);
981
+cleanup_entity:
982
+	media_entity_cleanup(&sensor->sd.entity);
983
+	v4l2_ctrl_handler_free(hdl);
1106
984
 
1107
985
 	return ret;
1108
986
 }
1109
987
 
1110
-static int ov2680_check_sensor_id(struct ov2680 *ov2680,
1111
-				  struct i2c_client *client)
988
+static int ov2680_get_regulators(struct ov2680_dev *sensor)
1112
989
 {
1113
-	struct device *dev = &ov2680->client->dev;
1114
-	u32 id = 0;
990
+	int i;
991
+
992
+	for (i = 0; i < OV2680_NUM_SUPPLIES; i++)
993
+		sensor->supplies[i].supply = ov2680_supply_name[i];
994
+
995
+	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
996
+				       OV2680_NUM_SUPPLIES,
997
+				       sensor->supplies);
998
+}
999
+
1000
+static int ov2680_check_id(struct ov2680_dev *sensor)
1001
+{
1002
+	struct device *dev = ov2680_to_dev(sensor);
1003
+	u32 chip_id;
1115
1004
 	int ret;
1116
1005
 
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);
1006
+	ov2680_power_on(sensor);
1007
+
1008
+	ret = ov2680_read_reg16(sensor, OV2680_REG_CHIP_ID_HIGH, &chip_id);
1009
+	if (ret < 0) {
1010
+		dev_err(dev, "failed to read chip id high\n");
1121
1011
 		return -ENODEV;
1122
1012
 	}
1123
1013
 
1124
-	dev_info(dev, "Detected OV%06x sensor\n", CHIP_ID);
1014
+	if (chip_id != OV2680_CHIP_ID) {
1015
+		dev_err(dev, "chip id: 0x%04x does not match expected 0x%04x\n",
1016
+			chip_id, OV2680_CHIP_ID);
1017
+		return -ENODEV;
1018
+	}
1125
1019
 
1126
1020
 	return 0;
1127
1021
 }
1128
1022
 
1129
-static int ov2680_configure_regulators(struct ov2680 *ov2680)
1023
+static int ov2680_parse_dt(struct ov2680_dev *sensor)
1130
1024
 {
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";
1025
+	struct device *dev = ov2680_to_dev(sensor);
1185
1026
 	int ret;
1186
1027
 
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");
1028
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1029
+						     GPIOD_OUT_HIGH);
1030
+	ret = PTR_ERR_OR_ZERO(sensor->reset_gpio);
1031
+	if (ret < 0) {
1032
+		dev_dbg(dev, "error while getting reset gpio: %d\n", ret);
1237
1033
 		return ret;
1238
1034
 	}
1239
-	ret = ov2680_parse_of(ov2680);
1240
-	if (ret != 0)
1035
+
1036
+	sensor->xvclk = devm_clk_get(dev, "xvclk");
1037
+	if (IS_ERR(sensor->xvclk)) {
1038
+		dev_err(dev, "xvclk clock missing or invalid\n");
1039
+		return PTR_ERR(sensor->xvclk);
1040
+	}
1041
+
1042
+	sensor->xvclk_freq = clk_get_rate(sensor->xvclk);
1043
+	if (sensor->xvclk_freq != OV2680_XVCLK_VALUE) {
1044
+		dev_err(dev, "wrong xvclk frequency %d HZ, expected: %d Hz\n",
1045
+			sensor->xvclk_freq, OV2680_XVCLK_VALUE);
1046
+		return -EINVAL;
1047
+	}
1048
+
1049
+	return 0;
1050
+}
1051
+
1052
+static int ov2680_probe(struct i2c_client *client)
1053
+{
1054
+	struct device *dev = &client->dev;
1055
+	struct ov2680_dev *sensor;
1056
+	int ret;
1057
+
1058
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
1059
+	if (!sensor)
1060
+		return -ENOMEM;
1061
+
1062
+	sensor->i2c_client = client;
1063
+
1064
+	ret = ov2680_parse_dt(sensor);
1065
+	if (ret < 0)
1241
1066
 		return -EINVAL;
1242
1067
 
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);
1068
+	ret = ov2680_mode_init(sensor);
1288
1069
 	if (ret < 0)
1289
-		goto err_power_off;
1290
-#endif
1070
+		return ret;
1291
1071
 
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;
1072
+	ret = ov2680_get_regulators(sensor);
1073
+	if (ret < 0) {
1074
+		dev_err(dev, "failed to get regulators\n");
1075
+		return ret;
1306
1076
 	}
1307
1077
 
1308
-	pm_runtime_set_active(dev);
1309
-	pm_runtime_enable(dev);
1310
-	pm_runtime_idle(dev);
1078
+	mutex_init(&sensor->lock);
1079
+
1080
+	ret = ov2680_check_id(sensor);
1081
+	if (ret < 0)
1082
+		goto lock_destroy;
1083
+
1084
+	ret = ov2680_v4l2_register(sensor);
1085
+	if (ret < 0)
1086
+		goto lock_destroy;
1087
+
1088
+	dev_info(dev, "ov2680 init correctly\n");
1311
1089
 
1312
1090
 	return 0;
1313
1091
 
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);
1092
+lock_destroy:
1093
+	dev_err(dev, "ov2680 init fail: %d\n", ret);
1094
+	mutex_destroy(&sensor->lock);
1324
1095
 
1325
1096
 	return ret;
1326
1097
 }
..
..
@@ -1328,59 +1099,71 @@
1328
1099
 static int ov2680_remove(struct i2c_client *client)
1329
1100
 {
1330
1101
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1331
-	struct ov2680 *ov2680 = to_ov2680(sd);
1102
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
1332
1103
 
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);
1104
+	v4l2_async_unregister_subdev(&sensor->sd);
1105
+	mutex_destroy(&sensor->lock);
1106
+	media_entity_cleanup(&sensor->sd.entity);
1107
+	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
1344
1108
 
1345
1109
 	return 0;
1346
1110
 }
1347
1111
 
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
1112
+static int __maybe_unused ov2680_suspend(struct device *dev)
1113
+{
1114
+	struct i2c_client *client = to_i2c_client(dev);
1115
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1116
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
1355
1117
 
1356
-static const struct i2c_device_id ov2680_match_id[] = {
1357
-	{ "ovti,ov2680", 0 },
1358
-	{ },
1118
+	if (sensor->is_streaming)
1119
+		ov2680_stream_disable(sensor);
1120
+
1121
+	return 0;
1122
+}
1123
+
1124
+static int __maybe_unused ov2680_resume(struct device *dev)
1125
+{
1126
+	struct i2c_client *client = to_i2c_client(dev);
1127
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1128
+	struct ov2680_dev *sensor = to_ov2680_dev(sd);
1129
+	int ret;
1130
+
1131
+	if (sensor->is_streaming) {
1132
+		ret = ov2680_stream_enable(sensor);
1133
+		if (ret < 0)
1134
+			goto stream_disable;
1135
+	}
1136
+
1137
+	return 0;
1138
+
1139
+stream_disable:
1140
+	ov2680_stream_disable(sensor);
1141
+	sensor->is_streaming = false;
1142
+
1143
+	return ret;
1144
+}
1145
+
1146
+static const struct dev_pm_ops ov2680_pm_ops = {
1147
+	SET_SYSTEM_SLEEP_PM_OPS(ov2680_suspend, ov2680_resume)
1359
1148
 };
1149
+
1150
+static const struct of_device_id ov2680_dt_ids[] = {
1151
+	{ .compatible = "ovti,ov2680" },
1152
+	{ /* sentinel */ },
1153
+};
1154
+MODULE_DEVICE_TABLE(of, ov2680_dt_ids);
1360
1155
 
1361
1156
 static struct i2c_driver ov2680_i2c_driver = {
1362
1157
 	.driver = {
1363
-		.name = OV2680_NAME,
1158
+		.name  = "ov2680",
1364
1159
 		.pm = &ov2680_pm_ops,
1365
-		.of_match_table = of_match_ptr(ov2680_of_match),
1160
+		.of_match_table	= of_match_ptr(ov2680_dt_ids),
1366
1161
 	},
1367
-	.probe		= &ov2680_probe,
1368
-	.remove		= &ov2680_remove,
1369
-	.id_table	= ov2680_match_id,
1162
+	.probe_new	= ov2680_probe,
1163
+	.remove		= ov2680_remove,
1370
1164
 };
1165
+module_i2c_driver(ov2680_i2c_driver);
1371
1166
 
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");
1167
+MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
1168
+MODULE_DESCRIPTION("OV2680 CMOS Image Sensor driver");
1386
1169
 MODULE_LICENSE("GPL v2");