移去rt

hc

2023-12-08 01573e231f18eb2d99162747186f59511f56b64d

 kernel/drivers/media/i2c/imx219.c
..
..
@@ -1,1147 +1,1593 @@
1
+// SPDX-License-Identifier: GPL-2.0
1
2
 /*
2
- * Driver for IMX219 CMOS Image Sensor from Sony
3
+ * A V4L2 driver for Sony IMX219 cameras.
4
+ * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
3
5
  *
4
- * Copyright (C) 2014, Andrew Chew <achew@nvidia.com>
6
+ * Based on Sony imx258 camera driver
7
+ * Copyright (C) 2018 Intel Corporation
5
8
  *
6
- * This program is free software; you can redistribute it and/or modify
7
- * it under the terms of the GNU General Public License version 2 as
8
- * published by the Free Software Foundation.
9
- * V0.0X01.0X01 add enum_frame_interval function.
10
- * V0.0X01.0X02 add function g_mbus_config.
9
+ * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
10
+ * Copyright 2018 Qtechnology A/S
11
+ *
12
+ * Flip handling taken from the Sony IMX319 driver.
13
+ * Copyright (C) 2018 Intel Corporation
14
+ *
11
15
  */
16
+
12
17
 #include <linux/clk.h>
13
18
 #include <linux/delay.h>
19
+#include <linux/gpio/consumer.h>
14
20
 #include <linux/i2c.h>
15
-#include <linux/init.h>
16
-#include <linux/io.h>
17
21
 #include <linux/module.h>
18
-#include <linux/of_graph.h>
19
-#include <linux/slab.h>
20
-#include <linux/videodev2.h>
21
-#include <linux/version.h>
22
-#include <linux/rk-camera-module.h>
22
+#include <linux/pm_runtime.h>
23
+#include <linux/regulator/consumer.h>
23
24
 #include <media/v4l2-ctrls.h>
24
25
 #include <media/v4l2-device.h>
26
+#include <media/v4l2-event.h>
25
27
 #include <media/v4l2-fwnode.h>
26
-#include <media/v4l2-image-sizes.h>
27
28
 #include <media/v4l2-mediabus.h>
29
+#include <asm/unaligned.h>
28
30
 
29
-#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x2)
31
+#define IMX219_REG_VALUE_08BIT		1
32
+#define IMX219_REG_VALUE_16BIT		2
30
33
 
31
-/* IMX219 supported geometry */
32
-#define IMX219_TABLE_END		0xffff
33
-#define IMX219_ANALOGUE_GAIN_MULTIPLIER	256
34
-#define IMX219_ANALOGUE_GAIN_MIN	(1 * IMX219_ANALOGUE_GAIN_MULTIPLIER)
35
-#define IMX219_ANALOGUE_GAIN_MAX	(11 * IMX219_ANALOGUE_GAIN_MULTIPLIER)
36
-#define IMX219_ANALOGUE_GAIN_DEFAULT	(2 * IMX219_ANALOGUE_GAIN_MULTIPLIER)
34
+#define IMX219_REG_MODE_SELECT		0x0100
35
+#define IMX219_MODE_STANDBY		0x00
36
+#define IMX219_MODE_STREAMING		0x01
37
37
 
38
-/* In dB*256 */
39
-#define IMX219_DIGITAL_GAIN_MIN		256
40
-#define IMX219_DIGITAL_GAIN_MAX		43663
41
-#define IMX219_DIGITAL_GAIN_DEFAULT	256
38
+/* Chip ID */
39
+#define IMX219_REG_CHIP_ID		0x0000
40
+#define IMX219_CHIP_ID			0x0219
42
41
 
43
-#define IMX219_DIGITAL_EXPOSURE_MIN	0
44
-#define IMX219_DIGITAL_EXPOSURE_MAX	4095
45
-#define IMX219_DIGITAL_EXPOSURE_DEFAULT	1575
42
+/* External clock frequency is 24.0M */
43
+#define IMX219_XCLK_FREQ		24000000
46
44
 
47
-#define IMX219_EXP_LINES_MARGIN	4
45
+/* Pixel rate is fixed at 182.4M for all the modes */
46
+#define IMX219_PIXEL_RATE		182400000
48
47
 
49
-#define IMX219_NAME			"imx219"
48
+#define IMX219_DEFAULT_LINK_FREQ	456000000
50
49
 
51
-#define IMX219_LANES			2
50
+/* V_TIMING internal */
51
+#define IMX219_REG_VTS			0x0160
52
+#define IMX219_VTS_15FPS		0x0dc6
53
+#define IMX219_VTS_30FPS_1080P		0x06e3
54
+#define IMX219_VTS_30FPS_BINNED		0x06e3
55
+#define IMX219_VTS_30FPS_640x480	0x06e3
56
+#define IMX219_VTS_MAX			0xffff
52
57
 
53
-static const s64 link_freq_menu_items[] = {
54
-	456000000,
55
-};
58
+#define IMX219_VBLANK_MIN		4
59
+
60
+/*Frame Length Line*/
61
+#define IMX219_FLL_MIN			0x08a6
62
+#define IMX219_FLL_MAX			0xffff
63
+#define IMX219_FLL_STEP			1
64
+#define IMX219_FLL_DEFAULT		0x0c98
65
+
66
+/* HBLANK control - read only */
67
+#define IMX219_PPL_DEFAULT		3448
68
+
69
+/* Exposure control */
70
+#define IMX219_REG_EXPOSURE		0x015a
71
+#define IMX219_EXPOSURE_MIN		4
72
+#define IMX219_EXPOSURE_STEP		1
73
+#define IMX219_EXPOSURE_DEFAULT		0x640
74
+#define IMX219_EXPOSURE_MAX		65535
75
+
76
+/* Analog gain control */
77
+#define IMX219_REG_ANALOG_GAIN		0x0157
78
+#define IMX219_ANA_GAIN_MIN		0
79
+#define IMX219_ANA_GAIN_MAX		232
80
+#define IMX219_ANA_GAIN_STEP		1
81
+#define IMX219_ANA_GAIN_DEFAULT		0x0
82
+
83
+/* Digital gain control */
84
+#define IMX219_REG_DIGITAL_GAIN		0x0158
85
+#define IMX219_DGTL_GAIN_MIN		0x0100
86
+#define IMX219_DGTL_GAIN_MAX		0x0fff
87
+#define IMX219_DGTL_GAIN_DEFAULT	0x0100
88
+#define IMX219_DGTL_GAIN_STEP		1
89
+
90
+#define IMX219_REG_ORIENTATION		0x0172
91
+
92
+/* Test Pattern Control */
93
+#define IMX219_REG_TEST_PATTERN		0x0600
94
+#define IMX219_TEST_PATTERN_DISABLE	0
95
+#define IMX219_TEST_PATTERN_SOLID_COLOR	1
96
+#define IMX219_TEST_PATTERN_COLOR_BARS	2
97
+#define IMX219_TEST_PATTERN_GREY_COLOR	3
98
+#define IMX219_TEST_PATTERN_PN9		4
99
+
100
+/* Test pattern colour components */
101
+#define IMX219_REG_TESTP_RED		0x0602
102
+#define IMX219_REG_TESTP_GREENR		0x0604
103
+#define IMX219_REG_TESTP_BLUE		0x0606
104
+#define IMX219_REG_TESTP_GREENB		0x0608
105
+#define IMX219_TESTP_COLOUR_MIN		0
106
+#define IMX219_TESTP_COLOUR_MAX		0x03ff
107
+#define IMX219_TESTP_COLOUR_STEP	1
108
+#define IMX219_TESTP_RED_DEFAULT	IMX219_TESTP_COLOUR_MAX
109
+#define IMX219_TESTP_GREENR_DEFAULT	0
110
+#define IMX219_TESTP_BLUE_DEFAULT	0
111
+#define IMX219_TESTP_GREENB_DEFAULT	0
112
+
113
+/* IMX219 native and active pixel array size. */
114
+#define IMX219_NATIVE_WIDTH		3296U
115
+#define IMX219_NATIVE_HEIGHT		2480U
116
+#define IMX219_PIXEL_ARRAY_LEFT		8U
117
+#define IMX219_PIXEL_ARRAY_TOP		8U
118
+#define IMX219_PIXEL_ARRAY_WIDTH	3280U
119
+#define IMX219_PIXEL_ARRAY_HEIGHT	2464U
56
120
 
57
121
 struct imx219_reg {
58
-	u16 addr;
122
+	u16 address;
59
123
 	u8 val;
60
124
 };
61
125
 
126
+struct imx219_reg_list {
127
+	unsigned int num_of_regs;
128
+	const struct imx219_reg *regs;
129
+};
130
+
131
+/* Mode : resolution and related config&values */
62
132
 struct imx219_mode {
63
-	u32 width;
64
-	u32 height;
65
-	struct v4l2_fract max_fps;
66
-	u32 hts_def;
67
-	u32 vts_def;
68
-	const struct imx219_reg *reg_list;
133
+	/* Frame width */
134
+	unsigned int width;
135
+	/* Frame height */
136
+	unsigned int height;
137
+
138
+	/* Analog crop rectangle. */
139
+	struct v4l2_rect crop;
140
+
141
+	/* V-timing */
142
+	unsigned int vts_def;
143
+
144
+	/* Default register values */
145
+	struct imx219_reg_list reg_list;
69
146
 };
70
147
 
