Linux 5.15.57
[platform/kernel/linux-rpi.git] / drivers / media / i2c / imx258.c
1 // SPDX-License-Identifier: GPL-2.0
2 // Copyright (C) 2018 Intel Corporation
3
4 #include <linux/acpi.h>
5 #include <linux/clk.h>
6 #include <linux/delay.h>
7 #include <linux/i2c.h>
8 #include <linux/module.h>
9 #include <linux/pm_runtime.h>
10 #include <media/v4l2-ctrls.h>
11 #include <media/v4l2-device.h>
12 #include <asm/unaligned.h>
13
14 #define IMX258_REG_VALUE_08BIT          1
15 #define IMX258_REG_VALUE_16BIT          2
16
17 #define IMX258_REG_MODE_SELECT          0x0100
18 #define IMX258_MODE_STANDBY             0x00
19 #define IMX258_MODE_STREAMING           0x01
20
21 /* Chip ID */
22 #define IMX258_REG_CHIP_ID              0x0016
23 #define IMX258_CHIP_ID                  0x0258
24
25 /* V_TIMING internal */
26 #define IMX258_VTS_30FPS                0x0c50
27 #define IMX258_VTS_30FPS_2K             0x0638
28 #define IMX258_VTS_30FPS_VGA            0x034c
29 #define IMX258_VTS_MAX                  0xffff
30
31 /*Frame Length Line*/
32 #define IMX258_FLL_MIN                  0x08a6
33 #define IMX258_FLL_MAX                  0xffff
34 #define IMX258_FLL_STEP                 1
35 #define IMX258_FLL_DEFAULT              0x0c98
36
37 /* HBLANK control - read only */
38 #define IMX258_PPL_DEFAULT              5352
39
40 /* Exposure control */
41 #define IMX258_REG_EXPOSURE             0x0202
42 #define IMX258_EXPOSURE_MIN             4
43 #define IMX258_EXPOSURE_STEP            1
44 #define IMX258_EXPOSURE_DEFAULT         0x640
45 #define IMX258_EXPOSURE_MAX             65535
46
47 /* Analog gain control */
48 #define IMX258_REG_ANALOG_GAIN          0x0204
49 #define IMX258_ANA_GAIN_MIN             0
50 #define IMX258_ANA_GAIN_MAX             480
51 #define IMX258_ANA_GAIN_STEP            1
52 #define IMX258_ANA_GAIN_DEFAULT         0x0
53
54 /* Digital gain control */
55 #define IMX258_REG_GR_DIGITAL_GAIN      0x020e
56 #define IMX258_REG_R_DIGITAL_GAIN       0x0210
57 #define IMX258_REG_B_DIGITAL_GAIN       0x0212
58 #define IMX258_REG_GB_DIGITAL_GAIN      0x0214
59 #define IMX258_DGTL_GAIN_MIN            0
60 #define IMX258_DGTL_GAIN_MAX            4096    /* Max = 0xFFF */
61 #define IMX258_DGTL_GAIN_DEFAULT        1024
62 #define IMX258_DGTL_GAIN_STEP           1
63
64 /* HDR control */
65 #define IMX258_REG_HDR                  0x0220
66 #define IMX258_HDR_ON                   BIT(0)
67 #define IMX258_REG_HDR_RATIO            0x0222
68 #define IMX258_HDR_RATIO_MIN            0
69 #define IMX258_HDR_RATIO_MAX            5
70 #define IMX258_HDR_RATIO_STEP           1
71 #define IMX258_HDR_RATIO_DEFAULT        0x0
72
73 /* Test Pattern Control */
74 #define IMX258_REG_TEST_PATTERN         0x0600
75
76 /* Orientation */
77 #define REG_MIRROR_FLIP_CONTROL         0x0101
78 #define REG_CONFIG_MIRROR_FLIP          0x03
79 #define REG_CONFIG_FLIP_TEST_PATTERN    0x02
80
81 /* Input clock frequency in Hz */
82 #define IMX258_INPUT_CLOCK_FREQ         19200000
83
84 struct imx258_reg {
85         u16 address;
86         u8 val;
87 };
88
89 struct imx258_reg_list {
90         u32 num_of_regs;
91         const struct imx258_reg *regs;
92 };
93
94 /* Link frequency config */
95 struct imx258_link_freq_config {
96         u32 pixels_per_line;
97
98         /* PLL registers for this link frequency */
99         struct imx258_reg_list reg_list;
100 };
101
102 /* Mode : resolution and related config&values */
103 struct imx258_mode {
104         /* Frame width */
105         u32 width;
106         /* Frame height */
107         u32 height;
108
109         /* V-timing */
110         u32 vts_def;
111         u32 vts_min;
112
113         /* Index of Link frequency config to be used */
114         u32 link_freq_index;
115         /* Default register values */
116         struct imx258_reg_list reg_list;
117 };
118
119 /* 4208x3118 needs 1267Mbps/lane, 4 lanes */
120 static const struct imx258_reg mipi_data_rate_1267mbps[] = {
121         { 0x0301, 0x05 },
122         { 0x0303, 0x02 },
123         { 0x0305, 0x03 },
124         { 0x0306, 0x00 },
125         { 0x0307, 0xC6 },
126         { 0x0309, 0x0A },
127         { 0x030B, 0x01 },
128         { 0x030D, 0x02 },
129         { 0x030E, 0x00 },
130         { 0x030F, 0xD8 },
131         { 0x0310, 0x00 },
132         { 0x0820, 0x13 },
133         { 0x0821, 0x4C },
134         { 0x0822, 0xCC },
135         { 0x0823, 0xCC },
136 };
137
138 static const struct imx258_reg mipi_data_rate_640mbps[] = {
139         { 0x0301, 0x05 },
140         { 0x0303, 0x02 },
141         { 0x0305, 0x03 },
142         { 0x0306, 0x00 },
143         { 0x0307, 0x64 },
144         { 0x0309, 0x0A },
145         { 0x030B, 0x01 },
146         { 0x030D, 0x02 },
147         { 0x030E, 0x00 },
148         { 0x030F, 0xD8 },
149         { 0x0310, 0x00 },
150         { 0x0820, 0x0A },
151         { 0x0821, 0x00 },
152         { 0x0822, 0x00 },
153         { 0x0823, 0x00 },
154 };
155
156 static const struct imx258_reg mode_4208x3118_regs[] = {
157         { 0x0136, 0x13 },
158         { 0x0137, 0x33 },
159         { 0x3051, 0x00 },
160         { 0x3052, 0x00 },
161         { 0x4E21, 0x14 },
162         { 0x6B11, 0xCF },
163         { 0x7FF0, 0x08 },
164         { 0x7FF1, 0x0F },
165         { 0x7FF2, 0x08 },
166         { 0x7FF3, 0x1B },
167         { 0x7FF4, 0x23 },
168         { 0x7FF5, 0x60 },
169         { 0x7FF6, 0x00 },
170         { 0x7FF7, 0x01 },
171         { 0x7FF8, 0x00 },
172         { 0x7FF9, 0x78 },
173         { 0x7FFA, 0x00 },
174         { 0x7FFB, 0x00 },
175         { 0x7FFC, 0x00 },
176         { 0x7FFD, 0x00 },
177         { 0x7FFE, 0x00 },
178         { 0x7FFF, 0x03 },
179         { 0x7F76, 0x03 },
180         { 0x7F77, 0xFE },
181         { 0x7FA8, 0x03 },
182         { 0x7FA9, 0xFE },
183         { 0x7B24, 0x81 },
184         { 0x7B25, 0x00 },
185         { 0x6564, 0x07 },
186         { 0x6B0D, 0x41 },
187         { 0x653D, 0x04 },
188         { 0x6B05, 0x8C },
189         { 0x6B06, 0xF9 },
190         { 0x6B08, 0x65 },
191         { 0x6B09, 0xFC },
192         { 0x6B0A, 0xCF },
193         { 0x6B0B, 0xD2 },
194         { 0x6700, 0x0E },
195         { 0x6707, 0x0E },
196         { 0x9104, 0x00 },
197         { 0x4648, 0x7F },
198         { 0x7420, 0x00 },
199         { 0x7421, 0x1C },
200         { 0x7422, 0x00 },
201         { 0x7423, 0xD7 },
202         { 0x5F04, 0x00 },
203         { 0x5F05, 0xED },
204         { 0x0112, 0x0A },
205         { 0x0113, 0x0A },
206         { 0x0114, 0x03 },
207         { 0x0342, 0x14 },
208         { 0x0343, 0xE8 },
209         { 0x0340, 0x0C },
210         { 0x0341, 0x50 },
211         { 0x0344, 0x00 },
212         { 0x0345, 0x00 },
213         { 0x0346, 0x00 },
214         { 0x0347, 0x00 },
215         { 0x0348, 0x10 },
216         { 0x0349, 0x6F },
217         { 0x034A, 0x0C },
218         { 0x034B, 0x2E },
219         { 0x0381, 0x01 },
220         { 0x0383, 0x01 },
221         { 0x0385, 0x01 },
222         { 0x0387, 0x01 },
223         { 0x0900, 0x00 },
224         { 0x0901, 0x11 },
225         { 0x0401, 0x00 },
226         { 0x0404, 0x00 },
227         { 0x0405, 0x10 },
228         { 0x0408, 0x00 },
229         { 0x0409, 0x00 },
230         { 0x040A, 0x00 },
231         { 0x040B, 0x00 },
232         { 0x040C, 0x10 },
233         { 0x040D, 0x70 },
234         { 0x040E, 0x0C },
235         { 0x040F, 0x30 },
236         { 0x3038, 0x00 },
237         { 0x303A, 0x00 },
238         { 0x303B, 0x10 },
239         { 0x300D, 0x00 },
240         { 0x034C, 0x10 },
241         { 0x034D, 0x70 },
242         { 0x034E, 0x0C },
243         { 0x034F, 0x30 },
244         { 0x0350, 0x01 },
245         { 0x0202, 0x0C },
246         { 0x0203, 0x46 },
247         { 0x0204, 0x00 },
248         { 0x0205, 0x00 },
249         { 0x020E, 0x01 },
250         { 0x020F, 0x00 },
251         { 0x0210, 0x01 },
252         { 0x0211, 0x00 },
253         { 0x0212, 0x01 },
254         { 0x0213, 0x00 },
255         { 0x0214, 0x01 },
256         { 0x0215, 0x00 },
257         { 0x7BCD, 0x00 },
258         { 0x94DC, 0x20 },
259         { 0x94DD, 0x20 },
260         { 0x94DE, 0x20 },
261         { 0x95DC, 0x20 },
262         { 0x95DD, 0x20 },
263         { 0x95DE, 0x20 },
264         { 0x7FB0, 0x00 },
265         { 0x9010, 0x3E },
266         { 0x9419, 0x50 },
267         { 0x941B, 0x50 },
268         { 0x9519, 0x50 },
269         { 0x951B, 0x50 },
270         { 0x3030, 0x00 },
271         { 0x3032, 0x00 },
272         { 0x0220, 0x00 },
273 };
274
275 static const struct imx258_reg mode_2104_1560_regs[] = {
276         { 0x0136, 0x13 },
277         { 0x0137, 0x33 },
278         { 0x3051, 0x00 },
279         { 0x3052, 0x00 },
280         { 0x4E21, 0x14 },
281         { 0x6B11, 0xCF },
282         { 0x7FF0, 0x08 },
283         { 0x7FF1, 0x0F },
284         { 0x7FF2, 0x08 },
285         { 0x7FF3, 0x1B },
286         { 0x7FF4, 0x23 },
287         { 0x7FF5, 0x60 },
288         { 0x7FF6, 0x00 },
289         { 0x7FF7, 0x01 },
290         { 0x7FF8, 0x00 },
291         { 0x7FF9, 0x78 },
292         { 0x7FFA, 0x00 },
293         { 0x7FFB, 0x00 },
294         { 0x7FFC, 0x00 },
295         { 0x7FFD, 0x00 },
296         { 0x7FFE, 0x00 },
297         { 0x7FFF, 0x03 },
298         { 0x7F76, 0x03 },
299         { 0x7F77, 0xFE },
300         { 0x7FA8, 0x03 },
301         { 0x7FA9, 0xFE },
302         { 0x7B24, 0x81 },
303         { 0x7B25, 0x00 },
304         { 0x6564, 0x07 },
305         { 0x6B0D, 0x41 },
306         { 0x653D, 0x04 },
307         { 0x6B05, 0x8C },
308         { 0x6B06, 0xF9 },
309         { 0x6B08, 0x65 },
310         { 0x6B09, 0xFC },
311         { 0x6B0A, 0xCF },
312         { 0x6B0B, 0xD2 },
313         { 0x6700, 0x0E },
314         { 0x6707, 0x0E },
315         { 0x9104, 0x00 },
316         { 0x4648, 0x7F },
317         { 0x7420, 0x00 },
318         { 0x7421, 0x1C },
319         { 0x7422, 0x00 },
320         { 0x7423, 0xD7 },
321         { 0x5F04, 0x00 },
322         { 0x5F05, 0xED },
323         { 0x0112, 0x0A },
324         { 0x0113, 0x0A },
325         { 0x0114, 0x03 },
326         { 0x0342, 0x14 },
327         { 0x0343, 0xE8 },
328         { 0x0340, 0x06 },
329         { 0x0341, 0x38 },
330         { 0x0344, 0x00 },
331         { 0x0345, 0x00 },
332         { 0x0346, 0x00 },
333         { 0x0347, 0x00 },
334         { 0x0348, 0x10 },
335         { 0x0349, 0x6F },
336         { 0x034A, 0x0C },
337         { 0x034B, 0x2E },
338         { 0x0381, 0x01 },
339         { 0x0383, 0x01 },
340         { 0x0385, 0x01 },
341         { 0x0387, 0x01 },
342         { 0x0900, 0x01 },
343         { 0x0901, 0x12 },
344         { 0x0401, 0x01 },
345         { 0x0404, 0x00 },
346         { 0x0405, 0x20 },
347         { 0x0408, 0x00 },
348         { 0x0409, 0x02 },
349         { 0x040A, 0x00 },
350         { 0x040B, 0x00 },
351         { 0x040C, 0x10 },
352         { 0x040D, 0x6A },
353         { 0x040E, 0x06 },
354         { 0x040F, 0x18 },
355         { 0x3038, 0x00 },
356         { 0x303A, 0x00 },
357         { 0x303B, 0x10 },
358         { 0x300D, 0x00 },
359         { 0x034C, 0x08 },
360         { 0x034D, 0x38 },
361         { 0x034E, 0x06 },
362         { 0x034F, 0x18 },
363         { 0x0350, 0x01 },
364         { 0x0202, 0x06 },
365         { 0x0203, 0x2E },
366         { 0x0204, 0x00 },
367         { 0x0205, 0x00 },
368         { 0x020E, 0x01 },
369         { 0x020F, 0x00 },
370         { 0x0210, 0x01 },
371         { 0x0211, 0x00 },
372         { 0x0212, 0x01 },
373         { 0x0213, 0x00 },
374         { 0x0214, 0x01 },
375         { 0x0215, 0x00 },
376         { 0x7BCD, 0x01 },
377         { 0x94DC, 0x20 },
378         { 0x94DD, 0x20 },
379         { 0x94DE, 0x20 },
380         { 0x95DC, 0x20 },
381         { 0x95DD, 0x20 },
382         { 0x95DE, 0x20 },
383         { 0x7FB0, 0x00 },
384         { 0x9010, 0x3E },
385         { 0x9419, 0x50 },
386         { 0x941B, 0x50 },
387         { 0x9519, 0x50 },
388         { 0x951B, 0x50 },
389         { 0x3030, 0x00 },
390         { 0x3032, 0x00 },
391         { 0x0220, 0x00 },
392 };
393
394 static const struct imx258_reg mode_1048_780_regs[] = {
395         { 0x0136, 0x13 },
396         { 0x0137, 0x33 },
397         { 0x3051, 0x00 },
398         { 0x3052, 0x00 },
399         { 0x4E21, 0x14 },
400         { 0x6B11, 0xCF },
401         { 0x7FF0, 0x08 },
402         { 0x7FF1, 0x0F },
403         { 0x7FF2, 0x08 },
404         { 0x7FF3, 0x1B },
405         { 0x7FF4, 0x23 },
406         { 0x7FF5, 0x60 },
407         { 0x7FF6, 0x00 },
408         { 0x7FF7, 0x01 },
409         { 0x7FF8, 0x00 },
410         { 0x7FF9, 0x78 },
411         { 0x7FFA, 0x00 },
412         { 0x7FFB, 0x00 },
413         { 0x7FFC, 0x00 },
414         { 0x7FFD, 0x00 },
415         { 0x7FFE, 0x00 },
416         { 0x7FFF, 0x03 },
417         { 0x7F76, 0x03 },
418         { 0x7F77, 0xFE },
419         { 0x7FA8, 0x03 },
420         { 0x7FA9, 0xFE },
421         { 0x7B24, 0x81 },
422         { 0x7B25, 0x00 },
423         { 0x6564, 0x07 },
424         { 0x6B0D, 0x41 },
425         { 0x653D, 0x04 },
426         { 0x6B05, 0x8C },
427         { 0x6B06, 0xF9 },
428         { 0x6B08, 0x65 },
429         { 0x6B09, 0xFC },
430         { 0x6B0A, 0xCF },
431         { 0x6B0B, 0xD2 },
432         { 0x6700, 0x0E },
433         { 0x6707, 0x0E },
434         { 0x9104, 0x00 },
435         { 0x4648, 0x7F },
436         { 0x7420, 0x00 },
437         { 0x7421, 0x1C },
438         { 0x7422, 0x00 },
439         { 0x7423, 0xD7 },
440         { 0x5F04, 0x00 },
441         { 0x5F05, 0xED },
442         { 0x0112, 0x0A },
443         { 0x0113, 0x0A },
444         { 0x0114, 0x03 },
445         { 0x0342, 0x14 },
446         { 0x0343, 0xE8 },
447         { 0x0340, 0x03 },
448         { 0x0341, 0x4C },
449         { 0x0344, 0x00 },
450         { 0x0345, 0x00 },
451         { 0x0346, 0x00 },
452         { 0x0347, 0x00 },
453         { 0x0348, 0x10 },
454         { 0x0349, 0x6F },
455         { 0x034A, 0x0C },
456         { 0x034B, 0x2E },
457         { 0x0381, 0x01 },
458         { 0x0383, 0x01 },
459         { 0x0385, 0x01 },
460         { 0x0387, 0x01 },
461         { 0x0900, 0x01 },
462         { 0x0901, 0x14 },
463         { 0x0401, 0x01 },
464         { 0x0404, 0x00 },
465         { 0x0405, 0x40 },
466         { 0x0408, 0x00 },
467         { 0x0409, 0x06 },
468         { 0x040A, 0x00 },
469         { 0x040B, 0x00 },
470         { 0x040C, 0x10 },
471         { 0x040D, 0x64 },
472         { 0x040E, 0x03 },
473         { 0x040F, 0x0C },
474         { 0x3038, 0x00 },
475         { 0x303A, 0x00 },
476         { 0x303B, 0x10 },
477         { 0x300D, 0x00 },
478         { 0x034C, 0x04 },
479         { 0x034D, 0x18 },
480         { 0x034E, 0x03 },
481         { 0x034F, 0x0C },
482         { 0x0350, 0x01 },
483         { 0x0202, 0x03 },
484         { 0x0203, 0x42 },
485         { 0x0204, 0x00 },
486         { 0x0205, 0x00 },
487         { 0x020E, 0x01 },
488         { 0x020F, 0x00 },
489         { 0x0210, 0x01 },
490         { 0x0211, 0x00 },
491         { 0x0212, 0x01 },
492         { 0x0213, 0x00 },
493         { 0x0214, 0x01 },
494         { 0x0215, 0x00 },
495         { 0x7BCD, 0x00 },
496         { 0x94DC, 0x20 },
497         { 0x94DD, 0x20 },
498         { 0x94DE, 0x20 },
499         { 0x95DC, 0x20 },
500         { 0x95DD, 0x20 },
501         { 0x95DE, 0x20 },
502         { 0x7FB0, 0x00 },
503         { 0x9010, 0x3E },
504         { 0x9419, 0x50 },
505         { 0x941B, 0x50 },
506         { 0x9519, 0x50 },
507         { 0x951B, 0x50 },
508         { 0x3030, 0x00 },
509         { 0x3032, 0x00 },
510         { 0x0220, 0x00 },
511 };
512
513 static const char * const imx258_test_pattern_menu[] = {
514         "Disabled",
515         "Solid Colour",
516         "Eight Vertical Colour Bars",
517         "Colour Bars With Fade to Grey",
518         "Pseudorandom Sequence (PN9)",
519 };
520
521 /* Configurations for supported link frequencies */
522 #define IMX258_LINK_FREQ_634MHZ 633600000ULL
523 #define IMX258_LINK_FREQ_320MHZ 320000000ULL
524
525 enum {
526         IMX258_LINK_FREQ_1267MBPS,
527         IMX258_LINK_FREQ_640MBPS,
528 };
529
530 /*
531  * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
532  * data rate => double data rate; number of lanes => 4; bits per pixel => 10
533  */
534 static u64 link_freq_to_pixel_rate(u64 f)
535 {
536         f *= 2 * 4;
537         do_div(f, 10);
538
539         return f;
540 }
541
542 /* Menu items for LINK_FREQ V4L2 control */
543 static const s64 link_freq_menu_items[] = {
544         IMX258_LINK_FREQ_634MHZ,
545         IMX258_LINK_FREQ_320MHZ,
546 };
547
548 /* Link frequency configs */
549 static const struct imx258_link_freq_config link_freq_configs[] = {
550         [IMX258_LINK_FREQ_1267MBPS] = {
551                 .pixels_per_line = IMX258_PPL_DEFAULT,
552                 .reg_list = {
553                         .num_of_regs = ARRAY_SIZE(mipi_data_rate_1267mbps),
554                         .regs = mipi_data_rate_1267mbps,
555                 }
556         },
557         [IMX258_LINK_FREQ_640MBPS] = {
558                 .pixels_per_line = IMX258_PPL_DEFAULT,
559                 .reg_list = {
560                         .num_of_regs = ARRAY_SIZE(mipi_data_rate_640mbps),
561                         .regs = mipi_data_rate_640mbps,
562                 }
563         },
564 };
565
566 /* Mode configs */
567 static const struct imx258_mode supported_modes[] = {
568         {
569                 .width = 4208,
570                 .height = 3118,
571                 .vts_def = IMX258_VTS_30FPS,
572                 .vts_min = IMX258_VTS_30FPS,
573                 .reg_list = {
574                         .num_of_regs = ARRAY_SIZE(mode_4208x3118_regs),
575                         .regs = mode_4208x3118_regs,
576                 },
577                 .link_freq_index = IMX258_LINK_FREQ_1267MBPS,
578         },
579         {
580                 .width = 2104,
581                 .height = 1560,
582                 .vts_def = IMX258_VTS_30FPS_2K,
583                 .vts_min = IMX258_VTS_30FPS_2K,
584                 .reg_list = {
585                         .num_of_regs = ARRAY_SIZE(mode_2104_1560_regs),
586                         .regs = mode_2104_1560_regs,
587                 },
588                 .link_freq_index = IMX258_LINK_FREQ_640MBPS,
589         },
590         {
591                 .width = 1048,
592                 .height = 780,
593                 .vts_def = IMX258_VTS_30FPS_VGA,
594                 .vts_min = IMX258_VTS_30FPS_VGA,
595                 .reg_list = {
596                         .num_of_regs = ARRAY_SIZE(mode_1048_780_regs),
597                         .regs = mode_1048_780_regs,
598                 },
599                 .link_freq_index = IMX258_LINK_FREQ_640MBPS,
600         },
601 };
602
603 struct imx258 {
604         struct v4l2_subdev sd;
605         struct media_pad pad;
606
607         struct v4l2_ctrl_handler ctrl_handler;
608         /* V4L2 Controls */
609         struct v4l2_ctrl *link_freq;
610         struct v4l2_ctrl *pixel_rate;
611         struct v4l2_ctrl *vblank;
612         struct v4l2_ctrl *hblank;
613         struct v4l2_ctrl *exposure;
614
615         /* Current mode */
616         const struct imx258_mode *cur_mode;
617
618         /*
619          * Mutex for serialized access:
620          * Protect sensor module set pad format and start/stop streaming safely.
621          */
622         struct mutex mutex;
623
624         /* Streaming on/off */
625         bool streaming;
626
627         struct clk *clk;
628 };
629
630 static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
631 {
632         return container_of(_sd, struct imx258, sd);
633 }
634
635 /* Read registers up to 2 at a time */
636 static int imx258_read_reg(struct imx258 *imx258, u16 reg, u32 len, u32 *val)
637 {
638         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
639         struct i2c_msg msgs[2];
640         u8 addr_buf[2] = { reg >> 8, reg & 0xff };
641         u8 data_buf[4] = { 0, };
642         int ret;
643
644         if (len > 4)
645                 return -EINVAL;
646
647         /* Write register address */
648         msgs[0].addr = client->addr;
649         msgs[0].flags = 0;
650         msgs[0].len = ARRAY_SIZE(addr_buf);
651         msgs[0].buf = addr_buf;
652
653         /* Read data from register */
654         msgs[1].addr = client->addr;
655         msgs[1].flags = I2C_M_RD;
656         msgs[1].len = len;
657         msgs[1].buf = &data_buf[4 - len];
658
659         ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
660         if (ret != ARRAY_SIZE(msgs))
661                 return -EIO;
662
663         *val = get_unaligned_be32(data_buf);
664
665         return 0;
666 }
667
668 /* Write registers up to 2 at a time */
669 static int imx258_write_reg(struct imx258 *imx258, u16 reg, u32 len, u32 val)
670 {
671         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
672         u8 buf[6];
673
674         if (len > 4)
675                 return -EINVAL;
676
677         put_unaligned_be16(reg, buf);
678         put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
679         if (i2c_master_send(client, buf, len + 2) != len + 2)
680                 return -EIO;
681
682         return 0;
683 }
684
685 /* Write a list of registers */
686 static int imx258_write_regs(struct imx258 *imx258,
687                              const struct imx258_reg *regs, u32 len)
688 {
689         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
690         unsigned int i;
691         int ret;
692
693         for (i = 0; i < len; i++) {
694                 ret = imx258_write_reg(imx258, regs[i].address, 1,
695                                         regs[i].val);
696                 if (ret) {
697                         dev_err_ratelimited(
698                                 &client->dev,
699                                 "Failed to write reg 0x%4.4x. error = %d\n",
700                                 regs[i].address, ret);
701
702                         return ret;
703                 }
704         }
705
706         return 0;
707 }
708
709 /* Open sub-device */
710 static int imx258_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
711 {
712         struct v4l2_mbus_framefmt *try_fmt =
713                 v4l2_subdev_get_try_format(sd, fh->state, 0);
714
715         /* Initialize try_fmt */
716         try_fmt->width = supported_modes[0].width;
717         try_fmt->height = supported_modes[0].height;
718         try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
719         try_fmt->field = V4L2_FIELD_NONE;
720
721         return 0;
722 }
723
724 static int imx258_update_digital_gain(struct imx258 *imx258, u32 len, u32 val)
725 {
726         int ret;
727
728         ret = imx258_write_reg(imx258, IMX258_REG_GR_DIGITAL_GAIN,
729                                 IMX258_REG_VALUE_16BIT,
730                                 val);
731         if (ret)
732                 return ret;
733         ret = imx258_write_reg(imx258, IMX258_REG_GB_DIGITAL_GAIN,
734                                 IMX258_REG_VALUE_16BIT,
735                                 val);
736         if (ret)
737                 return ret;
738         ret = imx258_write_reg(imx258, IMX258_REG_R_DIGITAL_GAIN,
739                                 IMX258_REG_VALUE_16BIT,
740                                 val);
741         if (ret)
742                 return ret;
743         ret = imx258_write_reg(imx258, IMX258_REG_B_DIGITAL_GAIN,
744                                 IMX258_REG_VALUE_16BIT,
745                                 val);
746         if (ret)
747                 return ret;
748         return 0;
749 }
750
751 static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
752 {
753         struct imx258 *imx258 =
754                 container_of(ctrl->handler, struct imx258, ctrl_handler);
755         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
756         int ret = 0;
757
758         /*
759          * Applying V4L2 control value only happens
760          * when power is up for streaming
761          */
762         if (pm_runtime_get_if_in_use(&client->dev) == 0)
763                 return 0;
764
765         switch (ctrl->id) {
766         case V4L2_CID_ANALOGUE_GAIN:
767                 ret = imx258_write_reg(imx258, IMX258_REG_ANALOG_GAIN,
768                                 IMX258_REG_VALUE_16BIT,
769                                 ctrl->val);
770                 break;
771         case V4L2_CID_EXPOSURE:
772                 ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
773                                 IMX258_REG_VALUE_16BIT,
774                                 ctrl->val);
775                 break;
776         case V4L2_CID_DIGITAL_GAIN:
777                 ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
778                                 ctrl->val);
779                 break;
780         case V4L2_CID_TEST_PATTERN:
781                 ret = imx258_write_reg(imx258, IMX258_REG_TEST_PATTERN,
782                                 IMX258_REG_VALUE_16BIT,
783                                 ctrl->val);
784                 ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
785                                 IMX258_REG_VALUE_08BIT,
786                                 !ctrl->val ? REG_CONFIG_MIRROR_FLIP :
787                                 REG_CONFIG_FLIP_TEST_PATTERN);
788                 break;
789         case V4L2_CID_WIDE_DYNAMIC_RANGE:
790                 if (!ctrl->val) {
791                         ret = imx258_write_reg(imx258, IMX258_REG_HDR,
792                                                IMX258_REG_VALUE_08BIT,
793                                                IMX258_HDR_RATIO_MIN);
794                 } else {
795                         ret = imx258_write_reg(imx258, IMX258_REG_HDR,
796                                                IMX258_REG_VALUE_08BIT,
797                                                IMX258_HDR_ON);
798                         if (ret)
799                                 break;
800                         ret = imx258_write_reg(imx258, IMX258_REG_HDR_RATIO,
801                                                IMX258_REG_VALUE_08BIT,
802                                                BIT(IMX258_HDR_RATIO_MAX));
803                 }
804                 break;
805         default:
806                 dev_info(&client->dev,
807                          "ctrl(id:0x%x,val:0x%x) is not handled\n",
808                          ctrl->id, ctrl->val);
809                 ret = -EINVAL;
810                 break;
811         }
812
813         pm_runtime_put(&client->dev);
814
815         return ret;
816 }
817
818 static const struct v4l2_ctrl_ops imx258_ctrl_ops = {
819         .s_ctrl = imx258_set_ctrl,
820 };
821
822 static int imx258_enum_mbus_code(struct v4l2_subdev *sd,
823                                   struct v4l2_subdev_state *sd_state,
824                                   struct v4l2_subdev_mbus_code_enum *code)
825 {
826         /* Only one bayer order(GRBG) is supported */
827         if (code->index > 0)
828                 return -EINVAL;
829
830         code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
831
832         return 0;
833 }
834
835 static int imx258_enum_frame_size(struct v4l2_subdev *sd,
836                                   struct v4l2_subdev_state *sd_state,
837                                   struct v4l2_subdev_frame_size_enum *fse)
838 {
839         if (fse->index >= ARRAY_SIZE(supported_modes))
840                 return -EINVAL;
841
842         if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
843                 return -EINVAL;
844
845         fse->min_width = supported_modes[fse->index].width;
846         fse->max_width = fse->min_width;
847         fse->min_height = supported_modes[fse->index].height;
848         fse->max_height = fse->min_height;
849
850         return 0;
851 }
852
853 static void imx258_update_pad_format(const struct imx258_mode *mode,
854                                      struct v4l2_subdev_format *fmt)
855 {
856         fmt->format.width = mode->width;
857         fmt->format.height = mode->height;
858         fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
859         fmt->format.field = V4L2_FIELD_NONE;
860 }
861
862 static int __imx258_get_pad_format(struct imx258 *imx258,
863                                    struct v4l2_subdev_state *sd_state,
864                                    struct v4l2_subdev_format *fmt)
865 {
866         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
867                 fmt->format = *v4l2_subdev_get_try_format(&imx258->sd,
868                                                           sd_state,
869                                                           fmt->pad);
870         else
871                 imx258_update_pad_format(imx258->cur_mode, fmt);
872
873         return 0;
874 }
875
876 static int imx258_get_pad_format(struct v4l2_subdev *sd,
877                                  struct v4l2_subdev_state *sd_state,
878                                  struct v4l2_subdev_format *fmt)
879 {
880         struct imx258 *imx258 = to_imx258(sd);
881         int ret;
882
883         mutex_lock(&imx258->mutex);
884         ret = __imx258_get_pad_format(imx258, sd_state, fmt);
885         mutex_unlock(&imx258->mutex);
886
887         return ret;
888 }
889
890 static int imx258_set_pad_format(struct v4l2_subdev *sd,
891                                  struct v4l2_subdev_state *sd_state,
892                                  struct v4l2_subdev_format *fmt)
893 {
894         struct imx258 *imx258 = to_imx258(sd);
895         const struct imx258_mode *mode;
896         struct v4l2_mbus_framefmt *framefmt;
897         s32 vblank_def;
898         s32 vblank_min;
899         s64 h_blank;
900         s64 pixel_rate;
901         s64 link_freq;
902
903         mutex_lock(&imx258->mutex);
904
905         /* Only one raw bayer(GBRG) order is supported */
906         fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
907
908         mode = v4l2_find_nearest_size(supported_modes,
909                 ARRAY_SIZE(supported_modes), width, height,
910                 fmt->format.width, fmt->format.height);
911         imx258_update_pad_format(mode, fmt);
912         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
913                 framefmt = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
914                 *framefmt = fmt->format;
915         } else {
916                 imx258->cur_mode = mode;
917                 __v4l2_ctrl_s_ctrl(imx258->link_freq, mode->link_freq_index);
918
919                 link_freq = link_freq_menu_items[mode->link_freq_index];
920                 pixel_rate = link_freq_to_pixel_rate(link_freq);
921                 __v4l2_ctrl_s_ctrl_int64(imx258->pixel_rate, pixel_rate);
922                 /* Update limits and set FPS to default */
923                 vblank_def = imx258->cur_mode->vts_def -
924                              imx258->cur_mode->height;
925                 vblank_min = imx258->cur_mode->vts_min -
926                              imx258->cur_mode->height;
927                 __v4l2_ctrl_modify_range(
928                         imx258->vblank, vblank_min,
929                         IMX258_VTS_MAX - imx258->cur_mode->height, 1,
930                         vblank_def);
931                 __v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
932                 h_blank =
933                         link_freq_configs[mode->link_freq_index].pixels_per_line
934                          - imx258->cur_mode->width;
935                 __v4l2_ctrl_modify_range(imx258->hblank, h_blank,
936                                          h_blank, 1, h_blank);
937         }
938
939         mutex_unlock(&imx258->mutex);
940
941         return 0;
942 }
943
944 /* Start streaming */
945 static int imx258_start_streaming(struct imx258 *imx258)
946 {
947         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
948         const struct imx258_reg_list *reg_list;
949         int ret, link_freq_index;
950
951         /* Setup PLL */
952         link_freq_index = imx258->cur_mode->link_freq_index;
953         reg_list = &link_freq_configs[link_freq_index].reg_list;
954         ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
955         if (ret) {
956                 dev_err(&client->dev, "%s failed to set plls\n", __func__);
957                 return ret;
958         }
959
960         /* Apply default values of current mode */
961         reg_list = &imx258->cur_mode->reg_list;
962         ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
963         if (ret) {
964                 dev_err(&client->dev, "%s failed to set mode\n", __func__);
965                 return ret;
966         }
967
968         /* Set Orientation be 180 degree */
969         ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
970                                IMX258_REG_VALUE_08BIT, REG_CONFIG_MIRROR_FLIP);
971         if (ret) {
972                 dev_err(&client->dev, "%s failed to set orientation\n",
973                         __func__);
974                 return ret;
975         }
976
977         /* Apply customized values from user */
978         ret =  __v4l2_ctrl_handler_setup(imx258->sd.ctrl_handler);
979         if (ret)
980                 return ret;
981
982         /* set stream on register */
983         return imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
984                                 IMX258_REG_VALUE_08BIT,
985                                 IMX258_MODE_STREAMING);
986 }
987
988 /* Stop streaming */
989 static int imx258_stop_streaming(struct imx258 *imx258)
990 {
991         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
992         int ret;
993
994         /* set stream off register */
995         ret = imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
996                 IMX258_REG_VALUE_08BIT, IMX258_MODE_STANDBY);
997         if (ret)
998                 dev_err(&client->dev, "%s failed to set stream\n", __func__);
999
1000         /*
1001          * Return success even if it was an error, as there is nothing the
1002          * caller can do about it.
1003          */
1004         return 0;
1005 }
1006
1007 static int imx258_power_on(struct device *dev)
1008 {
1009         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1010         struct imx258 *imx258 = to_imx258(sd);
1011         int ret;
1012
1013         ret = clk_prepare_enable(imx258->clk);
1014         if (ret)
1015                 dev_err(dev, "failed to enable clock\n");
1016
1017         return ret;
1018 }
1019
1020 static int imx258_power_off(struct device *dev)
1021 {
1022         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1023         struct imx258 *imx258 = to_imx258(sd);
1024
1025         clk_disable_unprepare(imx258->clk);
1026
1027         return 0;
1028 }
1029
1030 static int imx258_set_stream(struct v4l2_subdev *sd, int enable)
1031 {
1032         struct imx258 *imx258 = to_imx258(sd);
1033         struct i2c_client *client = v4l2_get_subdevdata(sd);
1034         int ret = 0;
1035
1036         mutex_lock(&imx258->mutex);
1037         if (imx258->streaming == enable) {
1038                 mutex_unlock(&imx258->mutex);
1039                 return 0;
1040         }
1041
1042         if (enable) {
1043                 ret = pm_runtime_resume_and_get(&client->dev);
1044                 if (ret < 0)
1045                         goto err_unlock;
1046
1047                 /*
1048                  * Apply default & customized values
1049                  * and then start streaming.
1050                  */
1051                 ret = imx258_start_streaming(imx258);
1052                 if (ret)
1053                         goto err_rpm_put;
1054         } else {
1055                 imx258_stop_streaming(imx258);
1056                 pm_runtime_put(&client->dev);
1057         }
1058
1059         imx258->streaming = enable;
1060         mutex_unlock(&imx258->mutex);
1061
1062         return ret;
1063
1064 err_rpm_put:
1065         pm_runtime_put(&client->dev);
1066 err_unlock:
1067         mutex_unlock(&imx258->mutex);
1068
1069         return ret;
1070 }
1071
1072 static int __maybe_unused imx258_suspend(struct device *dev)
1073 {
1074         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1075         struct imx258 *imx258 = to_imx258(sd);
1076
1077         if (imx258->streaming)
1078                 imx258_stop_streaming(imx258);
1079
1080         return 0;
1081 }
1082
1083 static int __maybe_unused imx258_resume(struct device *dev)
1084 {
1085         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1086         struct imx258 *imx258 = to_imx258(sd);
1087         int ret;
1088
1089         if (imx258->streaming) {
1090                 ret = imx258_start_streaming(imx258);
1091                 if (ret)
1092                         goto error;
1093         }
1094
1095         return 0;
1096
1097 error:
1098         imx258_stop_streaming(imx258);
1099         imx258->streaming = 0;
1100         return ret;
1101 }
1102
1103 /* Verify chip ID */
1104 static int imx258_identify_module(struct imx258 *imx258)
1105 {
1106         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1107         int ret;
1108         u32 val;
1109
1110         ret = imx258_read_reg(imx258, IMX258_REG_CHIP_ID,
1111                               IMX258_REG_VALUE_16BIT, &val);
1112         if (ret) {
1113                 dev_err(&client->dev, "failed to read chip id %x\n",
1114                         IMX258_CHIP_ID);
1115                 return ret;
1116         }
1117
1118         if (val != IMX258_CHIP_ID) {
1119                 dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
1120                         IMX258_CHIP_ID, val);
1121                 return -EIO;
1122         }
1123
1124         return 0;
1125 }
1126
1127 static const struct v4l2_subdev_video_ops imx258_video_ops = {
1128         .s_stream = imx258_set_stream,
1129 };
1130
1131 static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
1132         .enum_mbus_code = imx258_enum_mbus_code,
1133         .get_fmt = imx258_get_pad_format,
1134         .set_fmt = imx258_set_pad_format,
1135         .enum_frame_size = imx258_enum_frame_size,
1136 };
1137
1138 static const struct v4l2_subdev_ops imx258_subdev_ops = {
1139         .video = &imx258_video_ops,
1140         .pad = &imx258_pad_ops,
1141 };
1142
1143 static const struct v4l2_subdev_internal_ops imx258_internal_ops = {
1144         .open = imx258_open,
1145 };
1146
1147 /* Initialize control handlers */
1148 static int imx258_init_controls(struct imx258 *imx258)
1149 {
1150         struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
1151         struct v4l2_ctrl_handler *ctrl_hdlr;
1152         s64 vblank_def;
1153         s64 vblank_min;
1154         s64 pixel_rate_min;
1155         s64 pixel_rate_max;
1156         int ret;
1157
1158         ctrl_hdlr = &imx258->ctrl_handler;
1159         ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
1160         if (ret)
1161                 return ret;
1162
1163         mutex_init(&imx258->mutex);
1164         ctrl_hdlr->lock = &imx258->mutex;
1165         imx258->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
1166                                 &imx258_ctrl_ops,
1167                                 V4L2_CID_LINK_FREQ,
1168                                 ARRAY_SIZE(link_freq_menu_items) - 1,
1169                                 0,
1170                                 link_freq_menu_items);
1171
1172         if (imx258->link_freq)
1173                 imx258->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1174
1175         pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
1176         pixel_rate_min = link_freq_to_pixel_rate(link_freq_menu_items[1]);
1177         /* By default, PIXEL_RATE is read only */
1178         imx258->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
1179                                 V4L2_CID_PIXEL_RATE,
1180                                 pixel_rate_min, pixel_rate_max,
1181                                 1, pixel_rate_max);
1182
1183
1184         vblank_def = imx258->cur_mode->vts_def - imx258->cur_mode->height;
1185         vblank_min = imx258->cur_mode->vts_min - imx258->cur_mode->height;
1186         imx258->vblank = v4l2_ctrl_new_std(
1187                                 ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_VBLANK,
1188                                 vblank_min,
1189                                 IMX258_VTS_MAX - imx258->cur_mode->height, 1,
1190                                 vblank_def);
1191
1192         if (imx258->vblank)
1193                 imx258->vblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1194
1195         imx258->hblank = v4l2_ctrl_new_std(
1196                                 ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_HBLANK,
1197                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width,
1198                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width,
1199                                 1,
1200                                 IMX258_PPL_DEFAULT - imx258->cur_mode->width);
1201
1202         if (imx258->hblank)
1203                 imx258->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1204
1205         imx258->exposure = v4l2_ctrl_new_std(
1206                                 ctrl_hdlr, &imx258_ctrl_ops,
1207                                 V4L2_CID_EXPOSURE, IMX258_EXPOSURE_MIN,
1208                                 IMX258_EXPOSURE_MAX, IMX258_EXPOSURE_STEP,
1209                                 IMX258_EXPOSURE_DEFAULT);
1210
1211         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
1212                                 IMX258_ANA_GAIN_MIN, IMX258_ANA_GAIN_MAX,
1213                                 IMX258_ANA_GAIN_STEP, IMX258_ANA_GAIN_DEFAULT);
1214
1215         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
1216                                 IMX258_DGTL_GAIN_MIN, IMX258_DGTL_GAIN_MAX,
1217                                 IMX258_DGTL_GAIN_STEP,
1218                                 IMX258_DGTL_GAIN_DEFAULT);
1219
1220         v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_WIDE_DYNAMIC_RANGE,
1221                                 0, 1, 1, IMX258_HDR_RATIO_DEFAULT);
1222
1223         v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx258_ctrl_ops,
1224                                 V4L2_CID_TEST_PATTERN,
1225                                 ARRAY_SIZE(imx258_test_pattern_menu) - 1,
1226                                 0, 0, imx258_test_pattern_menu);
1227
1228         if (ctrl_hdlr->error) {
1229                 ret = ctrl_hdlr->error;
1230                 dev_err(&client->dev, "%s control init failed (%d)\n",
1231                                 __func__, ret);
1232                 goto error;
1233         }
1234
1235         imx258->sd.ctrl_handler = ctrl_hdlr;
1236
1237         return 0;
1238
1239 error:
1240         v4l2_ctrl_handler_free(ctrl_hdlr);
1241         mutex_destroy(&imx258->mutex);
1242
1243         return ret;
1244 }
1245
1246 static void imx258_free_controls(struct imx258 *imx258)
1247 {
1248         v4l2_ctrl_handler_free(imx258->sd.ctrl_handler);
1249         mutex_destroy(&imx258->mutex);
1250 }
1251
1252 static int imx258_probe(struct i2c_client *client)
1253 {
1254         struct imx258 *imx258;
1255         int ret;
1256         u32 val = 0;
1257
1258         imx258 = devm_kzalloc(&client->dev, sizeof(*imx258), GFP_KERNEL);
1259         if (!imx258)
1260                 return -ENOMEM;
1261
1262         imx258->clk = devm_clk_get_optional(&client->dev, NULL);
1263         if (IS_ERR(imx258->clk))
1264                 return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
1265                                      "error getting clock\n");
1266         if (!imx258->clk) {
1267                 dev_dbg(&client->dev,
1268                         "no clock provided, using clock-frequency property\n");
1269
1270                 device_property_read_u32(&client->dev, "clock-frequency", &val);
1271         } else {
1272                 val = clk_get_rate(imx258->clk);
1273         }
1274         if (val != IMX258_INPUT_CLOCK_FREQ) {
1275                 dev_err(&client->dev, "input clock frequency not supported\n");
1276                 return -EINVAL;
1277         }
1278
1279         /*
1280          * Check that the device is mounted upside down. The driver only
1281          * supports a single pixel order right now.
1282          */
1283         ret = device_property_read_u32(&client->dev, "rotation", &val);
1284         if (ret || val != 180)
1285                 return -EINVAL;
1286
1287         /* Initialize subdev */
1288         v4l2_i2c_subdev_init(&imx258->sd, client, &imx258_subdev_ops);
1289
1290         /* Will be powered off via pm_runtime_idle */
1291         ret = imx258_power_on(&client->dev);
1292         if (ret)
1293                 return ret;
1294
1295         /* Check module identity */
1296         ret = imx258_identify_module(imx258);
1297         if (ret)
1298                 goto error_identify;
1299
1300         /* Set default mode to max resolution */
1301         imx258->cur_mode = &supported_modes[0];
1302
1303         ret = imx258_init_controls(imx258);
1304         if (ret)
1305                 goto error_identify;
1306
1307         /* Initialize subdev */
1308         imx258->sd.internal_ops = &imx258_internal_ops;
1309         imx258->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1310         imx258->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1311
1312         /* Initialize source pad */
1313         imx258->pad.flags = MEDIA_PAD_FL_SOURCE;
1314
1315         ret = media_entity_pads_init(&imx258->sd.entity, 1, &imx258->pad);
1316         if (ret)
1317                 goto error_handler_free;
1318
1319         ret = v4l2_async_register_subdev_sensor(&imx258->sd);
1320         if (ret < 0)
1321                 goto error_media_entity;
1322
1323         pm_runtime_set_active(&client->dev);
1324         pm_runtime_enable(&client->dev);
1325         pm_runtime_idle(&client->dev);
1326
1327         return 0;
1328
1329 error_media_entity:
1330         media_entity_cleanup(&imx258->sd.entity);
1331
1332 error_handler_free:
1333         imx258_free_controls(imx258);
1334
1335 error_identify:
1336         imx258_power_off(&client->dev);
1337
1338         return ret;
1339 }
1340
1341 static int imx258_remove(struct i2c_client *client)
1342 {
1343         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1344         struct imx258 *imx258 = to_imx258(sd);
1345
1346         v4l2_async_unregister_subdev(sd);
1347         media_entity_cleanup(&sd->entity);
1348         imx258_free_controls(imx258);
1349
1350         pm_runtime_disable(&client->dev);
1351         if (!pm_runtime_status_suspended(&client->dev))
1352                 imx258_power_off(&client->dev);
1353         pm_runtime_set_suspended(&client->dev);
1354
1355         return 0;
1356 }
1357
1358 static const struct dev_pm_ops imx258_pm_ops = {
1359         SET_SYSTEM_SLEEP_PM_OPS(imx258_suspend, imx258_resume)
1360         SET_RUNTIME_PM_OPS(imx258_power_off, imx258_power_on, NULL)
1361 };
1362
1363 #ifdef CONFIG_ACPI
1364 static const struct acpi_device_id imx258_acpi_ids[] = {
1365         { "SONY258A" },
1366         { /* sentinel */ }
1367 };
1368
1369 MODULE_DEVICE_TABLE(acpi, imx258_acpi_ids);
1370 #endif
1371
1372 static const struct of_device_id imx258_dt_ids[] = {
1373         { .compatible = "sony,imx258" },
1374         { /* sentinel */ }
1375 };
1376 MODULE_DEVICE_TABLE(of, imx258_dt_ids);
1377
1378 static struct i2c_driver imx258_i2c_driver = {
1379         .driver = {
1380                 .name = "imx258",
1381                 .pm = &imx258_pm_ops,
1382                 .acpi_match_table = ACPI_PTR(imx258_acpi_ids),
1383                 .of_match_table = imx258_dt_ids,
1384         },
1385         .probe_new = imx258_probe,
1386         .remove = imx258_remove,
1387 };
1388
1389 module_i2c_driver(imx258_i2c_driver);
1390
1391 MODULE_AUTHOR("Yeh, Andy <andy.yeh@intel.com>");
1392 MODULE_AUTHOR("Chiang, Alan");
1393 MODULE_AUTHOR("Chen, Jason");
1394 MODULE_DESCRIPTION("Sony IMX258 sensor driver");
1395 MODULE_LICENSE("GPL v2");