71
-/* MCLK:24MHz  3280x2464  21.2fps   MIPI LANE2 */
72
-static const struct imx219_reg imx219_init_tab_3280_2464_21fps[] = {
73
-	{0x30EB, 0x05},		/* Access Code for address over 0x3000 */
74
-	{0x30EB, 0x0C},		/* Access Code for address over 0x3000 */
75
-	{0x300A, 0xFF},		/* Access Code for address over 0x3000 */
76
-	{0x300B, 0xFF},		/* Access Code for address over 0x3000 */
77
-	{0x30EB, 0x05},		/* Access Code for address over 0x3000 */
78
-	{0x30EB, 0x09},		/* Access Code for address over 0x3000 */
79
-	{0x0114, 0x01},		/* CSI_LANE_MODE[1:0} */
80
-	{0x0128, 0x00},		/* DPHY_CNTRL */
81
-	{0x012A, 0x18},		/* EXCK_FREQ[15:8] */
82
-	{0x012B, 0x00},		/* EXCK_FREQ[7:0] */
83
-	{0x015A, 0x01},		/* INTEG TIME[15:8] */
84
-	{0x015B, 0xF4},		/* INTEG TIME[7:0] */
85
-	{0x0160, 0x09},		/* FRM_LENGTH_A[15:8] */
86
-	{0x0161, 0xC4},		/* FRM_LENGTH_A[7:0] */
87
-	{0x0162, 0x0D},		/* LINE_LENGTH_A[15:8] */
88
-	{0x0163, 0x78},		/* LINE_LENGTH_A[7:0] */
89
-	{0x0260, 0x09},		/* FRM_LENGTH_B[15:8] */
90
-	{0x0261, 0xC4},		/* FRM_LENGTH_B[7:0] */
91
-	{0x0262, 0x0D},		/* LINE_LENGTH_B[15:8] */
92
-	{0x0263, 0x78},		/* LINE_LENGTH_B[7:0] */
93
-	{0x0170, 0x01},		/* X_ODD_INC_A[2:0] */
94
-	{0x0171, 0x01},		/* Y_ODD_INC_A[2:0] */
95
-	{0x0270, 0x01},		/* X_ODD_INC_B[2:0] */
96
-	{0x0271, 0x01},		/* Y_ODD_INC_B[2:0] */
97
-	{0x0174, 0x00},		/* BINNING_MODE_H_A */
98
-	{0x0175, 0x00},		/* BINNING_MODE_V_A */
99
-	{0x0274, 0x00},		/* BINNING_MODE_H_B */
100
-	{0x0275, 0x00},		/* BINNING_MODE_V_B */
101
-	{0x018C, 0x0A},		/* CSI_DATA_FORMAT_A[15:8] */
102
-	{0x018D, 0x0A},		/* CSI_DATA_FORMAT_A[7:0] */
103
-	{0x028C, 0x0A},		/* CSI_DATA_FORMAT_B[15:8] */
104
-	{0x028D, 0x0A},		/* CSI_DATA_FORMAT_B[7:0] */
105
-	{0x0301, 0x05},		/* VTPXCK_DIV */
106
-	{0x0303, 0x01},		/* VTSYCK_DIV */
107
-	{0x0304, 0x03},		/* PREPLLCK_VT_DIV[3:0] */
108
-	{0x0305, 0x03},		/* PREPLLCK_OP_DIV[3:0] */
109
-	{0x0306, 0x00},		/* PLL_VT_MPY[10:8] */
110
-	{0x0307, 0x39},		/* PLL_VT_MPY[7:0] */
111
-	{0x0309, 0x0A},		/* OPPXCK_DIV[4:0] */
112
-	{0x030B, 0x01},		/* OPSYCK_DIV */
113
-	{0x030C, 0x00},		/* PLL_OP_MPY[10:8] */
114
-	{0x030D, 0x72},		/* PLL_OP_MPY[7:0] */
115
-	{0x455E, 0x00},		/* CIS Tuning */
116
-	{0x471E, 0x4B},		/* CIS Tuning */
117
-	{0x4767, 0x0F},		/* CIS Tuning */
118
-	{0x4750, 0x14},		/* CIS Tuning */
119
-	{0x47B4, 0x14},		/* CIS Tuning */
120
-	{IMX219_TABLE_END, 0x00}
121
-};
122
-
123
-/* MCLK:24MHz  1920x1080  30fps   MIPI LANE2 */
124
-static const struct imx219_reg imx219_init_tab_1920_1080_30fps[] = {
125
-	{0x30EB, 0x05},
126
-	{0x30EB, 0x0C},
127
-	{0x300A, 0xFF},
128
-	{0x300B, 0xFF},
129
-	{0x30EB, 0x05},
130
-	{0x30EB, 0x09},
148
+/*
149
+ * Register sets lifted off the i2C interface from the Raspberry Pi firmware
150
+ * driver.
151
+ * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
152
+ */
153
+static const struct imx219_reg mode_3280x2464_regs[] = {
154
+	{0x0100, 0x00},
155
+	{0x30eb, 0x0c},
156
+	{0x30eb, 0x05},
157
+	{0x300a, 0xff},
158
+	{0x300b, 0xff},
159
+	{0x30eb, 0x05},
160
+	{0x30eb, 0x09},
131
161
 	{0x0114, 0x01},
132
162
 	{0x0128, 0x00},
133
-	{0x012A, 0x18},
134
-	{0x012B, 0x00},
135
-	{0x0160, 0x06},
136
-	{0x0161, 0xE6},
137
-	{0x0162, 0x0D},
138
-	{0x0163, 0x78},
139
-	{0x0164, 0x02},
140
-	{0x0165, 0xA8},
141
-	{0x0166, 0x0A},
142
-	{0x0167, 0x27},
143
-	{0x0168, 0x02},
144
-	{0x0169, 0xB4},
145
-	{0x016A, 0x06},
146
-	{0x016B, 0xEB},
147
-	{0x016C, 0x07},
148
-	{0x016D, 0x80},
149
-	{0x016E, 0x04},
150
-	{0x016F, 0x38},
163
+	{0x012a, 0x18},
164
+	{0x012b, 0x00},
165
+	{0x0164, 0x00},
166
+	{0x0165, 0x00},
167
+	{0x0166, 0x0c},
168
+	{0x0167, 0xcf},
169
+	{0x0168, 0x00},
170
+	{0x0169, 0x00},
171
+	{0x016a, 0x09},
172
+	{0x016b, 0x9f},
173
+	{0x016c, 0x0c},
174
+	{0x016d, 0xd0},
175
+	{0x016e, 0x09},
176
+	{0x016f, 0xa0},
151
177
 	{0x0170, 0x01},
152
178
 	{0x0171, 0x01},
153
179
 	{0x0174, 0x00},
154
180
 	{0x0175, 0x00},
155
-	{0x018C, 0x0A},
156
-	{0x018D, 0x0A},
157
181
 	{0x0301, 0x05},
158
182
 	{0x0303, 0x01},
159
183
 	{0x0304, 0x03},
160
184
 	{0x0305, 0x03},
161
185
 	{0x0306, 0x00},
162
186
 	{0x0307, 0x39},
163
-	{0x0309, 0x0A},
164
-	{0x030B, 0x01},
165
-	{0x030C, 0x00},
166
-	{0x030D, 0x72},
167
-	{0x455E, 0x00},
168
-	{0x471E, 0x4B},
169
-	{0x4767, 0x0F},
187
+	{0x030b, 0x01},
188
+	{0x030c, 0x00},
189
+	{0x030d, 0x72},
190
+	{0x0624, 0x0c},
191
+	{0x0625, 0xd0},
192
+	{0x0626, 0x09},
193
+	{0x0627, 0xa0},
194
+	{0x455e, 0x00},
195
+	{0x471e, 0x4b},
196
+	{0x4767, 0x0f},
170
197
 	{0x4750, 0x14},
171
198
 	{0x4540, 0x00},
172
-	{0x47B4, 0x14},
173
-	{IMX219_TABLE_END, 0x00}
199
+	{0x47b4, 0x14},
200
+	{0x4713, 0x30},
201
+	{0x478b, 0x10},
202
+	{0x478f, 0x10},
203
+	{0x4793, 0x10},
204
+	{0x4797, 0x0e},
205
+	{0x479b, 0x0e},
206
+	{0x0162, 0x0d},
207
+	{0x0163, 0x78},
174
208
 };
175
209
 
176
-static const struct imx219_reg start[] = {
177
-	{0x0100, 0x01},		/* mode select streaming on */
178
-	{IMX219_TABLE_END, 0x00}
210
+static const struct imx219_reg mode_1920_1080_regs[] = {
211
+	{0x0100, 0x00},
212
+	{0x30eb, 0x05},
213
+	{0x30eb, 0x0c},
214
+	{0x300a, 0xff},
215
+	{0x300b, 0xff},
216
+	{0x30eb, 0x05},
217
+	{0x30eb, 0x09},
218
+	{0x0114, 0x01},
219
+	{0x0128, 0x00},
220
+	{0x012a, 0x18},
221
+	{0x012b, 0x00},
222
+	{0x0162, 0x0d},
223
+	{0x0163, 0x78},
224
+	{0x0164, 0x02},
225
+	{0x0165, 0xa8},
226
+	{0x0166, 0x0a},
227
+	{0x0167, 0x27},
228
+	{0x0168, 0x02},
229
+	{0x0169, 0xb4},
230
+	{0x016a, 0x06},
231
+	{0x016b, 0xeb},
232
+	{0x016c, 0x07},
233
+	{0x016d, 0x80},
234
+	{0x016e, 0x04},
235
+	{0x016f, 0x38},
236
+	{0x0170, 0x01},
237
+	{0x0171, 0x01},
238
+	{0x0174, 0x00},
239
+	{0x0175, 0x00},
240
+	{0x0301, 0x05},
241
+	{0x0303, 0x01},
242
+	{0x0304, 0x03},
243
+	{0x0305, 0x03},
244
+	{0x0306, 0x00},
245
+	{0x0307, 0x39},
246
+	{0x030b, 0x01},
247
+	{0x030c, 0x00},
248
+	{0x030d, 0x72},
249
+	{0x0624, 0x07},
250
+	{0x0625, 0x80},
251
+	{0x0626, 0x04},
252
+	{0x0627, 0x38},
253
+	{0x455e, 0x00},
254
+	{0x471e, 0x4b},
255
+	{0x4767, 0x0f},
256
+	{0x4750, 0x14},
257
+	{0x4540, 0x00},
258
+	{0x47b4, 0x14},
259
+	{0x4713, 0x30},
260
+	{0x478b, 0x10},
261
+	{0x478f, 0x10},
262
+	{0x4793, 0x10},
263
+	{0x4797, 0x0e},
264
+	{0x479b, 0x0e},
265
+	{0x0162, 0x0d},
266
+	{0x0163, 0x78},
179
267
 };
180
268
 
181
-static const struct imx219_reg stop[] = {
182
-	{0x0100, 0x00},		/* mode select streaming off */
183
-	{IMX219_TABLE_END, 0x00}
269
+static const struct imx219_reg mode_1640_1232_regs[] = {
270
+	{0x0100, 0x00},
271
+	{0x30eb, 0x0c},
272
+	{0x30eb, 0x05},
273
+	{0x300a, 0xff},
274
+	{0x300b, 0xff},
275
+	{0x30eb, 0x05},
276
+	{0x30eb, 0x09},
277
+	{0x0114, 0x01},
278
+	{0x0128, 0x00},
279
+	{0x012a, 0x18},
280
+	{0x012b, 0x00},
281
+	{0x0164, 0x00},
282
+	{0x0165, 0x00},
283
+	{0x0166, 0x0c},
284
+	{0x0167, 0xcf},
285
+	{0x0168, 0x00},
286
+	{0x0169, 0x00},
287
+	{0x016a, 0x09},
288
+	{0x016b, 0x9f},
289
+	{0x016c, 0x06},
290
+	{0x016d, 0x68},
291
+	{0x016e, 0x04},
292
+	{0x016f, 0xd0},
293
+	{0x0170, 0x01},
294
+	{0x0171, 0x01},
295
+	{0x0174, 0x01},
296
+	{0x0175, 0x01},
297
+	{0x0301, 0x05},
298
+	{0x0303, 0x01},
299
+	{0x0304, 0x03},
300
+	{0x0305, 0x03},
301
+	{0x0306, 0x00},
302
+	{0x0307, 0x39},
303
+	{0x030b, 0x01},
304
+	{0x030c, 0x00},
305
+	{0x030d, 0x72},
306
+	{0x0624, 0x06},
307
+	{0x0625, 0x68},
308
+	{0x0626, 0x04},
309
+	{0x0627, 0xd0},
310
+	{0x455e, 0x00},
311
+	{0x471e, 0x4b},
312
+	{0x4767, 0x0f},
313
+	{0x4750, 0x14},
314
+	{0x4540, 0x00},
315
+	{0x47b4, 0x14},
316
+	{0x4713, 0x30},
317
+	{0x478b, 0x10},
318
+	{0x478f, 0x10},
319
+	{0x4793, 0x10},
320
+	{0x4797, 0x0e},
321
+	{0x479b, 0x0e},
322
+	{0x0162, 0x0d},
323
+	{0x0163, 0x78},
184
324
 };
185
325
 
186
-enum {
187
-	TEST_PATTERN_DISABLED,
188
-	TEST_PATTERN_SOLID_BLACK,
189
-	TEST_PATTERN_SOLID_WHITE,
190
-	TEST_PATTERN_SOLID_RED,
191
-	TEST_PATTERN_SOLID_GREEN,
192
-	TEST_PATTERN_SOLID_BLUE,
193
-	TEST_PATTERN_COLOR_BAR,
194
-	TEST_PATTERN_FADE_TO_GREY_COLOR_BAR,
195
-	TEST_PATTERN_PN9,
196
-	TEST_PATTERN_16_SPLIT_COLOR_BAR,
197
-	TEST_PATTERN_16_SPLIT_INVERTED_COLOR_BAR,
198
-	TEST_PATTERN_COLUMN_COUNTER,
199
-	TEST_PATTERN_INVERTED_COLUMN_COUNTER,
200
-	TEST_PATTERN_PN31,
201
-	TEST_PATTERN_MAX
326
+static const struct imx219_reg mode_640_480_regs[] = {
327
+	{0x0100, 0x00},
328
+	{0x30eb, 0x05},
329
+	{0x30eb, 0x0c},
330
+	{0x300a, 0xff},
331
+	{0x300b, 0xff},
332
+	{0x30eb, 0x05},
333
+	{0x30eb, 0x09},
334
+	{0x0114, 0x01},
335
+	{0x0128, 0x00},
336
+	{0x012a, 0x18},
337
+	{0x012b, 0x00},
338
+	{0x0162, 0x0d},
339
+	{0x0163, 0x78},
340
+	{0x0164, 0x03},
341
+	{0x0165, 0xe8},
342
+	{0x0166, 0x08},
343
+	{0x0167, 0xe7},
344
+	{0x0168, 0x02},
345
+	{0x0169, 0xf0},
346
+	{0x016a, 0x06},
347
+	{0x016b, 0xaf},
348
+	{0x016c, 0x02},
349
+	{0x016d, 0x80},
350
+	{0x016e, 0x01},
351
+	{0x016f, 0xe0},
352
+	{0x0170, 0x01},
353
+	{0x0171, 0x01},
354
+	{0x0174, 0x03},
355
+	{0x0175, 0x03},
356
+	{0x0301, 0x05},
357
+	{0x0303, 0x01},
358
+	{0x0304, 0x03},
359
+	{0x0305, 0x03},
360
+	{0x0306, 0x00},
361
+	{0x0307, 0x39},
362
+	{0x030b, 0x01},
363
+	{0x030c, 0x00},
364
+	{0x030d, 0x72},
365
+	{0x0624, 0x06},
366
+	{0x0625, 0x68},
367
+	{0x0626, 0x04},
368
+	{0x0627, 0xd0},
369
+	{0x455e, 0x00},
370
+	{0x471e, 0x4b},
371
+	{0x4767, 0x0f},
372
+	{0x4750, 0x14},
373
+	{0x4540, 0x00},
374
+	{0x47b4, 0x14},
375
+	{0x4713, 0x30},
376
+	{0x478b, 0x10},
377
+	{0x478f, 0x10},
378
+	{0x4793, 0x10},
379
+	{0x4797, 0x0e},
380
+	{0x479b, 0x0e},
202
381
 };
203
382
 
204
-static const char *const tp_qmenu[] = {
383
+static const struct imx219_reg raw8_framefmt_regs[] = {
384
+	{0x018c, 0x08},
385
+	{0x018d, 0x08},
386
+	{0x0309, 0x08},
387
+};
388
+
389
+static const struct imx219_reg raw10_framefmt_regs[] = {
390
+	{0x018c, 0x0a},
391
+	{0x018d, 0x0a},
392
+	{0x0309, 0x0a},
393
+};
394
+
395
+static const char * const imx219_test_pattern_menu[] = {
205
396
 	"Disabled",
206
-	"Solid Black",
207
-	"Solid White",
208
-	"Solid Red",
209
-	"Solid Green",
210
-	"Solid Blue",
211
-	"Color Bar",
212
-	"Fade to Grey Color Bar",
213
-	"PN9",
214
-	"16 Split Color Bar",
215
-	"16 Split Inverted Color Bar",
216
-	"Column Counter",
217
-	"Inverted Column Counter",
218
-	"PN31",
397
+	"Color Bars",
398
+	"Solid Color",
399
+	"Grey Color Bars",
400
+	"PN9"
219
401
 };
220
402
 
221
-#define SIZEOF_I2C_TRANSBUF 32
222
-
223
-struct imx219 {
224
-	struct v4l2_subdev subdev;
225
-	struct media_pad pad;
226
-	struct v4l2_ctrl_handler ctrl_handler;
227
-	struct clk *clk;
228
-	struct v4l2_rect crop_rect;
229
-	int hflip;
230
-	int vflip;
231
-	u8 analogue_gain;
232
-	u16 digital_gain;	/* bits 11:0 */
233
-	u16 exposure_time;
234
-	u16 test_pattern;
235
-	u16 test_pattern_solid_color_r;
236
-	u16 test_pattern_solid_color_gr;
237
-	u16 test_pattern_solid_color_b;
238
-	u16 test_pattern_solid_color_gb;
239
-	struct v4l2_ctrl *hblank;
240
-	struct v4l2_ctrl *vblank;
241
-	struct v4l2_ctrl *pixel_rate;
242
-	const struct imx219_mode *cur_mode;
243
-	u32 cfg_num;
244
-	u16 cur_vts;
245
-	u32 module_index;
246
-	const char *module_facing;
247
-	const char *module_name;
248
-	const char *len_name;
403
+static const int imx219_test_pattern_val[] = {
404
+	IMX219_TEST_PATTERN_DISABLE,
405
+	IMX219_TEST_PATTERN_COLOR_BARS,
406
+	IMX219_TEST_PATTERN_SOLID_COLOR,
407
+	IMX219_TEST_PATTERN_GREY_COLOR,
408
+	IMX219_TEST_PATTERN_PN9,
249
409
 };
250
410
 
411
+/* regulator supplies */
412
+static const char * const imx219_supply_name[] = {
413
+	/* Supplies can be enabled in any order */
414
+	"VANA",  /* Analog (2.8V) supply */
415
+	"VDIG",  /* Digital Core (1.8V) supply */
416
+	"VDDL",  /* IF (1.2V) supply */
417
+};
418
+
419
+#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
420
+
421
+/*
422
+ * The supported formats.
423
+ * This table MUST contain 4 entries per format, to cover the various flip
424
+ * combinations in the order
425
+ * - no flip
426
+ * - h flip
427
+ * - v flip
428
+ * - h&v flips
429
+ */
430
+static const u32 codes[] = {
431
+	MEDIA_BUS_FMT_SRGGB10_1X10,
432
+	MEDIA_BUS_FMT_SGRBG10_1X10,
433
+	MEDIA_BUS_FMT_SGBRG10_1X10,
434
+	MEDIA_BUS_FMT_SBGGR10_1X10,
435
+
436
+	MEDIA_BUS_FMT_SRGGB8_1X8,
437
+	MEDIA_BUS_FMT_SGRBG8_1X8,
438
+	MEDIA_BUS_FMT_SGBRG8_1X8,
439
+	MEDIA_BUS_FMT_SBGGR8_1X8,
440
+};
441
+
442
+/*
443
+ * Initialisation delay between XCLR low->high and the moment when the sensor
444
+ * can start capture (i.e. can leave software stanby) must be not less than:
445
+ *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
446
+ * where
447
+ *   t4 is fixed, and is max 200uS,
448
+ *   t5 is fixed, and is 6000uS,
449
+ *   t6 depends on the sensor external clock, and is max 32000 clock periods.
450
+ * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
451
+ * So for any acceptable external clock t6 is always within the range of
452
+ * 1185 to 5333 uS, and is always less than t5.
453
+ * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
454
+ * initialize the sensor over I2C, and then exit the software standby.
455
+ *
456
+ * This start-up time can be optimized a bit more, if we start the writes
457
+ * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
458
+ * initialization over I2C may complete before (t4+t5) expires, and we must
459
+ * ensure that capture is not started before (t4+t5).
460
+ *
461
+ * This delay doesn't account for the power supply startup time. If needed,
462
+ * this should be taken care of via the regulator framework. E.g. in the
463
+ * case of DT for regulator-fixed one should define the startup-delay-us
464
+ * property.
465
+ */
466
+#define IMX219_XCLR_MIN_DELAY_US	6200
467
+#define IMX219_XCLR_DELAY_RANGE_US	1000
468
+
469
+/* Mode configs */
251
470
 static const struct imx219_mode supported_modes[] = {
252
471
 	{
253
-		.width = 1920,
254
-		.height = 1080,
255
-		.max_fps = {
256
-			.numerator = 10000,
257
-			.denominator = 300000,
258
-		},
259
-		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
260
-		.vts_def = 0x06E6,
261
-		.reg_list = imx219_init_tab_1920_1080_30fps,
262
-	},
263
-	{
472
+		/* 8MPix 15fps mode */
264
473
 		.width = 3280,
265
474
 		.height = 2464,
266
-		.max_fps = {
267
-			.numerator = 10000,
268
-			.denominator = 210000,
475
+		.crop = {
476
+			.left = IMX219_PIXEL_ARRAY_LEFT,
477
+			.top = IMX219_PIXEL_ARRAY_TOP,
478
+			.width = 3280,
479
+			.height = 2464
269
480
 		},
270
-		.hts_def = 0x0d78 - IMX219_EXP_LINES_MARGIN,
271
-		.vts_def = 0x09c4,
272
-		.reg_list = imx219_init_tab_3280_2464_21fps,
481
+		.vts_def = IMX219_VTS_15FPS,
482
+		.reg_list = {
483
+			.num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
484
+			.regs = mode_3280x2464_regs,
485
+		},
486
+	},
487
+	{
488
+		/* 1080P 30fps cropped */
489
+		.width = 1920,
490
+		.height = 1080,
491
+		.crop = {
492
+			.left = 688,
493
+			.top = 700,
494
+			.width = 1920,
495
+			.height = 1080
496
+		},
497
+		.vts_def = IMX219_VTS_30FPS_1080P,
498
+		.reg_list = {
499
+			.num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
500
+			.regs = mode_1920_1080_regs,
501
+		},
502
+	},
503
+	{
504
+		/* 2x2 binned 30fps mode */
505
+		.width = 1640,
506
+		.height = 1232,
507
+		.crop = {
508
+			.left = IMX219_PIXEL_ARRAY_LEFT,
509
+			.top = IMX219_PIXEL_ARRAY_TOP,
510
+			.width = 3280,
511
+			.height = 2464
512
+		},
513
+		.vts_def = IMX219_VTS_30FPS_BINNED,
514
+		.reg_list = {
515
+			.num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
516
+			.regs = mode_1640_1232_regs,
517
+		},
518
+	},
519
+	{
520
+		/* 640x480 30fps mode */
521
+		.width = 640,
522
+		.height = 480,
523
+		.crop = {
524
+			.left = 1008,
525
+			.top = 760,
526
+			.width = 1280,
527
+			.height = 960
528
+		},
529
+		.vts_def = IMX219_VTS_30FPS_640x480,
530
+		.reg_list = {
531
+			.num_of_regs = ARRAY_SIZE(mode_640_480_regs),
532
+			.regs = mode_640_480_regs,
533
+		},
273
534
 	},
274
535
 };
275
536
 
276
-static struct imx219 *to_imx219(const struct i2c_client *client)
537
+struct imx219 {
538
+	struct v4l2_subdev sd;
539
+	struct media_pad pad;
540
+
541
+	struct v4l2_mbus_framefmt fmt;
542
+
543
+	struct clk *xclk; /* system clock to IMX219 */
544
+	u32 xclk_freq;
545
+
546
+	struct gpio_desc *reset_gpio;
547
+	struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
548
+
549
+	struct v4l2_ctrl_handler ctrl_handler;
550
+	/* V4L2 Controls */
551
+	struct v4l2_ctrl *pixel_rate;
552
+	struct v4l2_ctrl *exposure;
553
+	struct v4l2_ctrl *vflip;
554
+	struct v4l2_ctrl *hflip;
555
+	struct v4l2_ctrl *vblank;
556
+	struct v4l2_ctrl *hblank;
557
+
558
+	/* Current mode */
559
+	const struct imx219_mode *mode;
560
+
561
+	/*
562
+	 * Mutex for serialized access:
563
+	 * Protect sensor module set pad format and start/stop streaming safely.
564
+	 */
565
+	struct mutex mutex;
566
+
567
+	/* Streaming on/off */
568
+	bool streaming;
569
+};
570
+
571
+static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
277
572
 {
278
-	return container_of(i2c_get_clientdata(client), struct imx219, subdev);
573
+	return container_of(_sd, struct imx219, sd);
279
574
 }
280
575
 
281
-static int reg_write(struct i2c_client *client, const u16 addr, const u8 data)
576
+/* Read registers up to 2 at a time */
577
+static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
282
578
 {
283
-	struct i2c_adapter *adap = client->adapter;
284
-	struct i2c_msg msg;
285
-	u8 tx[3];
579
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
580
+	struct i2c_msg msgs[2];
581
+	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
582
+	u8 data_buf[4] = { 0, };
286
583
 	int ret;
287
584
 
288
-	msg.addr = client->addr;
289
-	msg.buf = tx;
290
-	msg.len = 3;
291
-	msg.flags = 0;
292
-	tx[0] = addr >> 8;
293
-	tx[1] = addr & 0xff;
294
-	tx[2] = data;
295
-	ret = i2c_transfer(adap, &msg, 1);
296
-	mdelay(2);
585
+	if (len > 4)
586
+		return -EINVAL;
297
587
 
298
-	return ret == 1 ? 0 : -EIO;
299
-}
588
+	/* Write register address */
589
+	msgs[0].addr = client->addr;
590
+	msgs[0].flags = 0;
591
+	msgs[0].len = ARRAY_SIZE(addr_buf);
592
+	msgs[0].buf = addr_buf;
300
593
 
301
-static int reg_read(struct i2c_client *client, const u16 addr)
302
-{
303
-	u8 buf[2] = {addr >> 8, addr & 0xff};
304
-	int ret;
305
-	struct i2c_msg msgs[] = {
306
-		{
307
-			.addr  = client->addr,
308
-			.flags = 0,
309
-			.len   = 2,
310
-			.buf   = buf,
311
-		}, {
312
-			.addr  = client->addr,
313
-			.flags = I2C_M_RD,
314
-			.len   = 1,
315
-			.buf   = buf,
316
-		},
317
-	};
594
+	/* Read data from register */
595
+	msgs[1].addr = client->addr;
596
+	msgs[1].flags = I2C_M_RD;
597
+	msgs[1].len = len;
598
+	msgs[1].buf = &data_buf[4 - len];
318
599
 
319
600
 	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
320
-	if (ret < 0) {
321
-		dev_warn(&client->dev, "Reading register %x from %x failed\n",
322
-			 addr, client->addr);
323
-		return ret;
324
-	}
601
+	if (ret != ARRAY_SIZE(msgs))
602
+		return -EIO;
325
603
 
326
-	return buf[0];
327
-}
328
-
329
-static int reg_write_table(struct i2c_client *client,
330
-			   const struct imx219_reg table[])
331
-{
332
-	const struct imx219_reg *reg;
333
-	int ret;
334
-
335
-	for (reg = table; reg->addr != IMX219_TABLE_END; reg++) {
336
-		ret = reg_write(client, reg->addr, reg->val);
337
-		if (ret < 0)
338
-			return ret;
339
-	}
604
+	*val = get_unaligned_be32(data_buf);
340
605
 
341
606
 	return 0;
342
607
 }
343
608
 
344
-/* V4L2 subdev video operations */
345
-static int imx219_s_stream(struct v4l2_subdev *sd, int enable)
609
+/* Write registers up to 2 at a time */
610
+static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
346
611
 {
347
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
348
-	struct imx219 *priv = to_imx219(client);
349
-	u8 reg = 0x00;
350
-	int ret;
612
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
613
+	u8 buf[6];
351
614
 
352
-	if (!enable)
353
-		return reg_write_table(client, stop);
354
-
355
-	ret = reg_write_table(client, priv->cur_mode->reg_list);
356
-	if (ret)
357
-		return ret;
358
-
359
-	/* Handle crop */
360
-	ret = reg_write(client, 0x0164, priv->crop_rect.left >> 8);
361
-	ret |= reg_write(client, 0x0165, priv->crop_rect.left & 0xff);
362
-	ret |= reg_write(client, 0x0166, (priv->crop_rect.left + priv->crop_rect.width - 1) >> 8);
363
-	ret |= reg_write(client, 0x0167, (priv->crop_rect.left + priv->crop_rect.width - 1) & 0xff);
364
-	ret |= reg_write(client, 0x0168, priv->crop_rect.top >> 8);
365
-	ret |= reg_write(client, 0x0169, priv->crop_rect.top & 0xff);
366
-	ret |= reg_write(client, 0x016A, (priv->crop_rect.top + priv->crop_rect.height - 1) >> 8);
367
-	ret |= reg_write(client, 0x016B, (priv->crop_rect.top + priv->crop_rect.height - 1) & 0xff);
368
-	ret |= reg_write(client, 0x016C, priv->crop_rect.width >> 8);
369
-	ret |= reg_write(client, 0x016D, priv->crop_rect.width & 0xff);
370
-	ret |= reg_write(client, 0x016E, priv->crop_rect.height >> 8);
371
-	ret |= reg_write(client, 0x016F, priv->crop_rect.height & 0xff);
372
-
373
-	if (ret)
374
-		return ret;
375
-
376
-	/* Handle flip/mirror */
377
-	if (priv->hflip)
378
-		reg |= 0x1;
379
-	if (priv->vflip)
380
-		reg |= 0x2;
381
-
382
-	ret = reg_write(client, 0x0172, reg);
383
-	if (ret)
384
-		return ret;
385
-
386
-	/* Handle test pattern */
387
-	if (priv->test_pattern) {
388
-		ret = reg_write(client, 0x0600, priv->test_pattern >> 8);
389
-		ret |= reg_write(client, 0x0601, priv->test_pattern & 0xff);
390
-		ret |= reg_write(client, 0x0602,
391
-				 priv->test_pattern_solid_color_r >> 8);
392
-		ret |= reg_write(client, 0x0603,
393
-				 priv->test_pattern_solid_color_r & 0xff);
394
-		ret |= reg_write(client, 0x0604,
395
-				 priv->test_pattern_solid_color_gr >> 8);
396
-		ret |= reg_write(client, 0x0605,
397
-				 priv->test_pattern_solid_color_gr & 0xff);
398
-		ret |= reg_write(client, 0x0606,
399
-				 priv->test_pattern_solid_color_b >> 8);
400
-		ret |= reg_write(client, 0x0607,
401
-				 priv->test_pattern_solid_color_b & 0xff);
402
-		ret |= reg_write(client, 0x0608,
403
-				 priv->test_pattern_solid_color_gb >> 8);
404
-		ret |= reg_write(client, 0x0609,
405
-				 priv->test_pattern_solid_color_gb & 0xff);
406
-		ret |= reg_write(client, 0x0620, priv->crop_rect.left >> 8);
407
-		ret |= reg_write(client, 0x0621, priv->crop_rect.left & 0xff);
408
-		ret |= reg_write(client, 0x0622, priv->crop_rect.top >> 8);
409
-		ret |= reg_write(client, 0x0623, priv->crop_rect.top & 0xff);
410
-		ret |= reg_write(client, 0x0624, priv->crop_rect.width >> 8);
411
-		ret |= reg_write(client, 0x0625, priv->crop_rect.width & 0xff);
412
-		ret |= reg_write(client, 0x0626, priv->crop_rect.height >> 8);
413
-		ret |= reg_write(client, 0x0627, priv->crop_rect.height & 0xff);
414
-	} else {
415
-		ret = reg_write(client, 0x0600, 0x00);
416
-		ret |= reg_write(client, 0x0601, 0x00);
417
-	}
418
-
419
-	priv->cur_vts = priv->cur_mode->vts_def - IMX219_EXP_LINES_MARGIN;
420
-	if (ret)
421
-		return ret;
422
-
423
-	return reg_write_table(client, start);
424
-}
425
-
426
-/* V4L2 subdev core operations */
427
-static int imx219_s_power(struct v4l2_subdev *sd, int on)
428
-{
429
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
430
-	struct imx219 *priv = to_imx219(client);
431
-
432
-	if (on)	{
433
-		dev_dbg(&client->dev, "imx219 power on\n");
434
-		clk_prepare_enable(priv->clk);
435
-	} else if (!on) {
436
-		dev_dbg(&client->dev, "imx219 power off\n");
437
-		clk_disable_unprepare(priv->clk);
438
-	}
439
-
440
-	return 0;
441
-}
442
-
443
-/* V4L2 ctrl operations */
444
-static int imx219_s_ctrl_test_pattern(struct v4l2_ctrl *ctrl)
445
-{
446
-	struct imx219 *priv =
447
-	    container_of(ctrl->handler, struct imx219, ctrl_handler);
448
-
449
-	switch (ctrl->val) {
450
-	case TEST_PATTERN_DISABLED:
451
-		priv->test_pattern = 0x0000;
452
-		break;
453
-	case TEST_PATTERN_SOLID_BLACK:
454
-		priv->test_pattern = 0x0001;
455
-		priv->test_pattern_solid_color_r = 0x0000;
456
-		priv->test_pattern_solid_color_gr = 0x0000;
457
-		priv->test_pattern_solid_color_b = 0x0000;
458
-		priv->test_pattern_solid_color_gb = 0x0000;
459
-		break;
460
-	case TEST_PATTERN_SOLID_WHITE:
461
-		priv->test_pattern = 0x0001;
462
-		priv->test_pattern_solid_color_r = 0x0fff;
463
-		priv->test_pattern_solid_color_gr = 0x0fff;
464
-		priv->test_pattern_solid_color_b = 0x0fff;
465
-		priv->test_pattern_solid_color_gb = 0x0fff;
466
-		break;
467
-	case TEST_PATTERN_SOLID_RED:
468
-		priv->test_pattern = 0x0001;
469
-		priv->test_pattern_solid_color_r = 0x0fff;
470
-		priv->test_pattern_solid_color_gr = 0x0000;
471
-		priv->test_pattern_solid_color_b = 0x0000;
472
-		priv->test_pattern_solid_color_gb = 0x0000;
473
-		break;
474
-	case TEST_PATTERN_SOLID_GREEN:
475
-		priv->test_pattern = 0x0001;
476
-		priv->test_pattern_solid_color_r = 0x0000;
477
-		priv->test_pattern_solid_color_gr = 0x0fff;
478
-		priv->test_pattern_solid_color_b = 0x0000;
479
-		priv->test_pattern_solid_color_gb = 0x0fff;
480
-		break;
481
-	case TEST_PATTERN_SOLID_BLUE:
482
-		priv->test_pattern = 0x0001;
483
-		priv->test_pattern_solid_color_r = 0x0000;
484
-		priv->test_pattern_solid_color_gr = 0x0000;
485
-		priv->test_pattern_solid_color_b = 0x0fff;
486
-		priv->test_pattern_solid_color_gb = 0x0000;
487
-		break;
488
-	case TEST_PATTERN_COLOR_BAR:
489
-		priv->test_pattern = 0x0002;
490
-		break;
491
-	case TEST_PATTERN_FADE_TO_GREY_COLOR_BAR:
492
-		priv->test_pattern = 0x0003;
493
-		break;
494
-	case TEST_PATTERN_PN9:
495
-		priv->test_pattern = 0x0004;
496
-		break;
497
-	case TEST_PATTERN_16_SPLIT_COLOR_BAR:
498
-		priv->test_pattern = 0x0005;
499
-		break;
500
-	case TEST_PATTERN_16_SPLIT_INVERTED_COLOR_BAR:
501
-		priv->test_pattern = 0x0006;
502
-		break;
503
-	case TEST_PATTERN_COLUMN_COUNTER:
504
-		priv->test_pattern = 0x0007;
505
-		break;
506
-	case TEST_PATTERN_INVERTED_COLUMN_COUNTER:
507
-		priv->test_pattern = 0x0008;
508
-		break;
509
-	case TEST_PATTERN_PN31:
510
-		priv->test_pattern = 0x0009;
511
-		break;
512
-	default:
615
+	if (len > 4)
513
616
 		return -EINVAL;
617
+
618
+	put_unaligned_be16(reg, buf);
619
+	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
620
+	if (i2c_master_send(client, buf, len + 2) != len + 2)
621
+		return -EIO;
622
+
623
+	return 0;
624
+}
625
+
626
+/* Write a list of registers */
627
+static int imx219_write_regs(struct imx219 *imx219,
628
+			     const struct imx219_reg *regs, u32 len)
629
+{
630
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
631
+	unsigned int i;
632
+	int ret;
633
+
634
+	for (i = 0; i < len; i++) {
635
+		ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
636
+		if (ret) {
637
+			dev_err_ratelimited(&client->dev,
638
+					    "Failed to write reg 0x%4.4x. error = %d\n",
639
+					    regs[i].address, ret);
640
+
641
+			return ret;
642
+		}
514
643
 	}
515
644
 
516
645
 	return 0;
517
646
 }
518
647
 
519
-static int imx219_g_frame_interval(struct v4l2_subdev *sd,
520
-				   struct v4l2_subdev_frame_interval *fi)
648
+/* Get bayer order based on flip setting. */
649
+static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
521
650
 {
522
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
523
-	struct imx219 *priv = to_imx219(client);
524
-	const struct imx219_mode *mode = priv->cur_mode;
651
+	unsigned int i;
525
652
 
526
-	fi->interval = mode->max_fps;
653
+	lockdep_assert_held(&imx219->mutex);
654
+
655
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
656
+		if (codes[i] == code)
657
+			break;
658
+
659
+	if (i >= ARRAY_SIZE(codes))
660
+		i = 0;
661
+
662
+	i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
663
+	    (imx219->hflip->val ? 1 : 0);
664
+
665
+	return codes[i];
666
+}
667
+
668
+static void imx219_set_default_format(struct imx219 *imx219)
669
+{
670
+	struct v4l2_mbus_framefmt *fmt;
671
+
672
+	fmt = &imx219->fmt;
673
+	fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
674
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
675
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
676
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
677
+							  fmt->colorspace,
678
+							  fmt->ycbcr_enc);
679
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
680
+	fmt->width = supported_modes[0].width;
681
+	fmt->height = supported_modes[0].height;
682
+	fmt->field = V4L2_FIELD_NONE;
683
+}
684
+
685
+static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
686
+{
687
+	struct imx219 *imx219 = to_imx219(sd);
688
+	struct v4l2_mbus_framefmt *try_fmt =
689
+		v4l2_subdev_get_try_format(sd, fh->pad, 0);
690
+	struct v4l2_rect *try_crop;
691
+
692
+	mutex_lock(&imx219->mutex);
693
+
694
+	/* Initialize try_fmt */
695
+	try_fmt->width = supported_modes[0].width;
696
+	try_fmt->height = supported_modes[0].height;
697
+	try_fmt->code = imx219_get_format_code(imx219,
698
+					       MEDIA_BUS_FMT_SRGGB10_1X10);
699
+	try_fmt->field = V4L2_FIELD_NONE;
700
+
701
+	/* Initialize try_crop rectangle. */
702
+	try_crop = v4l2_subdev_get_try_crop(sd, fh->pad, 0);
703
+	try_crop->top = IMX219_PIXEL_ARRAY_TOP;
704
+	try_crop->left = IMX219_PIXEL_ARRAY_LEFT;
705
+	try_crop->width = IMX219_PIXEL_ARRAY_WIDTH;
706
+	try_crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
707
+
708
+	mutex_unlock(&imx219->mutex);
527
709
 
528
710
 	return 0;
529
711
 }
530
712
 
531
-static int imx219_s_ctrl(struct v4l2_ctrl *ctrl)
713
+static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
532
714
 {
533
-	struct imx219 *priv =
534
-	    container_of(ctrl->handler, struct imx219, ctrl_handler);
535
-	struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
536
-	u8 reg;
715
+	struct imx219 *imx219 =
716
+		container_of(ctrl->handler, struct imx219, ctrl_handler);
717
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
537
718
 	int ret;
538
-	u16 gain = 256;
539
-	u16 a_gain = 256;
540
-	u16 d_gain = 1;
719
+
720
+	if (ctrl->id == V4L2_CID_VBLANK) {
721
+		int exposure_max, exposure_def;
722
+
723
+		/* Update max exposure while meeting expected vblanking */
724
+		exposure_max = imx219->mode->height + ctrl->val - 4;
725
+		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
726
+			exposure_max : IMX219_EXPOSURE_DEFAULT;
727
+		__v4l2_ctrl_modify_range(imx219->exposure,
728
+					 imx219->exposure->minimum,
729
+					 exposure_max, imx219->exposure->step,
730
+					 exposure_def);
731
+	}
732
+
733
+	/*
734
+	 * Applying V4L2 control value only happens
735
+	 * when power is up for streaming
736
+	 */
737
+	if (pm_runtime_get_if_in_use(&client->dev) == 0)
738
+		return 0;
541
739
 
542
740
 	switch (ctrl->id) {
543
-	case V4L2_CID_HFLIP:
544
-		priv->hflip = ctrl->val;
545
-		break;
546
-
547
-	case V4L2_CID_VFLIP:
548
-		priv->vflip = ctrl->val;
549
-		break;
550
-
551
741
 	case V4L2_CID_ANALOGUE_GAIN:
552
-	case V4L2_CID_GAIN:
553
-		/*
554
-		 * hal transfer (gain * 256)  to kernel
555
-		 * than divide into analog gain & digital gain in kernel
556
-		 */
557
-
558
-		gain = ctrl->val;
559
-		if (gain < 256)
560
-			gain = 256;
561
-		if (gain > 43663)
562
-			gain = 43663;
563
-		if (gain >= 256 && gain <= 2728) {
564
-			a_gain = gain;
565
-			d_gain = 1 * 256;
566
-		} else {
567
-			a_gain = 2728;
568
-			d_gain = (gain * 256) / a_gain;
569
-		}
570
-
571
-		/*
572
-		 * Analog gain, reg range[0, 232], gain value[1, 10.66]
573
-		 * reg = 256 - 256 / again
574
-		 * a_gain here is 256 multify
575
-		 * so the reg = 256 - 256 * 256 / a_gain
576
-		 */
577
-		priv->analogue_gain = (256 - (256 * 256) / a_gain);
578
-		if (a_gain < 256)
579
-			priv->analogue_gain = 0;
580
-		if (priv->analogue_gain > 232)
581
-			priv->analogue_gain = 232;
582
-
583
-		/*
584
-		 * Digital gain, reg range[256, 4095], gain rage[1, 16]
585
-		 * reg = dgain * 256
586
-		 */
587
-		priv->digital_gain = d_gain;
588
-		if (priv->digital_gain < 256)
589
-			priv->digital_gain = 256;
590
-		if (priv->digital_gain > 4095)
591
-			priv->digital_gain = 4095;
592
-
593
-		/*
594
-		 * for bank A and bank B switch
595
-		 * exposure time , gain, vts must change at the same time
596
-		 * so the exposure & gain can reflect at the same frame
597
-		 */
598
-
599
-		ret = reg_write(client, 0x0157, priv->analogue_gain);
600
-		ret |= reg_write(client, 0x0158, priv->digital_gain >> 8);
601
-		ret |= reg_write(client, 0x0159, priv->digital_gain & 0xff);
602
-
603
-		return ret;
604
-
742
+		ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
743
+				       IMX219_REG_VALUE_08BIT, ctrl->val);
744
+		break;
605
745
 	case V4L2_CID_EXPOSURE:
606
-		priv->exposure_time = ctrl->val;
607
-
608
-		ret = reg_write(client, 0x015a, priv->exposure_time >> 8);
609
-		ret |= reg_write(client, 0x015b, priv->exposure_time & 0xff);
610
-		return ret;
611
-
746
+		ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
747
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
748
+		break;
749
+	case V4L2_CID_DIGITAL_GAIN:
750
+		ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
751
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
752
+		break;
612
753
 	case V4L2_CID_TEST_PATTERN:
613
-		return imx219_s_ctrl_test_pattern(ctrl);
614
-
754
+		ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
755
+				       IMX219_REG_VALUE_16BIT,
756
+				       imx219_test_pattern_val[ctrl->val]);
757
+		break;
758
+	case V4L2_CID_HFLIP:
759
+	case V4L2_CID_VFLIP:
760
+		ret = imx219_write_reg(imx219, IMX219_REG_ORIENTATION, 1,
761
+				       imx219->hflip->val |
762
+				       imx219->vflip->val << 1);
763
+		break;
615
764
 	case V4L2_CID_VBLANK:
616
-		if (ctrl->val < priv->cur_mode->vts_def)
617
-			ctrl->val = priv->cur_mode->vts_def;
618
-		if ((ctrl->val - IMX219_EXP_LINES_MARGIN) != priv->cur_vts)
619
-			priv->cur_vts = ctrl->val - IMX219_EXP_LINES_MARGIN;
620
-		ret = reg_write(client, 0x0160, ((priv->cur_vts >> 8) & 0xff));
621
-		ret |= reg_write(client, 0x0161, (priv->cur_vts & 0xff));
622
-		return ret;
623
-
765
+		ret = imx219_write_reg(imx219, IMX219_REG_VTS,
766
+				       IMX219_REG_VALUE_16BIT,
767
+				       imx219->mode->height + ctrl->val);
768
+		break;
769
+	case V4L2_CID_TEST_PATTERN_RED:
770
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
771
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
772
+		break;
773
+	case V4L2_CID_TEST_PATTERN_GREENR:
774
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
775
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
776
+		break;
777
+	case V4L2_CID_TEST_PATTERN_BLUE:
778
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
779
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
780
+		break;
781
+	case V4L2_CID_TEST_PATTERN_GREENB:
782
+		ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
783
+				       IMX219_REG_VALUE_16BIT, ctrl->val);
784
+		break;
624
785
 	default:
625
-		return -EINVAL;
786
+		dev_info(&client->dev,
787
+			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
788
+			 ctrl->id, ctrl->val);
789
+		ret = -EINVAL;
790
+		break;
626
791
 	}
627
-	/* If enabled, apply settings immediately */
628
-	reg = reg_read(client, 0x0100);
629
-	if ((reg & 0x1f) == 0x01)
630
-		imx219_s_stream(&priv->subdev, 1);
631
792
 
632
-	return 0;
793
+	pm_runtime_put(&client->dev);
794
+
795
+	return ret;
633
796
 }
797
+
798
+static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
799
+	.s_ctrl = imx219_set_ctrl,
800
+};
634
801
 
635
802
 static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
636
803
 				 struct v4l2_subdev_pad_config *cfg,
637
804
 				 struct v4l2_subdev_mbus_code_enum *code)
638
805
 {
639
-	if (code->index != 0)
806
+	struct imx219 *imx219 = to_imx219(sd);
807
+
808
+	if (code->index >= (ARRAY_SIZE(codes) / 4))
640
809
 		return -EINVAL;
641
-	code->code = MEDIA_BUS_FMT_SRGGB10_1X10;
810
+
811
+	code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
642
812
 
643
813
 	return 0;
644
814
 }
645
815
 
646
-static int imx219_get_reso_dist(const struct imx219_mode *mode,
647
-				struct v4l2_mbus_framefmt *framefmt)
816
+static int imx219_enum_frame_size(struct v4l2_subdev *sd,
817
+				  struct v4l2_subdev_pad_config *cfg,
818
+				  struct v4l2_subdev_frame_size_enum *fse)
648
819
 {
649
-	return abs(mode->width - framefmt->width) +
650
-	       abs(mode->height - framefmt->height);
820
+	struct imx219 *imx219 = to_imx219(sd);
821
+
822
+	if (fse->index >= ARRAY_SIZE(supported_modes))
823
+		return -EINVAL;
824
+
825
+	if (fse->code != imx219_get_format_code(imx219, fse->code))
826
+		return -EINVAL;
827
+
828
+	fse->min_width = supported_modes[fse->index].width;
829
+	fse->max_width = fse->min_width;
830
+	fse->min_height = supported_modes[fse->index].height;
831
+	fse->max_height = fse->min_height;
832
+
833
+	return 0;
651
834
 }
652
835
 
653
-static const struct imx219_mode *imx219_find_best_fit(
654
-					struct v4l2_subdev_format *fmt)
836
+static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
655
837
 {
656
-	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
657
-	int dist;
658
-	int cur_best_fit = 0;
659
-	int cur_best_fit_dist = -1;
660
-	int i;
838
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
839
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
840
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
841
+							  fmt->colorspace,
842
+							  fmt->ycbcr_enc);
843
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
844
+}
661
845
 
662
-	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
663
-		dist = imx219_get_reso_dist(&supported_modes[i], framefmt);
664
-		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
665
-			cur_best_fit_dist = dist;
666
-			cur_best_fit = i;
667
-		}
846
+static void imx219_update_pad_format(struct imx219 *imx219,
847
+				     const struct imx219_mode *mode,
848
+				     struct v4l2_subdev_format *fmt)
849
+{
850
+	fmt->format.width = mode->width;
851
+	fmt->format.height = mode->height;
852
+	fmt->format.field = V4L2_FIELD_NONE;
853
+	imx219_reset_colorspace(&fmt->format);
854
+}
855
+
856
+static int __imx219_get_pad_format(struct imx219 *imx219,
857
+				   struct v4l2_subdev_pad_config *cfg,
858
+				   struct v4l2_subdev_format *fmt)
859
+{
860
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
861
+		struct v4l2_mbus_framefmt *try_fmt =
862
+			v4l2_subdev_get_try_format(&imx219->sd, cfg, fmt->pad);
863
+		/* update the code which could change due to vflip or hflip: */
864
+		try_fmt->code = imx219_get_format_code(imx219, try_fmt->code);
865
+		fmt->format = *try_fmt;
866
+	} else {
867
+		imx219_update_pad_format(imx219, imx219->mode, fmt);
868
+		fmt->format.code = imx219_get_format_code(imx219,
869
+							  imx219->fmt.code);
668
870
 	}
669
871
 
670
-	return &supported_modes[cur_best_fit];
872
+	return 0;
671
873
 }
672
874
 
673
-static int imx219_set_fmt(struct v4l2_subdev *sd,
674
-			  struct v4l2_subdev_pad_config *cfg,
675
-			  struct v4l2_subdev_format *fmt)
875
+static int imx219_get_pad_format(struct v4l2_subdev *sd,
876
+				 struct v4l2_subdev_pad_config *cfg,
877
+				 struct v4l2_subdev_format *fmt)
676
878
 {
677
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
678
-	struct imx219 *priv = to_imx219(client);
879
+	struct imx219 *imx219 = to_imx219(sd);
880
+	int ret;
881
+
882
+	mutex_lock(&imx219->mutex);
883
+	ret = __imx219_get_pad_format(imx219, cfg, fmt);
884
+	mutex_unlock(&imx219->mutex);
885
+
886
+	return ret;
887
+}
888
+
889
+static int imx219_set_pad_format(struct v4l2_subdev *sd,
890
+				 struct v4l2_subdev_pad_config *cfg,
891
+				 struct v4l2_subdev_format *fmt)
892
+{
893
+	struct imx219 *imx219 = to_imx219(sd);
679
894
 	const struct imx219_mode *mode;
680
-	s64 h_blank, v_blank, pixel_rate;
681
-	u32 fps = 0;
895
+	struct v4l2_mbus_framefmt *framefmt;
896
+	int exposure_max, exposure_def, hblank;
897
+	unsigned int i;
682
898
 
683
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
684
-		return 0;
899
+	mutex_lock(&imx219->mutex);
685
900
 
686
-	mode = imx219_find_best_fit(fmt);
687
-	fmt->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
688
-	fmt->format.width = mode->width;
689
-	fmt->format.height = mode->height;
690
-	fmt->format.field = V4L2_FIELD_NONE;
691
-	priv->cur_mode = mode;
692
-	h_blank = mode->hts_def - mode->width;
693
-	__v4l2_ctrl_modify_range(priv->hblank, h_blank,
694
-					h_blank, 1, h_blank);
695
-	v_blank = mode->vts_def - mode->height;
696
-	__v4l2_ctrl_modify_range(priv->vblank, v_blank,
697
-					v_blank,
698
-					1, v_blank);
699
-	fps = DIV_ROUND_CLOSEST(mode->max_fps.denominator,
700
-		mode->max_fps.numerator);
701
-	pixel_rate = mode->vts_def * mode->hts_def * fps;
702
-	__v4l2_ctrl_modify_range(priv->pixel_rate, pixel_rate,
703
-					pixel_rate, 1, pixel_rate);
901
+	for (i = 0; i < ARRAY_SIZE(codes); i++)
902
+		if (codes[i] == fmt->format.code)
903
+			break;
904
+	if (i >= ARRAY_SIZE(codes))
905
+		i = 0;
704
906
 
705
-	/* reset crop window */
706
-	priv->crop_rect.left = 1640 - (mode->width / 2);
707
-	if (priv->crop_rect.left < 0)
708
-		priv->crop_rect.left = 0;
709
-	priv->crop_rect.top = 1232 - (mode->height / 2);
710
-	if (priv->crop_rect.top < 0)
711
-		priv->crop_rect.top = 0;
712
-	priv->crop_rect.width = mode->width;
713
-	priv->crop_rect.height = mode->height;
907
+	/* Bayer order varies with flips */
908
+	fmt->format.code = imx219_get_format_code(imx219, codes[i]);
714
909
 
715
-	return 0;
716
-}
717
-
718
-static int imx219_get_fmt(struct v4l2_subdev *sd,
719
-			  struct v4l2_subdev_pad_config *cfg,
720
-			  struct v4l2_subdev_format *fmt)
721
-{
722
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
723
-	struct imx219 *priv = to_imx219(client);
724
-	const struct imx219_mode *mode = priv->cur_mode;
725
-
726
-	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
727
-		return 0;
728
-
729
-	fmt->format.width = mode->width;
730
-	fmt->format.height = mode->height;
731
-	fmt->format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
732
-	fmt->format.field = V4L2_FIELD_NONE;
733
-
734
-	return 0;
735
-}
736
-
737
-static void imx219_get_module_inf(struct imx219 *imx219,
738
-				  struct rkmodule_inf *inf)
739
-{
740
-	memset(inf, 0, sizeof(*inf));
741
-	strlcpy(inf->base.sensor, IMX219_NAME, sizeof(inf->base.sensor));
742
-	strlcpy(inf->base.module, imx219->module_name,
743
-		sizeof(inf->base.module));
744
-	strlcpy(inf->base.lens, imx219->len_name, sizeof(inf->base.lens));
745
-}
746
-
747
-static long imx219_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
748
-{
749
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
750
-	struct imx219 *imx219 = to_imx219(client);
751
-	long ret = 0;
752
-
753
-	switch (cmd) {
754
-	case RKMODULE_GET_MODULE_INFO:
755
-		imx219_get_module_inf(imx219, (struct rkmodule_inf *)arg);
756
-		break;
757
-	default:
758
-		ret = -ENOIOCTLCMD;
759
-		break;
910
+	mode = v4l2_find_nearest_size(supported_modes,
911
+				      ARRAY_SIZE(supported_modes),
912
+				      width, height,
913
+				      fmt->format.width, fmt->format.height);
914
+	imx219_update_pad_format(imx219, mode, fmt);
915
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
916
+		framefmt = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
917
+		*framefmt = fmt->format;
918
+	} else if (imx219->mode != mode ||
919
+		   imx219->fmt.code != fmt->format.code) {
920
+		imx219->fmt = fmt->format;
921
+		imx219->mode = mode;
922
+		/* Update limits and set FPS to default */
923
+		__v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
924
+					 IMX219_VTS_MAX - mode->height, 1,
925
+					 mode->vts_def - mode->height);
926
+		__v4l2_ctrl_s_ctrl(imx219->vblank,
927
+				   mode->vts_def - mode->height);
928
+		/* Update max exposure while meeting expected vblanking */
929
+		exposure_max = mode->vts_def - 4;
930
+		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
931
+			exposure_max : IMX219_EXPOSURE_DEFAULT;
932
+		__v4l2_ctrl_modify_range(imx219->exposure,
933
+					 imx219->exposure->minimum,
934
+					 exposure_max, imx219->exposure->step,
935
+					 exposure_def);
936
+		/*
937
+		 * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
938
+		 * depends on mode->width only, and is not changeble in any
939
+		 * way other than changing the mode.
940
+		 */
941
+		hblank = IMX219_PPL_DEFAULT - mode->width;
942
+		__v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1,
943
+					 hblank);
760
944
 	}
761
945
 
762
-	return ret;
946
+	mutex_unlock(&imx219->mutex);
947
+
948
+	return 0;
763
949
 }
764
950
 
765
-#ifdef CONFIG_COMPAT
766
-static long imx219_compat_ioctl32(struct v4l2_subdev *sd,
767
-				  unsigned int cmd, unsigned long arg)
951
+static int imx219_set_framefmt(struct imx219 *imx219)
768
952
 {
769
-	void __user *up = compat_ptr(arg);
770
-	struct rkmodule_inf *inf;
771
-	struct rkmodule_awb_cfg *cfg;
772
-	long ret;
953
+	switch (imx219->fmt.code) {
954
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
955
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
956
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
957
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
958
+		return imx219_write_regs(imx219, raw8_framefmt_regs,
959
+					ARRAY_SIZE(raw8_framefmt_regs));
773
960
 
774
-	switch (cmd) {
775
-	case RKMODULE_GET_MODULE_INFO:
776
-		inf = kzalloc(sizeof(*inf), GFP_KERNEL);
777
-		if (!inf) {
778
-			ret = -ENOMEM;
779
-			return ret;
780
-		}
781
-
782
-		ret = imx219_ioctl(sd, cmd, inf);
783
-		if (!ret)
784
-			ret = copy_to_user(up, inf, sizeof(*inf));
785
-		kfree(inf);
786
-		break;
787
-	case RKMODULE_AWB_CFG:
788
-		cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
789
-		if (!cfg) {
790
-			ret = -ENOMEM;
791
-			return ret;
792
-		}
793
-
794
-		ret = copy_from_user(cfg, up, sizeof(*cfg));
795
-		if (!ret)
796
-			ret = imx219_ioctl(sd, cmd, cfg);
797
-		kfree(cfg);
798
-		break;
799
-	default:
800
-		ret = -ENOIOCTLCMD;
801
-		break;
961
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
962
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
963
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
964
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
965
+		return imx219_write_regs(imx219, raw10_framefmt_regs,
966
+					ARRAY_SIZE(raw10_framefmt_regs));
802
967
 	}
803
968
 
804
-	return ret;
805
-}
806
-#endif
807
-
808
-static int imx219_enum_frame_interval(struct v4l2_subdev *sd,
809
-				       struct v4l2_subdev_pad_config *cfg,
810
-				       struct v4l2_subdev_frame_interval_enum *fie)
811
-{
812
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
813
-	struct imx219 *priv = to_imx219(client);
814
-
815
-	if (fie->index >= priv->cfg_num)
816
-		return -EINVAL;
817
-
818
-	if (fie->code != MEDIA_BUS_FMT_SRGGB10_1X10)
819
-		return -EINVAL;
820
-
821
-	fie->width = supported_modes[fie->index].width;
822
-	fie->height = supported_modes[fie->index].height;
823
-	fie->interval = supported_modes[fie->index].max_fps;
824
-	return 0;
969
+	return -EINVAL;
825
970
 }
826
971
 
827
-static int imx219_g_mbus_config(struct v4l2_subdev *sd,
828
-				struct v4l2_mbus_config *config)
972
+static const struct v4l2_rect *
973
+__imx219_get_pad_crop(struct imx219 *imx219, struct v4l2_subdev_pad_config *cfg,
974
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
829
975
 {
830
-	u32 val = 0;
976
+	switch (which) {
977
+	case V4L2_SUBDEV_FORMAT_TRY:
978
+		return v4l2_subdev_get_try_crop(&imx219->sd, cfg, pad);
979
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
980
+		return &imx219->mode->crop;
981
+	}
831
982
 
832
-	val = 1 << (IMX219_LANES - 1) |
833
-	      V4L2_MBUS_CSI2_CHANNEL_0 |
834
-	      V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
835
-	config->type = V4L2_MBUS_CSI2;
836
-	config->flags = val;
837
-
838
-	return 0;
983
+	return NULL;
839
984
 }
840
985
 
841
-/* Various V4L2 operations tables */
842
-static struct v4l2_subdev_video_ops imx219_subdev_video_ops = {
843
-	.s_stream = imx219_s_stream,
844
-	.g_frame_interval = imx219_g_frame_interval,
845
-	.g_mbus_config = imx219_g_mbus_config,
846
-};
847
-
848
-static struct v4l2_subdev_core_ops imx219_subdev_core_ops = {
849
-	.s_power = imx219_s_power,
850
-	.ioctl = imx219_ioctl,
851
-#ifdef CONFIG_COMPAT
852
-	.compat_ioctl32 = imx219_compat_ioctl32,
853
-#endif
854
-};
855
-
856
-static const struct v4l2_subdev_pad_ops imx219_subdev_pad_ops = {
857
-	.enum_mbus_code = imx219_enum_mbus_code,
858
-	.enum_frame_interval = imx219_enum_frame_interval,
859
-	.set_fmt = imx219_set_fmt,
860
-	.get_fmt = imx219_get_fmt,
861
-};
862
-
863
-static struct v4l2_subdev_ops imx219_subdev_ops = {
864
-	.core = &imx219_subdev_core_ops,
865
-	.video = &imx219_subdev_video_ops,
866
-	.pad = &imx219_subdev_pad_ops,
867
-};
868
-
869
-static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
870
-	.s_ctrl = imx219_s_ctrl,
871
-};
872
-
873
-static int imx219_video_probe(struct i2c_client *client)
986
+static int imx219_get_selection(struct v4l2_subdev *sd,
987
+				struct v4l2_subdev_pad_config *cfg,
988
+				struct v4l2_subdev_selection *sel)
874
989
 {
875
-	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
876
-	u16 model_id;
877
-	u32 lot_id;
878
-	u16 chip_id;
990
+	switch (sel->target) {
991
+	case V4L2_SEL_TGT_CROP: {
992
+		struct imx219 *imx219 = to_imx219(sd);
993
+
994
+		mutex_lock(&imx219->mutex);
995
+		sel->r = *__imx219_get_pad_crop(imx219, cfg, sel->pad,
996
+						sel->which);
997
+		mutex_unlock(&imx219->mutex);
998
+
999
+		return 0;
1000
+	}
1001
+
1002
+	case V4L2_SEL_TGT_NATIVE_SIZE:
1003
+		sel->r.top = 0;
1004
+		sel->r.left = 0;
1005
+		sel->r.width = IMX219_NATIVE_WIDTH;
1006
+		sel->r.height = IMX219_NATIVE_HEIGHT;
1007
+
1008
+		return 0;
1009
+
1010
+	case V4L2_SEL_TGT_CROP_DEFAULT:
1011
+	case V4L2_SEL_TGT_CROP_BOUNDS:
1012
+		sel->r.top = IMX219_PIXEL_ARRAY_TOP;
1013
+		sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
1014
+		sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
1015
+		sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
1016
+
1017
+		return 0;
1018
+	}
1019
+
1020
+	return -EINVAL;
1021
+}
1022
+
1023
+static int imx219_start_streaming(struct imx219 *imx219)
1024
+{
1025
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1026
+	const struct imx219_reg_list *reg_list;
879
1027
 	int ret;
880
1028
 
881
-	ret = imx219_s_power(subdev, 1);
882
-	if (ret < 0)
1029
+	ret = pm_runtime_get_sync(&client->dev);
1030
+	if (ret < 0) {
1031
+		pm_runtime_put_noidle(&client->dev);
883
1032
 		return ret;
884
-
885
-	/* Check and show model, lot, and chip ID. */
886
-	ret = reg_read(client, 0x0000);
887
-	if (ret < 0) {
888
-		dev_err(&client->dev, "Failure to read Model ID (high byte)\n");
889
-		goto done;
890
1033
 	}
891
-	model_id = ret << 8;
892
1034
 
893
-	ret = reg_read(client, 0x0001);
894
-	if (ret < 0) {
895
-		dev_err(&client->dev, "Failure to read Model ID (low byte)\n");
896
-		goto done;
1035
+	/* Apply default values of current mode */
1036
+	reg_list = &imx219->mode->reg_list;
1037
+	ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
1038
+	if (ret) {
1039
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
1040
+		goto err_rpm_put;
897
1041
 	}
898
-	model_id |= ret;
899
1042
 
900
-	ret = reg_read(client, 0x0004);
901
-	if (ret < 0) {
902
-		dev_err(&client->dev, "Failure to read Lot ID (high byte)\n");
903
-		goto done;
1043
+	ret = imx219_set_framefmt(imx219);
1044
+	if (ret) {
1045
+		dev_err(&client->dev, "%s failed to set frame format: %d\n",
1046
+			__func__, ret);
1047
+		goto err_rpm_put;
904
1048
 	}
905
-	lot_id = ret << 16;
906
1049
 
907
-	ret = reg_read(client, 0x0005);
908
-	if (ret < 0) {
909
-		dev_err(&client->dev, "Failure to read Lot ID (mid byte)\n");
910
-		goto done;
911
-	}
912
-	lot_id |= ret << 8;
1050
+	/* Apply customized values from user */
1051
+	ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
1052
+	if (ret)
1053
+		goto err_rpm_put;
913
1054
 
914
-	ret = reg_read(client, 0x0006);
915
-	if (ret < 0) {
916
-		dev_err(&client->dev, "Failure to read Lot ID (low byte)\n");
917
-		goto done;
918
-	}
919
-	lot_id |= ret;
1055
+	/* set stream on register */
1056
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1057
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
1058
+	if (ret)
1059
+		goto err_rpm_put;
920
1060
 
921
-	ret = reg_read(client, 0x000D);
922
-	if (ret < 0) {
923
-		dev_err(&client->dev, "Failure to read Chip ID (high byte)\n");
924
-		goto done;
925
-	}
926
-	chip_id = ret << 8;
1061
+	/* vflip and hflip cannot change during streaming */
1062
+	__v4l2_ctrl_grab(imx219->vflip, true);
1063
+	__v4l2_ctrl_grab(imx219->hflip, true);
927
1064
 
928
-	ret = reg_read(client, 0x000E);
929
-	if (ret < 0) {
930
-		dev_err(&client->dev, "Failure to read Chip ID (low byte)\n");
931
-		goto done;
932
-	}
933
-	chip_id |= ret;
1065
+	return 0;
934
1066
 
935
-	if (model_id != 0x0219) {
936
-		dev_err(&client->dev, "Model ID: %x not supported!\n",
937
-			model_id);
938
-		ret = -ENODEV;
939
-		goto done;
940
-	}
941
-	dev_info(&client->dev,
942
-		 "Model ID 0x%04x, Lot ID 0x%06x, Chip ID 0x%04x\n",
943
-		 model_id, lot_id, chip_id);
944
-done:
945
-	imx219_s_power(subdev, 0);
1067
+err_rpm_put:
1068
+	pm_runtime_put(&client->dev);
946
1069
 	return ret;
947
1070
 }
948
1071
 
949
-static int imx219_ctrls_init(struct v4l2_subdev *sd)
1072
+static void imx219_stop_streaming(struct imx219 *imx219)
950
1073
 {
951
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
952
-	struct imx219 *priv = to_imx219(client);
953
-	const struct imx219_mode *mode = priv->cur_mode;
954
-	s64 pixel_rate, h_blank, v_blank;
1074
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
955
1075
 	int ret;
956
-	u32 fps = 0;
957
1076
 
958
-	v4l2_ctrl_handler_init(&priv->ctrl_handler, 10);
959
-	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
960
-			  V4L2_CID_HFLIP, 0, 1, 1, 0);
961
-	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
962
-			  V4L2_CID_VFLIP, 0, 1, 1, 0);
1077
+	/* set stream off register */
1078
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1079
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
1080
+	if (ret)
1081
+		dev_err(&client->dev, "%s failed to set stream\n", __func__);
963
1082
 
964
-	/* exposure */
965
-	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
966
-			  V4L2_CID_ANALOGUE_GAIN,
967
-			  IMX219_ANALOGUE_GAIN_MIN,
968
-			  IMX219_ANALOGUE_GAIN_MAX,
969
-			  1, IMX219_ANALOGUE_GAIN_DEFAULT);
970
-	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
971
-			  V4L2_CID_GAIN,
972
-			  IMX219_DIGITAL_GAIN_MIN,
973
-			  IMX219_DIGITAL_GAIN_MAX, 1,
974
-			  IMX219_DIGITAL_GAIN_DEFAULT);
975
-	v4l2_ctrl_new_std(&priv->ctrl_handler, &imx219_ctrl_ops,
976
-			  V4L2_CID_EXPOSURE,
977
-			  IMX219_DIGITAL_EXPOSURE_MIN,
978
-			  IMX219_DIGITAL_EXPOSURE_MAX, 1,
979
-			  IMX219_DIGITAL_EXPOSURE_DEFAULT);
1083
+	__v4l2_ctrl_grab(imx219->vflip, false);
1084
+	__v4l2_ctrl_grab(imx219->hflip, false);
980
1085
 
981
-	/* blank */
982
-	h_blank = mode->hts_def - mode->width;
983
-	priv->hblank = v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_HBLANK,
984
-			  h_blank, h_blank, 1, h_blank);
985
-	v_blank = mode->vts_def - mode->height;
986
-	priv->vblank = v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_VBLANK,
987
-			  v_blank, v_blank, 1, v_blank);
1086
+	pm_runtime_put(&client->dev);
1087
+}
988
1088
 
989
-	/* freq */
990
-	v4l2_ctrl_new_int_menu(&priv->ctrl_handler, NULL, V4L2_CID_LINK_FREQ,
991
-			       0, 0, link_freq_menu_items);
992
-	fps = DIV_ROUND_CLOSEST(mode->max_fps.denominator,
993
-		mode->max_fps.numerator);
994
-	pixel_rate = mode->vts_def * mode->hts_def * fps;
995
-	priv->pixel_rate = v4l2_ctrl_new_std(&priv->ctrl_handler, NULL, V4L2_CID_PIXEL_RATE,
996
-			  0, pixel_rate, 1, pixel_rate);
1089
+static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
1090
+{
1091
+	struct imx219 *imx219 = to_imx219(sd);
1092
+	int ret = 0;
997
1093
 
998
-	v4l2_ctrl_new_std_menu_items(&priv->ctrl_handler, &imx219_ctrl_ops,
999
-				     V4L2_CID_TEST_PATTERN,
1000
-				     ARRAY_SIZE(tp_qmenu) - 1, 0, 0, tp_qmenu);
1001
-
1002
-	priv->subdev.ctrl_handler = &priv->ctrl_handler;
1003
-	if (priv->ctrl_handler.error) {
1004
-		dev_err(&client->dev, "Error %d adding controls\n",
1005
-			priv->ctrl_handler.error);
1006
-		ret = priv->ctrl_handler.error;
1007
-		goto error;
1094
+	mutex_lock(&imx219->mutex);
1095
+	if (imx219->streaming == enable) {
1096
+		mutex_unlock(&imx219->mutex);
1097
+		return 0;
1008
1098
 	}
1009
1099
 
1010
-	ret = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
1011
-	if (ret < 0) {
1012
-		dev_err(&client->dev, "Error %d setting default controls\n",
1013
-			ret);
1014
-		goto error;
1100
+	if (enable) {
1101
+		/*
1102
+		 * Apply default & customized values
1103
+		 * and then start streaming.
1104
+		 */
1105
+		ret = imx219_start_streaming(imx219);
1106
+		if (ret)
1107
+			goto err_unlock;
1108
+	} else {
1109
+		imx219_stop_streaming(imx219);
1110
+	}
1111
+
1112
+	imx219->streaming = enable;
1113
+
1114
+	mutex_unlock(&imx219->mutex);
1115
+
1116
+	return ret;
1117
+
1118
+err_unlock:
1119
+	mutex_unlock(&imx219->mutex);
1120
+
1121
+	return ret;
1122
+}
1123
+
1124
+/* Power/clock management functions */
1125
+static int imx219_power_on(struct device *dev)
1126
+{
1127
+	struct i2c_client *client = to_i2c_client(dev);
1128
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1129
+	struct imx219 *imx219 = to_imx219(sd);
1130
+	int ret;
1131
+
1132
+	ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES,
1133
+				    imx219->supplies);
1134
+	if (ret) {
1135
+		dev_err(&client->dev, "%s: failed to enable regulators\n",
1136
+			__func__);
1137
+		return ret;
1138
+	}
1139
+
1140
+	ret = clk_prepare_enable(imx219->xclk);
1141
+	if (ret) {
1142
+		dev_err(&client->dev, "%s: failed to enable clock\n",
1143
+			__func__);
1144
+		goto reg_off;
1145
+	}
1146
+
1147
+	gpiod_set_value_cansleep(imx219->reset_gpio, 1);
1148
+	usleep_range(IMX219_XCLR_MIN_DELAY_US,
1149
+		     IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
1150
+
1151
+	return 0;
1152
+
1153
+reg_off:
1154
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
1155
+
1156
+	return ret;
1157
+}
1158
+
1159
+static int imx219_power_off(struct device *dev)
1160
+{
1161
+	struct i2c_client *client = to_i2c_client(dev);
1162
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1163
+	struct imx219 *imx219 = to_imx219(sd);
1164
+
1165
+	gpiod_set_value_cansleep(imx219->reset_gpio, 0);
1166
+	regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
1167
+	clk_disable_unprepare(imx219->xclk);
1168
+
1169
+	return 0;
1170
+}
1171
+
1172
+static int __maybe_unused imx219_suspend(struct device *dev)
1173
+{
1174
+	struct i2c_client *client = to_i2c_client(dev);
1175
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1176
+	struct imx219 *imx219 = to_imx219(sd);
1177
+
1178
+	if (imx219->streaming)
1179
+		imx219_stop_streaming(imx219);
1180
+
1181
+	return 0;
1182
+}
1183
+
1184
+static int __maybe_unused imx219_resume(struct device *dev)
1185
+{
1186
+	struct i2c_client *client = to_i2c_client(dev);
1187
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1188
+	struct imx219 *imx219 = to_imx219(sd);
1189
+	int ret;
1190
+
1191
+	if (imx219->streaming) {
1192
+		ret = imx219_start_streaming(imx219);
1193
+		if (ret)
1194
+			goto error;
1015
1195
 	}
1016
1196
 
1017
1197
 	return 0;
1198
+
1018
1199
 error:
1019
-	v4l2_ctrl_handler_free(&priv->ctrl_handler);
1200
+	imx219_stop_streaming(imx219);
1201
+	imx219->streaming = false;
1202
+
1020
1203
 	return ret;
1021
1204
 }
1022
1205
 
1023
-static int imx219_probe(struct i2c_client *client,
1024
-			const struct i2c_device_id *did)
1206
+static int imx219_get_regulators(struct imx219 *imx219)
1025
1207
 {
1026
-	struct imx219 *priv;
1027
-	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1028
-	struct device *dev = &client->dev;
1029
-	struct device_node *node = dev->of_node;
1030
-	struct v4l2_subdev *sd;
1031
-	char facing[2];
1208
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1209
+	unsigned int i;
1210
+
1211
+	for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
1212
+		imx219->supplies[i].supply = imx219_supply_name[i];
1213
+
1214
+	return devm_regulator_bulk_get(&client->dev,
1215
+				       IMX219_NUM_SUPPLIES,
1216
+				       imx219->supplies);
1217
+}
1218
+
1219
+/* Verify chip ID */
1220
+static int imx219_identify_module(struct imx219 *imx219)
1221
+{
1222
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1032
1223
 	int ret;
1224
+	u32 val;
1033
1225
 
1034
-	dev_info(dev, "driver version: %02x.%02x.%02x",
1035
-		DRIVER_VERSION >> 16,
1036
-		(DRIVER_VERSION & 0xff00) >> 8,
1037
-		DRIVER_VERSION & 0x00ff);
1226
+	ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
1227
+			      IMX219_REG_VALUE_16BIT, &val);
1228
+	if (ret) {
1229
+		dev_err(&client->dev, "failed to read chip id %x\n",
1230
+			IMX219_CHIP_ID);
1231
+		return ret;
1232
+	}
1038
1233
 
1039
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
1040
-		dev_warn(&adapter->dev,
1041
-			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_BYTE\n");
1234
+	if (val != IMX219_CHIP_ID) {
1235
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
1236
+			IMX219_CHIP_ID, val);
1042
1237
 		return -EIO;
1043
1238
 	}
1044
-	priv = devm_kzalloc(&client->dev, sizeof(struct imx219), GFP_KERNEL);
1045
-	if (!priv)
1046
-		return -ENOMEM;
1047
1239
 
1048
-	ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
1049
-				   &priv->module_index);
1050
-	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
1051
-				       &priv->module_facing);
1052
-	ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME,
1053
-				       &priv->module_name);
1054
-	ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME,
1055
-				       &priv->len_name);
1056
-	if (ret) {
1057
-		dev_err(dev, "could not get module information!\n");
1240
+	return 0;
1241
+}
1242
+
1243
+static const struct v4l2_subdev_core_ops imx219_core_ops = {
1244
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1245
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
1246
+};
1247
+
1248
+static const struct v4l2_subdev_video_ops imx219_video_ops = {
1249
+	.s_stream = imx219_set_stream,
1250
+};
1251
+
1252
+static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
1253
+	.enum_mbus_code = imx219_enum_mbus_code,
1254
+	.get_fmt = imx219_get_pad_format,
1255
+	.set_fmt = imx219_set_pad_format,
1256
+	.get_selection = imx219_get_selection,
1257
+	.enum_frame_size = imx219_enum_frame_size,
1258
+};
1259
+
1260
+static const struct v4l2_subdev_ops imx219_subdev_ops = {
1261
+	.core = &imx219_core_ops,
1262
+	.video = &imx219_video_ops,
1263
+	.pad = &imx219_pad_ops,
1264
+};
1265
+
1266
+static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
1267
+	.open = imx219_open,
1268
+};
1269
+
1270
+/* Initialize control handlers */
1271
+static int imx219_init_controls(struct imx219 *imx219)
1272
+{
1273
+	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1274
+	struct v4l2_ctrl_handler *ctrl_hdlr;
1275
+	unsigned int height = imx219->mode->height;
1276
+	struct v4l2_fwnode_device_properties props;
1277
+	int exposure_max, exposure_def, hblank;
1278
+	int i, ret;
1279
+
1280
+	ctrl_hdlr = &imx219->ctrl_handler;
1281
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 11);
1282
+	if (ret)
1283
+		return ret;
1284
+
1285
+	mutex_init(&imx219->mutex);
1286
+	ctrl_hdlr->lock = &imx219->mutex;
1287
+
1288
+	/* By default, PIXEL_RATE is read only */
1289
+	imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1290
+					       V4L2_CID_PIXEL_RATE,
1291
+					       IMX219_PIXEL_RATE,
1292
+					       IMX219_PIXEL_RATE, 1,
1293
+					       IMX219_PIXEL_RATE);
1294
+
1295
+	/* Initial vblank/hblank/exposure parameters based on current mode */
1296
+	imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1297
+					   V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
1298
+					   IMX219_VTS_MAX - height, 1,
1299
+					   imx219->mode->vts_def - height);
1300
+	hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
1301
+	imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1302
+					   V4L2_CID_HBLANK, hblank, hblank,
1303
+					   1, hblank);
1304
+	if (imx219->hblank)
1305
+		imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1306
+	exposure_max = imx219->mode->vts_def - 4;
1307
+	exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
1308
+		exposure_max : IMX219_EXPOSURE_DEFAULT;
1309
+	imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1310
+					     V4L2_CID_EXPOSURE,
1311
+					     IMX219_EXPOSURE_MIN, exposure_max,
1312
+					     IMX219_EXPOSURE_STEP,
1313
+					     exposure_def);
1314
+
1315
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
1316
+			  IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
1317
+			  IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
1318
+
1319
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
1320
+			  IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
1321
+			  IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
1322
+
1323
+	imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1324
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
1325
+	if (imx219->hflip)
1326
+		imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1327
+
1328
+	imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1329
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
1330
+	if (imx219->vflip)
1331
+		imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1332
+
1333
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops,
1334
+				     V4L2_CID_TEST_PATTERN,
1335
+				     ARRAY_SIZE(imx219_test_pattern_menu) - 1,
1336
+				     0, 0, imx219_test_pattern_menu);
1337
+	for (i = 0; i < 4; i++) {
1338
+		/*
1339
+		 * The assumption is that
1340
+		 * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
1341
+		 * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
1342
+		 * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
1343
+		 */
1344
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1345
+				  V4L2_CID_TEST_PATTERN_RED + i,
1346
+				  IMX219_TESTP_COLOUR_MIN,
1347
+				  IMX219_TESTP_COLOUR_MAX,
1348
+				  IMX219_TESTP_COLOUR_STEP,
1349
+				  IMX219_TESTP_COLOUR_MAX);
1350
+		/* The "Solid color" pattern is white by default */
1351
+	}
1352
+
1353
+	if (ctrl_hdlr->error) {
1354
+		ret = ctrl_hdlr->error;
1355
+		dev_err(&client->dev, "%s control init failed (%d)\n",
1356
+			__func__, ret);
1357
+		goto error;
1358
+	}
1359
+
1360
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
1361
+	if (ret)
1362
+		goto error;
1363
+
1364
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops,
1365
+					      &props);
1366
+	if (ret)
1367
+		goto error;
1368
+
1369
+	imx219->sd.ctrl_handler = ctrl_hdlr;
1370
+
1371
+	return 0;
1372
+
1373
+error:
1374
+	v4l2_ctrl_handler_free(ctrl_hdlr);
1375
+	mutex_destroy(&imx219->mutex);
1376
+
1377
+	return ret;
1378
+}
1379
+
1380
+static void imx219_free_controls(struct imx219 *imx219)
1381
+{
1382
+	v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
1383
+	mutex_destroy(&imx219->mutex);
1384
+}
1385
+
1386
+static int imx219_check_hwcfg(struct device *dev)
1387
+{
1388
+	struct fwnode_handle *endpoint;
1389
+	struct v4l2_fwnode_endpoint ep_cfg = {
1390
+		.bus_type = V4L2_MBUS_CSI2_DPHY
1391
+	};
1392
+	int ret = -EINVAL;
1393
+
1394
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
1395
+	if (!endpoint) {
1396
+		dev_err(dev, "endpoint node not found\n");
1058
1397
 		return -EINVAL;
1059
1398
 	}
1060
1399
 
1061
-	priv->clk = devm_clk_get(&client->dev, NULL);
1062
-	if (IS_ERR(priv->clk)) {
1063
-		dev_info(&client->dev, "Error %ld getting clock\n",
1064
-			 PTR_ERR(priv->clk));
1065
-		return -EPROBE_DEFER;
1400
+	if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
1401
+		dev_err(dev, "could not parse endpoint\n");
1402
+		goto error_out;
1066
1403
 	}
1067
1404
 
1068
-	/* 1920 * 1080 by default */
1069
-	priv->cur_mode = &supported_modes[0];
1070
-	priv->cfg_num = ARRAY_SIZE(supported_modes);
1405
+	/* Check the number of MIPI CSI2 data lanes */
1406
+	if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2) {
1407
+		dev_err(dev, "only 2 data lanes are currently supported\n");
1408
+		goto error_out;
1409
+	}
1071
1410
 
1072
-	priv->crop_rect.left = 680;
1073
-	priv->crop_rect.top = 692;
1074
-	priv->crop_rect.width = priv->cur_mode->width;
1075
-	priv->crop_rect.height = priv->cur_mode->height;
1411
+	/* Check the link frequency set in device tree */
1412
+	if (!ep_cfg.nr_of_link_frequencies) {
1413
+		dev_err(dev, "link-frequency property not found in DT\n");
1414
+		goto error_out;
1415
+	}
1076
1416
 
1077
-	v4l2_i2c_subdev_init(&priv->subdev, client, &imx219_subdev_ops);
1078
-	ret = imx219_ctrls_init(&priv->subdev);
1079
-	if (ret < 0)
1417
+	if (ep_cfg.nr_of_link_frequencies != 1 ||
1418
+	    ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
1419
+		dev_err(dev, "Link frequency not supported: %lld\n",
1420
+			ep_cfg.link_frequencies[0]);
1421
+		goto error_out;
1422
+	}
1423
+
1424
+	ret = 0;
1425
+
1426
+error_out:
1427
+	v4l2_fwnode_endpoint_free(&ep_cfg);
1428
+	fwnode_handle_put(endpoint);
1429
+
1430
+	return ret;
1431
+}
1432
+
1433
+static int imx219_probe(struct i2c_client *client)
1434
+{
1435
+	struct device *dev = &client->dev;
1436
+	struct imx219 *imx219;
1437
+	int ret;
1438
+
1439
+	imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
1440
+	if (!imx219)
1441
+		return -ENOMEM;
1442
+
1443
+	v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
1444
+
1445
+	/* Check the hardware configuration in device tree */
1446
+	if (imx219_check_hwcfg(dev))
1447
+		return -EINVAL;
1448
+
1449
+	/* Get system clock (xclk) */
1450
+	imx219->xclk = devm_clk_get(dev, NULL);
1451
+	if (IS_ERR(imx219->xclk)) {
1452
+		dev_err(dev, "failed to get xclk\n");
1453
+		return PTR_ERR(imx219->xclk);
1454
+	}
1455
+
1456
+	imx219->xclk_freq = clk_get_rate(imx219->xclk);
1457
+	if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
1458
+		dev_err(dev, "xclk frequency not supported: %d Hz\n",
1459
+			imx219->xclk_freq);
1460
+		return -EINVAL;
1461
+	}
1462
+
1463
+	ret = imx219_get_regulators(imx219);
1464
+	if (ret) {
1465
+		dev_err(dev, "failed to get regulators\n");
1080
1466
 		return ret;
1081
-	ret = imx219_video_probe(client);
1082
-	if (ret < 0)
1467
+	}
1468
+
1469
+	/* Request optional enable pin */
1470
+	imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1471
+						     GPIOD_OUT_HIGH);
1472
+
1473
+	/*
1474
+	 * The sensor must be powered for imx219_identify_module()
1475
+	 * to be able to read the CHIP_ID register
1476
+	 */
1477
+	ret = imx219_power_on(dev);
1478
+	if (ret)
1083
1479
 		return ret;
1084
1480
 
1085
-	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1086
-		     V4L2_SUBDEV_FL_HAS_EVENTS;
1481
+	ret = imx219_identify_module(imx219);
1482
+	if (ret)
1483
+		goto error_power_off;
1087
1484
 
1088
-	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
1089
-	priv->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1090
-	ret = media_entity_pads_init(&priv->subdev.entity, 1, &priv->pad);
1485
+	/* Set default mode to max resolution */
1486
+	imx219->mode = &supported_modes[0];
1487
+
1488
+	/* sensor doesn't enter LP-11 state upon power up until and unless
1489
+	 * streaming is started, so upon power up switch the modes to:
1490
+	 * streaming -> standby
1491
+	 */
1492
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1493
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
1091
1494
 	if (ret < 0)
1092
-		return ret;
1495
+		goto error_power_off;
1496
+	usleep_range(100, 110);
1093
1497
 
1094
-	sd = &priv->subdev;
1095
-	memset(facing, 0, sizeof(facing));
1096
-	if (strcmp(priv->module_facing, "back") == 0)
1097
-		facing[0] = 'b';
1098
-	else
1099
-		facing[0] = 'f';
1100
-
1101
-	snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
1102
-		 priv->module_index, facing,
1103
-		 IMX219_NAME, dev_name(sd->dev));
1104
-	ret = v4l2_async_register_subdev_sensor_common(sd);
1498
+	/* put sensor back to standby mode */
1499
+	ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1500
+			       IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
1105
1501
 	if (ret < 0)
1106
-		return ret;
1502
+		goto error_power_off;
1503
+	usleep_range(100, 110);
1504
+
1505
+	ret = imx219_init_controls(imx219);
1506
+	if (ret)
1507
+		goto error_power_off;
1508
+
1509
+	/* Initialize subdev */
1510
+	imx219->sd.internal_ops = &imx219_internal_ops;
1511
+	imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1512
+	imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1513
+
1514
+	/* Initialize source pad */
1515
+	imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
1516
+
1517
+	/* Initialize default format */
1518
+	imx219_set_default_format(imx219);
1519
+
1520
+	ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
1521
+	if (ret) {
1522
+		dev_err(dev, "failed to init entity pads: %d\n", ret);
1523
+		goto error_handler_free;
1524
+	}
1525
+
1526
+	ret = v4l2_async_register_subdev_sensor_common(&imx219->sd);
1527
+	if (ret < 0) {
1528
+		dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
1529
+		goto error_media_entity;
1530
+	}
1531
+
1532
+	/* Enable runtime PM and turn off the device */
1533
+	pm_runtime_set_active(dev);
1534
+	pm_runtime_enable(dev);
1535
+	pm_runtime_idle(dev);
1536
+
1537
+	return 0;
1538
+
1539
+error_media_entity:
1540
+	media_entity_cleanup(&imx219->sd.entity);
1541
+
1542
+error_handler_free:
1543
+	imx219_free_controls(imx219);
1544
+
1545
+error_power_off:
1546
+	imx219_power_off(dev);
1107
1547
 
1108
1548
 	return ret;
1109
1549
 }
1110
1550
 
1111
1551
 static int imx219_remove(struct i2c_client *client)
1112
1552
 {
1113
-	struct imx219 *priv = to_imx219(client);
1553
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1554
+	struct imx219 *imx219 = to_imx219(sd);
1114
1555
 
1115
-	v4l2_async_unregister_subdev(&priv->subdev);
1116
-	media_entity_cleanup(&priv->subdev.entity);
1117
-	v4l2_ctrl_handler_free(&priv->ctrl_handler);
1556
+	v4l2_async_unregister_subdev(sd);
1557
+	media_entity_cleanup(&sd->entity);
1558
+	imx219_free_controls(imx219);
1559
+
1560
+	pm_runtime_disable(&client->dev);
1561
+	if (!pm_runtime_status_suspended(&client->dev))
1562
+		imx219_power_off(&client->dev);
1563
+	pm_runtime_set_suspended(&client->dev);
1118
1564
 
1119
1565
 	return 0;
1120
1566
 }
1121
1567
 
1122
-static const struct i2c_device_id imx219_id[] = {
1123
-	{"imx219", 0},
1124
-	{}
1125
-};
1126
-
1127
-static const struct of_device_id imx219_of_match[] = {
1568
+static const struct of_device_id imx219_dt_ids[] = {
1128
1569
 	{ .compatible = "sony,imx219" },
1129
-	{ /* sentinel */ },
1570
+	{ /* sentinel */ }
1130
1571
 };
1131
-MODULE_DEVICE_TABLE(of, imx219_of_match);
1572
+MODULE_DEVICE_TABLE(of, imx219_dt_ids);
1132
1573
 
1133
-MODULE_DEVICE_TABLE(i2c, imx219_id);
1574
+static const struct dev_pm_ops imx219_pm_ops = {
1575
+	SET_SYSTEM_SLEEP_PM_OPS(imx219_suspend, imx219_resume)
1576
+	SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
1577
+};
1578
+
1134
1579
 static struct i2c_driver imx219_i2c_driver = {
1135
1580
 	.driver = {
1136
-		.of_match_table = of_match_ptr(imx219_of_match),
1137
-		.name = IMX219_NAME,
1581
+		.name = "imx219",
1582
+		.of_match_table	= imx219_dt_ids,
1583
+		.pm = &imx219_pm_ops,
1138
1584
 	},
1139
-	.probe = imx219_probe,
1585
+	.probe_new = imx219_probe,
1140
1586
 	.remove = imx219_remove,
1141
-	.id_table = imx219_id,
1142
1587
 };
1143
1588
 
1144
1589
 module_i2c_driver(imx219_i2c_driver);
1145
-MODULE_DESCRIPTION("Sony IMX219 Camera driver");
1146
-MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
1590
+
1591
+MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com");
1592
+MODULE_DESCRIPTION("Sony IMX219 sensor driver");
1147
1593
 MODULE_LICENSE("GPL v2");