xref: /openbmc/linux/drivers/media/i2c/s5k5baf.c (revision 11976fe2)
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Driver for Samsung S5K5BAF UXGA 1/5" 2M CMOS Image Sensor
4  * with embedded SoC ISP.
5  *
6  * Copyright (C) 2013, Samsung Electronics Co., Ltd.
7  * Andrzej Hajda <a.hajda@samsung.com>
8  *
9  * Based on S5K6AA driver authored by Sylwester Nawrocki
10  * Copyright (C) 2013, Samsung Electronics Co., Ltd.
11  */
12 
13 #include <linux/clk.h>
14 #include <linux/delay.h>
15 #include <linux/firmware.h>
16 #include <linux/gpio/consumer.h>
17 #include <linux/i2c.h>
18 #include <linux/media.h>
19 #include <linux/module.h>
20 #include <linux/of_graph.h>
21 #include <linux/regulator/consumer.h>
22 #include <linux/slab.h>
23 
24 #include <media/media-entity.h>
25 #include <media/v4l2-ctrls.h>
26 #include <media/v4l2-device.h>
27 #include <media/v4l2-subdev.h>
28 #include <media/v4l2-mediabus.h>
29 #include <media/v4l2-fwnode.h>
30 
31 static int debug;
32 module_param(debug, int, 0644);
33 
34 #define S5K5BAF_DRIVER_NAME		"s5k5baf"
35 #define S5K5BAF_DEFAULT_MCLK_FREQ	24000000U
36 #define S5K5BAF_CLK_NAME		"mclk"
37 
38 #define S5K5BAF_FW_FILENAME		"s5k5baf-cfg.bin"
39 #define S5K5BAF_FW_TAG			"SF00"
40 #define S5K5BAG_FW_TAG_LEN		2
41 #define S5K5BAG_FW_MAX_COUNT		16
42 
43 #define S5K5BAF_CIS_WIDTH		1600
44 #define S5K5BAF_CIS_HEIGHT		1200
45 #define S5K5BAF_WIN_WIDTH_MIN		8
46 #define S5K5BAF_WIN_HEIGHT_MIN		8
47 #define S5K5BAF_GAIN_RED_DEF		127
48 #define S5K5BAF_GAIN_GREEN_DEF		95
49 #define S5K5BAF_GAIN_BLUE_DEF		180
50 /* Default number of MIPI CSI-2 data lanes used */
51 #define S5K5BAF_DEF_NUM_LANES		1
52 
53 #define AHB_MSB_ADDR_PTR		0xfcfc
54 
55 /*
56  * Register interface pages (the most significant word of the address)
57  */
58 #define PAGE_IF_HW			0xd000
59 #define PAGE_IF_SW			0x7000
60 
61 /*
62  * H/W register Interface (PAGE_IF_HW)
63  */
64 #define REG_SW_LOAD_COMPLETE		0x0014
65 #define REG_CMDWR_PAGE			0x0028
66 #define REG_CMDWR_ADDR			0x002a
67 #define REG_CMDRD_PAGE			0x002c
68 #define REG_CMDRD_ADDR			0x002e
69 #define REG_CMD_BUF			0x0f12
70 #define REG_SET_HOST_INT		0x1000
71 #define REG_CLEAR_HOST_INT		0x1030
72 #define REG_PATTERN_SET			0x3100
73 #define REG_PATTERN_WIDTH		0x3118
74 #define REG_PATTERN_HEIGHT		0x311a
75 #define REG_PATTERN_PARAM		0x311c
76 
77 /*
78  * S/W register interface (PAGE_IF_SW)
79  */
80 
81 /* Firmware revision information */
82 #define REG_FW_APIVER			0x012e
83 #define  S5K5BAF_FW_APIVER		0x0001
84 #define REG_FW_REVISION			0x0130
85 #define REG_FW_SENSOR_ID		0x0152
86 
87 /* Initialization parameters */
88 /* Master clock frequency in KHz */
89 #define REG_I_INCLK_FREQ_L		0x01b8
90 #define REG_I_INCLK_FREQ_H		0x01ba
91 #define  MIN_MCLK_FREQ_KHZ		6000U
92 #define  MAX_MCLK_FREQ_KHZ		48000U
93 #define REG_I_USE_NPVI_CLOCKS		0x01c6
94 #define  NPVI_CLOCKS			1
95 #define REG_I_USE_NMIPI_CLOCKS		0x01c8
96 #define  NMIPI_CLOCKS			1
97 #define REG_I_BLOCK_INTERNAL_PLL_CALC	0x01ca
98 
99 /* Clock configurations, n = 0..2. REG_I_* frequency unit is 4 kHz. */
100 #define REG_I_OPCLK_4KHZ(n)		((n) * 6 + 0x01cc)
101 #define REG_I_MIN_OUTRATE_4KHZ(n)	((n) * 6 + 0x01ce)
102 #define REG_I_MAX_OUTRATE_4KHZ(n)	((n) * 6 + 0x01d0)
103 #define  SCLK_PVI_FREQ			24000
104 #define  SCLK_MIPI_FREQ			48000
105 #define  PCLK_MIN_FREQ			6000
106 #define  PCLK_MAX_FREQ			48000
107 #define REG_I_USE_REGS_API		0x01de
108 #define REG_I_INIT_PARAMS_UPDATED	0x01e0
109 #define REG_I_ERROR_INFO		0x01e2
110 
111 /* General purpose parameters */
112 #define REG_USER_BRIGHTNESS		0x01e4
113 #define REG_USER_CONTRAST		0x01e6
114 #define REG_USER_SATURATION		0x01e8
115 #define REG_USER_SHARPBLUR		0x01ea
116 
117 #define REG_G_SPEC_EFFECTS		0x01ee
118 #define REG_G_ENABLE_PREV		0x01f0
119 #define REG_G_ENABLE_PREV_CHG		0x01f2
120 #define REG_G_NEW_CFG_SYNC		0x01f8
121 #define REG_G_PREVREQ_IN_WIDTH		0x01fa
122 #define REG_G_PREVREQ_IN_HEIGHT		0x01fc
123 #define REG_G_PREVREQ_IN_XOFFS		0x01fe
124 #define REG_G_PREVREQ_IN_YOFFS		0x0200
125 #define REG_G_PREVZOOM_IN_WIDTH		0x020a
126 #define REG_G_PREVZOOM_IN_HEIGHT	0x020c
127 #define REG_G_PREVZOOM_IN_XOFFS		0x020e
128 #define REG_G_PREVZOOM_IN_YOFFS		0x0210
129 #define REG_G_INPUTS_CHANGE_REQ		0x021a
130 #define REG_G_ACTIVE_PREV_CFG		0x021c
131 #define REG_G_PREV_CFG_CHG		0x021e
132 #define REG_G_PREV_OPEN_AFTER_CH	0x0220
133 #define REG_G_PREV_CFG_ERROR		0x0222
134 #define  CFG_ERROR_RANGE		0x0b
135 #define REG_G_PREV_CFG_BYPASS_CHANGED	0x022a
136 #define REG_G_ACTUAL_P_FR_TIME		0x023a
137 #define REG_G_ACTUAL_P_OUT_RATE		0x023c
138 #define REG_G_ACTUAL_C_FR_TIME		0x023e
139 #define REG_G_ACTUAL_C_OUT_RATE		0x0240
140 
141 /* Preview control section. n = 0...4. */
142 #define PREG(n, x)			((n) * 0x26 + x)
143 #define REG_P_OUT_WIDTH(n)		PREG(n, 0x0242)
144 #define REG_P_OUT_HEIGHT(n)		PREG(n, 0x0244)
145 #define REG_P_FMT(n)			PREG(n, 0x0246)
146 #define REG_P_MAX_OUT_RATE(n)		PREG(n, 0x0248)
147 #define REG_P_MIN_OUT_RATE(n)		PREG(n, 0x024a)
148 #define REG_P_PVI_MASK(n)		PREG(n, 0x024c)
149 #define  PVI_MASK_MIPI			0x52
150 #define REG_P_CLK_INDEX(n)		PREG(n, 0x024e)
151 #define  CLK_PVI_INDEX			0
152 #define  CLK_MIPI_INDEX			NPVI_CLOCKS
153 #define REG_P_FR_RATE_TYPE(n)		PREG(n, 0x0250)
154 #define  FR_RATE_DYNAMIC		0
155 #define  FR_RATE_FIXED			1
156 #define  FR_RATE_FIXED_ACCURATE		2
157 #define REG_P_FR_RATE_Q_TYPE(n)		PREG(n, 0x0252)
158 #define  FR_RATE_Q_DYNAMIC		0
159 #define  FR_RATE_Q_BEST_FRRATE		1 /* Binning enabled */
160 #define  FR_RATE_Q_BEST_QUALITY		2 /* Binning disabled */
161 /* Frame period in 0.1 ms units */
162 #define REG_P_MAX_FR_TIME(n)		PREG(n, 0x0254)
163 #define REG_P_MIN_FR_TIME(n)		PREG(n, 0x0256)
164 #define  S5K5BAF_MIN_FR_TIME		333  /* x100 us */
165 #define  S5K5BAF_MAX_FR_TIME		6500 /* x100 us */
166 /* The below 5 registers are for "device correction" values */
167 #define REG_P_SATURATION(n)		PREG(n, 0x0258)
168 #define REG_P_SHARP_BLUR(n)		PREG(n, 0x025a)
169 #define REG_P_GLAMOUR(n)		PREG(n, 0x025c)
170 #define REG_P_COLORTEMP(n)		PREG(n, 0x025e)
171 #define REG_P_GAMMA_INDEX(n)		PREG(n, 0x0260)
172 #define REG_P_PREV_MIRROR(n)		PREG(n, 0x0262)
173 #define REG_P_CAP_MIRROR(n)		PREG(n, 0x0264)
174 #define REG_P_CAP_ROTATION(n)		PREG(n, 0x0266)
175 
176 /* Extended image property controls */
177 /* Exposure time in 10 us units */
178 #define REG_SF_USR_EXPOSURE_L		0x03bc
179 #define REG_SF_USR_EXPOSURE_H		0x03be
180 #define REG_SF_USR_EXPOSURE_CHG		0x03c0
181 #define REG_SF_USR_TOT_GAIN		0x03c2
182 #define REG_SF_USR_TOT_GAIN_CHG		0x03c4
183 #define REG_SF_RGAIN			0x03c6
184 #define REG_SF_RGAIN_CHG		0x03c8
185 #define REG_SF_GGAIN			0x03ca
186 #define REG_SF_GGAIN_CHG		0x03cc
187 #define REG_SF_BGAIN			0x03ce
188 #define REG_SF_BGAIN_CHG		0x03d0
189 #define REG_SF_WBGAIN_CHG		0x03d2
190 #define REG_SF_FLICKER_QUANT		0x03d4
191 #define REG_SF_FLICKER_QUANT_CHG	0x03d6
192 
193 /* Output interface (parallel/MIPI) setup */
194 #define REG_OIF_EN_MIPI_LANES		0x03f2
195 #define REG_OIF_EN_PACKETS		0x03f4
196 #define  EN_PACKETS_CSI2		0xc3
197 #define REG_OIF_CFG_CHG			0x03f6
198 
199 /* Auto-algorithms enable mask */
200 #define REG_DBG_AUTOALG_EN		0x03f8
201 #define  AALG_ALL_EN			BIT(0)
202 #define  AALG_AE_EN			BIT(1)
203 #define  AALG_DIVLEI_EN			BIT(2)
204 #define  AALG_WB_EN			BIT(3)
205 #define  AALG_USE_WB_FOR_ISP		BIT(4)
206 #define  AALG_FLICKER_EN		BIT(5)
207 #define  AALG_FIT_EN			BIT(6)
208 #define  AALG_WRHW_EN			BIT(7)
209 
210 /* Pointers to color correction matrices */
211 #define REG_PTR_CCM_HORIZON		0x06d0
212 #define REG_PTR_CCM_INCANDESCENT	0x06d4
213 #define REG_PTR_CCM_WARM_WHITE		0x06d8
214 #define REG_PTR_CCM_COOL_WHITE		0x06dc
215 #define REG_PTR_CCM_DL50		0x06e0
216 #define REG_PTR_CCM_DL65		0x06e4
217 #define REG_PTR_CCM_OUTDOOR		0x06ec
218 
219 #define REG_ARR_CCM(n)			(0x2800 + 36 * (n))
220 
221 static const char * const s5k5baf_supply_names[] = {
222 	"vdda",		/* Analog power supply 2.8V (2.6V to 3.0V) */
223 	"vddreg",	/* Regulator input power supply 1.8V (1.7V to 1.9V)
224 			   or 2.8V (2.6V to 3.0) */
225 	"vddio",	/* I/O power supply 1.8V (1.65V to 1.95V)
226 			   or 2.8V (2.5V to 3.1V) */
227 };
228 #define S5K5BAF_NUM_SUPPLIES ARRAY_SIZE(s5k5baf_supply_names)
229 
230 enum s5k5baf_gpio_id {
231 	STBY,
232 	RSET,
233 	NUM_GPIOS,
234 };
235 
236 #define PAD_CIS 0
237 #define PAD_OUT 1
238 #define NUM_CIS_PADS 1
239 #define NUM_ISP_PADS 2
240 
241 struct s5k5baf_pixfmt {
242 	u32 code;
243 	u32 colorspace;
244 	/* REG_P_FMT(x) register value */
245 	u16 reg_p_fmt;
246 };
247 
248 struct s5k5baf_ctrls {
249 	struct v4l2_ctrl_handler handler;
250 	struct { /* Auto / manual white balance cluster */
251 		struct v4l2_ctrl *awb;
252 		struct v4l2_ctrl *gain_red;
253 		struct v4l2_ctrl *gain_blue;
254 	};
255 	struct { /* Mirror cluster */
256 		struct v4l2_ctrl *hflip;
257 		struct v4l2_ctrl *vflip;
258 	};
259 	struct { /* Auto exposure / manual exposure and gain cluster */
260 		struct v4l2_ctrl *auto_exp;
261 		struct v4l2_ctrl *exposure;
262 		struct v4l2_ctrl *gain;
263 	};
264 };
265 
266 enum {
267 	S5K5BAF_FW_ID_PATCH,
268 	S5K5BAF_FW_ID_CCM,
269 	S5K5BAF_FW_ID_CIS,
270 };
271 
272 struct s5k5baf_fw {
273 	u16 count;
274 	struct {
275 		u16 id;
276 		u16 offset;
277 	} seq[];
278 };
279 
280 struct s5k5baf {
281 	struct gpio_desc *gpios[NUM_GPIOS];
282 	enum v4l2_mbus_type bus_type;
283 	u8 nlanes;
284 	struct regulator_bulk_data supplies[S5K5BAF_NUM_SUPPLIES];
285 
286 	struct clk *clock;
287 	u32 mclk_frequency;
288 
289 	struct s5k5baf_fw *fw;
290 
291 	struct v4l2_subdev cis_sd;
292 	struct media_pad cis_pad;
293 
294 	struct v4l2_subdev sd;
295 	struct media_pad pads[NUM_ISP_PADS];
296 
297 	/* protects the struct members below */
298 	struct mutex lock;
299 
300 	int error;
301 
302 	struct v4l2_rect crop_sink;
303 	struct v4l2_rect compose;
304 	struct v4l2_rect crop_source;
305 	/* index to s5k5baf_formats array */
306 	int pixfmt;
307 	/* actual frame interval in 100us */
308 	u16 fiv;
309 	/* requested frame interval in 100us */
310 	u16 req_fiv;
311 	/* cache for REG_DBG_AUTOALG_EN register */
312 	u16 auto_alg;
313 
314 	struct s5k5baf_ctrls ctrls;
315 
316 	unsigned int streaming:1;
317 	unsigned int apply_cfg:1;
318 	unsigned int apply_crop:1;
319 	unsigned int valid_auto_alg:1;
320 	unsigned int power;
321 };
322 
323 static const struct s5k5baf_pixfmt s5k5baf_formats[] = {
324 	{ MEDIA_BUS_FMT_VYUY8_2X8,	V4L2_COLORSPACE_JPEG,	5 },
325 	/* range 16-240 */
326 	{ MEDIA_BUS_FMT_VYUY8_2X8,	V4L2_COLORSPACE_REC709,	6 },
327 	{ MEDIA_BUS_FMT_RGB565_2X8_BE,	V4L2_COLORSPACE_JPEG,	0 },
328 };
329 
330 static struct v4l2_rect s5k5baf_cis_rect = {
331 	0, 0, S5K5BAF_CIS_WIDTH, S5K5BAF_CIS_HEIGHT
332 };
333 
334 /* Setfile contains set of I2C command sequences. Each sequence has its ID.
335  * setfile format:
336  *	u8 magic[4];
337  *	u16 count;		number of sequences
338  *	struct {
339  *		u16 id;		sequence id
340  *		u16 offset;	sequence offset in data array
341  *	} seq[count];
342  *	u16 data[*];		array containing sequences
343  *
344  */
345 static int s5k5baf_fw_parse(struct device *dev, struct s5k5baf_fw **fw,
346 			    size_t count, const __le16 *data)
347 {
348 	struct s5k5baf_fw *f;
349 	u16 *d, i, *end;
350 	int ret;
351 
352 	if (count < S5K5BAG_FW_TAG_LEN + 1) {
353 		dev_err(dev, "firmware file too short (%zu)\n", count);
354 		return -EINVAL;
355 	}
356 
357 	ret = memcmp(data, S5K5BAF_FW_TAG, S5K5BAG_FW_TAG_LEN * sizeof(u16));
358 	if (ret != 0) {
359 		dev_err(dev, "invalid firmware magic number\n");
360 		return -EINVAL;
361 	}
362 
363 	data += S5K5BAG_FW_TAG_LEN;
364 	count -= S5K5BAG_FW_TAG_LEN;
365 
366 	d = devm_kcalloc(dev, count, sizeof(u16), GFP_KERNEL);
367 	if (!d)
368 		return -ENOMEM;
369 
370 	for (i = 0; i < count; ++i)
371 		d[i] = le16_to_cpu(data[i]);
372 
373 	f = (struct s5k5baf_fw *)d;
374 	if (count < 1 + 2 * f->count) {
375 		dev_err(dev, "invalid firmware header (count=%d size=%zu)\n",
376 			f->count, 2 * (count + S5K5BAG_FW_TAG_LEN));
377 		return -EINVAL;
378 	}
379 	end = d + count;
380 	d += 1 + 2 * f->count;
381 
382 	for (i = 0; i < f->count; ++i) {
383 		if (f->seq[i].offset + d <= end)
384 			continue;
385 		dev_err(dev, "invalid firmware header (seq=%d)\n", i);
386 		return -EINVAL;
387 	}
388 
389 	*fw = f;
390 
391 	return 0;
392 }
393 
394 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
395 {
396 	return &container_of(ctrl->handler, struct s5k5baf, ctrls.handler)->sd;
397 }
398 
399 static inline bool s5k5baf_is_cis_subdev(struct v4l2_subdev *sd)
400 {
401 	return sd->entity.function == MEDIA_ENT_F_CAM_SENSOR;
402 }
403 
404 static inline struct s5k5baf *to_s5k5baf(struct v4l2_subdev *sd)
405 {
406 	if (s5k5baf_is_cis_subdev(sd))
407 		return container_of(sd, struct s5k5baf, cis_sd);
408 	else
409 		return container_of(sd, struct s5k5baf, sd);
410 }
411 
412 static u16 s5k5baf_i2c_read(struct s5k5baf *state, u16 addr)
413 {
414 	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
415 	__be16 w, r;
416 	u16 res;
417 	struct i2c_msg msg[] = {
418 		{ .addr = c->addr, .flags = 0,
419 		  .len = 2, .buf = (u8 *)&w },
420 		{ .addr = c->addr, .flags = I2C_M_RD,
421 		  .len = 2, .buf = (u8 *)&r },
422 	};
423 	int ret;
424 
425 	if (state->error)
426 		return 0;
427 
428 	w = cpu_to_be16(addr);
429 	ret = i2c_transfer(c->adapter, msg, 2);
430 	res = be16_to_cpu(r);
431 
432 	v4l2_dbg(3, debug, c, "i2c_read: 0x%04x : 0x%04x\n", addr, res);
433 
434 	if (ret != 2) {
435 		v4l2_err(c, "i2c_read: error during transfer (%d)\n", ret);
436 		state->error = ret;
437 	}
438 	return res;
439 }
440 
441 static void s5k5baf_i2c_write(struct s5k5baf *state, u16 addr, u16 val)
442 {
443 	u8 buf[4] = { addr >> 8, addr & 0xFF, val >> 8, val & 0xFF };
444 	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
445 	int ret;
446 
447 	if (state->error)
448 		return;
449 
450 	ret = i2c_master_send(c, buf, 4);
451 	v4l2_dbg(3, debug, c, "i2c_write: 0x%04x : 0x%04x\n", addr, val);
452 
453 	if (ret != 4) {
454 		v4l2_err(c, "i2c_write: error during transfer (%d)\n", ret);
455 		state->error = ret;
456 	}
457 }
458 
459 static u16 s5k5baf_read(struct s5k5baf *state, u16 addr)
460 {
461 	s5k5baf_i2c_write(state, REG_CMDRD_ADDR, addr);
462 	return s5k5baf_i2c_read(state, REG_CMD_BUF);
463 }
464 
465 static void s5k5baf_write(struct s5k5baf *state, u16 addr, u16 val)
466 {
467 	s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
468 	s5k5baf_i2c_write(state, REG_CMD_BUF, val);
469 }
470 
471 static void s5k5baf_write_arr_seq(struct s5k5baf *state, u16 addr,
472 				  u16 count, const u16 *seq)
473 {
474 	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
475 	__be16 buf[65];
476 
477 	s5k5baf_i2c_write(state, REG_CMDWR_ADDR, addr);
478 	if (state->error)
479 		return;
480 
481 	v4l2_dbg(3, debug, c, "i2c_write_seq(count=%d): %*ph\n", count,
482 		 min(2 * count, 64), seq);
483 
484 	buf[0] = cpu_to_be16(REG_CMD_BUF);
485 
486 	while (count > 0) {
487 		int n = min_t(int, count, ARRAY_SIZE(buf) - 1);
488 		int ret, i;
489 
490 		for (i = 1; i <= n; ++i)
491 			buf[i] = cpu_to_be16(*seq++);
492 
493 		i *= 2;
494 		ret = i2c_master_send(c, (char *)buf, i);
495 		if (ret != i) {
496 			v4l2_err(c, "i2c_write_seq: error during transfer (%d)\n", ret);
497 			state->error = ret;
498 			break;
499 		}
500 
501 		count -= n;
502 	}
503 }
504 
505 #define s5k5baf_write_seq(state, addr, seq...) \
506 	s5k5baf_write_arr_seq(state, addr, sizeof((char[]){ seq }), \
507 			      (const u16 []){ seq })
508 
509 /* add items count at the beginning of the list */
510 #define NSEQ(seq...) sizeof((char[]){ seq }), seq
511 
512 /*
513  * s5k5baf_write_nseq() - Writes sequences of values to sensor memory via i2c
514  * @nseq: sequence of u16 words in format:
515  *	(N, address, value[1]...value[N-1])*,0
516  * Ex.:
517  *	u16 seq[] = { NSEQ(0x4000, 1, 1), NSEQ(0x4010, 640, 480), 0 };
518  *	ret = s5k5baf_write_nseq(c, seq);
519  */
520 static void s5k5baf_write_nseq(struct s5k5baf *state, const u16 *nseq)
521 {
522 	int count;
523 
524 	while ((count = *nseq++)) {
525 		u16 addr = *nseq++;
526 		--count;
527 
528 		s5k5baf_write_arr_seq(state, addr, count, nseq);
529 		nseq += count;
530 	}
531 }
532 
533 static void s5k5baf_synchronize(struct s5k5baf *state, int timeout, u16 addr)
534 {
535 	unsigned long end = jiffies + msecs_to_jiffies(timeout);
536 	u16 reg;
537 
538 	s5k5baf_write(state, addr, 1);
539 	do {
540 		reg = s5k5baf_read(state, addr);
541 		if (state->error || !reg)
542 			return;
543 		usleep_range(5000, 10000);
544 	} while (time_is_after_jiffies(end));
545 
546 	v4l2_err(&state->sd, "timeout on register synchronize (%#x)\n", addr);
547 	state->error = -ETIMEDOUT;
548 }
549 
550 static u16 *s5k5baf_fw_get_seq(struct s5k5baf *state, u16 seq_id)
551 {
552 	struct s5k5baf_fw *fw = state->fw;
553 	u16 *data;
554 	int i;
555 
556 	if (fw == NULL)
557 		return NULL;
558 
559 	data = &fw->seq[0].id + 2 * fw->count;
560 
561 	for (i = 0; i < fw->count; ++i) {
562 		if (fw->seq[i].id == seq_id)
563 			return data + fw->seq[i].offset;
564 	}
565 
566 	return NULL;
567 }
568 
569 static void s5k5baf_hw_patch(struct s5k5baf *state)
570 {
571 	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_PATCH);
572 
573 	if (seq)
574 		s5k5baf_write_nseq(state, seq);
575 }
576 
577 static void s5k5baf_hw_set_clocks(struct s5k5baf *state)
578 {
579 	unsigned long mclk = state->mclk_frequency / 1000;
580 	u16 status;
581 	static const u16 nseq_clk_cfg[] = {
582 		NSEQ(REG_I_USE_NPVI_CLOCKS,
583 		  NPVI_CLOCKS, NMIPI_CLOCKS, 0,
584 		  SCLK_PVI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4,
585 		  SCLK_MIPI_FREQ / 4, PCLK_MIN_FREQ / 4, PCLK_MAX_FREQ / 4),
586 		NSEQ(REG_I_USE_REGS_API, 1),
587 		0
588 	};
589 
590 	s5k5baf_write_seq(state, REG_I_INCLK_FREQ_L, mclk & 0xffff, mclk >> 16);
591 	s5k5baf_write_nseq(state, nseq_clk_cfg);
592 
593 	s5k5baf_synchronize(state, 250, REG_I_INIT_PARAMS_UPDATED);
594 	status = s5k5baf_read(state, REG_I_ERROR_INFO);
595 	if (!state->error && status) {
596 		v4l2_err(&state->sd, "error configuring PLL (%d)\n", status);
597 		state->error = -EINVAL;
598 	}
599 }
600 
601 /* set custom color correction matrices for various illuminations */
602 static void s5k5baf_hw_set_ccm(struct s5k5baf *state)
603 {
604 	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CCM);
605 
606 	if (seq)
607 		s5k5baf_write_nseq(state, seq);
608 }
609 
610 /* CIS sensor tuning, based on undocumented android driver code */
611 static void s5k5baf_hw_set_cis(struct s5k5baf *state)
612 {
613 	u16 *seq = s5k5baf_fw_get_seq(state, S5K5BAF_FW_ID_CIS);
614 
615 	if (!seq)
616 		return;
617 
618 	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_HW);
619 	s5k5baf_write_nseq(state, seq);
620 	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
621 }
622 
623 static void s5k5baf_hw_sync_cfg(struct s5k5baf *state)
624 {
625 	s5k5baf_write(state, REG_G_PREV_CFG_CHG, 1);
626 	if (state->apply_crop) {
627 		s5k5baf_write(state, REG_G_INPUTS_CHANGE_REQ, 1);
628 		s5k5baf_write(state, REG_G_PREV_CFG_BYPASS_CHANGED, 1);
629 	}
630 	s5k5baf_synchronize(state, 500, REG_G_NEW_CFG_SYNC);
631 }
632 /* Set horizontal and vertical image flipping */
633 static void s5k5baf_hw_set_mirror(struct s5k5baf *state)
634 {
635 	u16 flip = state->ctrls.vflip->val | (state->ctrls.vflip->val << 1);
636 
637 	s5k5baf_write(state, REG_P_PREV_MIRROR(0), flip);
638 	if (state->streaming)
639 		s5k5baf_hw_sync_cfg(state);
640 }
641 
642 static void s5k5baf_hw_set_alg(struct s5k5baf *state, u16 alg, bool enable)
643 {
644 	u16 cur_alg, new_alg;
645 
646 	if (!state->valid_auto_alg)
647 		cur_alg = s5k5baf_read(state, REG_DBG_AUTOALG_EN);
648 	else
649 		cur_alg = state->auto_alg;
650 
651 	new_alg = enable ? (cur_alg | alg) : (cur_alg & ~alg);
652 
653 	if (new_alg != cur_alg)
654 		s5k5baf_write(state, REG_DBG_AUTOALG_EN, new_alg);
655 
656 	if (state->error)
657 		return;
658 
659 	state->valid_auto_alg = 1;
660 	state->auto_alg = new_alg;
661 }
662 
663 /* Configure auto/manual white balance and R/G/B gains */
664 static void s5k5baf_hw_set_awb(struct s5k5baf *state, int awb)
665 {
666 	struct s5k5baf_ctrls *ctrls = &state->ctrls;
667 
668 	if (!awb)
669 		s5k5baf_write_seq(state, REG_SF_RGAIN,
670 				  ctrls->gain_red->val, 1,
671 				  S5K5BAF_GAIN_GREEN_DEF, 1,
672 				  ctrls->gain_blue->val, 1,
673 				  1);
674 
675 	s5k5baf_hw_set_alg(state, AALG_WB_EN, awb);
676 }
677 
678 /* Program FW with exposure time, 'exposure' in us units */
679 static void s5k5baf_hw_set_user_exposure(struct s5k5baf *state, int exposure)
680 {
681 	unsigned int time = exposure / 10;
682 
683 	s5k5baf_write_seq(state, REG_SF_USR_EXPOSURE_L,
684 			  time & 0xffff, time >> 16, 1);
685 }
686 
687 static void s5k5baf_hw_set_user_gain(struct s5k5baf *state, int gain)
688 {
689 	s5k5baf_write_seq(state, REG_SF_USR_TOT_GAIN, gain, 1);
690 }
691 
692 /* Set auto/manual exposure and total gain */
693 static void s5k5baf_hw_set_auto_exposure(struct s5k5baf *state, int value)
694 {
695 	if (value == V4L2_EXPOSURE_AUTO) {
696 		s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, true);
697 	} else {
698 		unsigned int exp_time = state->ctrls.exposure->val;
699 
700 		s5k5baf_hw_set_user_exposure(state, exp_time);
701 		s5k5baf_hw_set_user_gain(state, state->ctrls.gain->val);
702 		s5k5baf_hw_set_alg(state, AALG_AE_EN | AALG_DIVLEI_EN, false);
703 	}
704 }
705 
706 static void s5k5baf_hw_set_anti_flicker(struct s5k5baf *state, int v)
707 {
708 	if (v == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) {
709 		s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, true);
710 	} else {
711 		/* The V4L2_CID_LINE_FREQUENCY control values match
712 		 * the register values */
713 		s5k5baf_write_seq(state, REG_SF_FLICKER_QUANT, v, 1);
714 		s5k5baf_hw_set_alg(state, AALG_FLICKER_EN, false);
715 	}
716 }
717 
718 static void s5k5baf_hw_set_colorfx(struct s5k5baf *state, int val)
719 {
720 	static const u16 colorfx[] = {
721 		[V4L2_COLORFX_NONE] = 0,
722 		[V4L2_COLORFX_BW] = 1,
723 		[V4L2_COLORFX_NEGATIVE] = 2,
724 		[V4L2_COLORFX_SEPIA] = 3,
725 		[V4L2_COLORFX_SKY_BLUE] = 4,
726 		[V4L2_COLORFX_SKETCH] = 5,
727 	};
728 
729 	s5k5baf_write(state, REG_G_SPEC_EFFECTS, colorfx[val]);
730 }
731 
732 static int s5k5baf_find_pixfmt(struct v4l2_mbus_framefmt *mf)
733 {
734 	int i, c = -1;
735 
736 	for (i = 0; i < ARRAY_SIZE(s5k5baf_formats); i++) {
737 		if (mf->colorspace != s5k5baf_formats[i].colorspace)
738 			continue;
739 		if (mf->code == s5k5baf_formats[i].code)
740 			return i;
741 		if (c < 0)
742 			c = i;
743 	}
744 	return (c < 0) ? 0 : c;
745 }
746 
747 static int s5k5baf_clear_error(struct s5k5baf *state)
748 {
749 	int ret = state->error;
750 
751 	state->error = 0;
752 	return ret;
753 }
754 
755 static int s5k5baf_hw_set_video_bus(struct s5k5baf *state)
756 {
757 	u16 en_pkts;
758 
759 	if (state->bus_type == V4L2_MBUS_CSI2_DPHY)
760 		en_pkts = EN_PACKETS_CSI2;
761 	else
762 		en_pkts = 0;
763 
764 	s5k5baf_write_seq(state, REG_OIF_EN_MIPI_LANES,
765 			  state->nlanes, en_pkts, 1);
766 
767 	return s5k5baf_clear_error(state);
768 }
769 
770 static u16 s5k5baf_get_cfg_error(struct s5k5baf *state)
771 {
772 	u16 err = s5k5baf_read(state, REG_G_PREV_CFG_ERROR);
773 	if (err)
774 		s5k5baf_write(state, REG_G_PREV_CFG_ERROR, 0);
775 	return err;
776 }
777 
778 static void s5k5baf_hw_set_fiv(struct s5k5baf *state, u16 fiv)
779 {
780 	s5k5baf_write(state, REG_P_MAX_FR_TIME(0), fiv);
781 	s5k5baf_hw_sync_cfg(state);
782 }
783 
784 static void s5k5baf_hw_find_min_fiv(struct s5k5baf *state)
785 {
786 	u16 err, fiv;
787 	int n;
788 
789 	fiv = s5k5baf_read(state,  REG_G_ACTUAL_P_FR_TIME);
790 	if (state->error)
791 		return;
792 
793 	for (n = 5; n > 0; --n) {
794 		s5k5baf_hw_set_fiv(state, fiv);
795 		err = s5k5baf_get_cfg_error(state);
796 		if (state->error)
797 			return;
798 		switch (err) {
799 		case CFG_ERROR_RANGE:
800 			++fiv;
801 			break;
802 		case 0:
803 			state->fiv = fiv;
804 			v4l2_info(&state->sd,
805 				  "found valid frame interval: %d00us\n", fiv);
806 			return;
807 		default:
808 			v4l2_err(&state->sd,
809 				 "error setting frame interval: %d\n", err);
810 			state->error = -EINVAL;
811 		}
812 	}
813 	v4l2_err(&state->sd, "cannot find correct frame interval\n");
814 	state->error = -ERANGE;
815 }
816 
817 static void s5k5baf_hw_validate_cfg(struct s5k5baf *state)
818 {
819 	u16 err;
820 
821 	err = s5k5baf_get_cfg_error(state);
822 	if (state->error)
823 		return;
824 
825 	switch (err) {
826 	case 0:
827 		state->apply_cfg = 1;
828 		return;
829 	case CFG_ERROR_RANGE:
830 		s5k5baf_hw_find_min_fiv(state);
831 		if (!state->error)
832 			state->apply_cfg = 1;
833 		return;
834 	default:
835 		v4l2_err(&state->sd,
836 			 "error setting format: %d\n", err);
837 		state->error = -EINVAL;
838 	}
839 }
840 
841 static void s5k5baf_rescale(struct v4l2_rect *r, const struct v4l2_rect *v,
842 			    const struct v4l2_rect *n,
843 			    const struct v4l2_rect *d)
844 {
845 	r->left = v->left * n->width / d->width;
846 	r->top = v->top * n->height / d->height;
847 	r->width = v->width * n->width / d->width;
848 	r->height = v->height * n->height / d->height;
849 }
850 
851 static int s5k5baf_hw_set_crop_rects(struct s5k5baf *state)
852 {
853 	struct v4l2_rect *p, r;
854 	u16 err;
855 	int ret;
856 
857 	p = &state->crop_sink;
858 	s5k5baf_write_seq(state, REG_G_PREVREQ_IN_WIDTH, p->width, p->height,
859 			  p->left, p->top);
860 
861 	s5k5baf_rescale(&r, &state->crop_source, &state->crop_sink,
862 			&state->compose);
863 	s5k5baf_write_seq(state, REG_G_PREVZOOM_IN_WIDTH, r.width, r.height,
864 			  r.left, r.top);
865 
866 	s5k5baf_synchronize(state, 500, REG_G_INPUTS_CHANGE_REQ);
867 	s5k5baf_synchronize(state, 500, REG_G_PREV_CFG_BYPASS_CHANGED);
868 	err = s5k5baf_get_cfg_error(state);
869 	ret = s5k5baf_clear_error(state);
870 	if (ret < 0)
871 		return ret;
872 
873 	switch (err) {
874 	case 0:
875 		break;
876 	case CFG_ERROR_RANGE:
877 		/* retry crop with frame interval set to max */
878 		s5k5baf_hw_set_fiv(state, S5K5BAF_MAX_FR_TIME);
879 		err = s5k5baf_get_cfg_error(state);
880 		ret = s5k5baf_clear_error(state);
881 		if (ret < 0)
882 			return ret;
883 		if (err) {
884 			v4l2_err(&state->sd,
885 				 "crop error on max frame interval: %d\n", err);
886 			state->error = -EINVAL;
887 		}
888 		s5k5baf_hw_set_fiv(state, state->req_fiv);
889 		s5k5baf_hw_validate_cfg(state);
890 		break;
891 	default:
892 		v4l2_err(&state->sd, "crop error: %d\n", err);
893 		return -EINVAL;
894 	}
895 
896 	if (!state->apply_cfg)
897 		return 0;
898 
899 	p = &state->crop_source;
900 	s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0), p->width, p->height);
901 	s5k5baf_hw_set_fiv(state, state->req_fiv);
902 	s5k5baf_hw_validate_cfg(state);
903 
904 	return s5k5baf_clear_error(state);
905 }
906 
907 static void s5k5baf_hw_set_config(struct s5k5baf *state)
908 {
909 	u16 reg_fmt = s5k5baf_formats[state->pixfmt].reg_p_fmt;
910 	struct v4l2_rect *r = &state->crop_source;
911 
912 	s5k5baf_write_seq(state, REG_P_OUT_WIDTH(0),
913 			  r->width, r->height, reg_fmt,
914 			  PCLK_MAX_FREQ >> 2, PCLK_MIN_FREQ >> 2,
915 			  PVI_MASK_MIPI, CLK_MIPI_INDEX,
916 			  FR_RATE_FIXED, FR_RATE_Q_DYNAMIC,
917 			  state->req_fiv, S5K5BAF_MIN_FR_TIME);
918 	s5k5baf_hw_sync_cfg(state);
919 	s5k5baf_hw_validate_cfg(state);
920 }
921 
922 
923 static void s5k5baf_hw_set_test_pattern(struct s5k5baf *state, int id)
924 {
925 	s5k5baf_i2c_write(state, REG_PATTERN_WIDTH, 800);
926 	s5k5baf_i2c_write(state, REG_PATTERN_HEIGHT, 511);
927 	s5k5baf_i2c_write(state, REG_PATTERN_PARAM, 0);
928 	s5k5baf_i2c_write(state, REG_PATTERN_SET, id);
929 }
930 
931 static void s5k5baf_gpio_assert(struct s5k5baf *state, int id)
932 {
933 	gpiod_set_value_cansleep(state->gpios[id], 1);
934 }
935 
936 static void s5k5baf_gpio_deassert(struct s5k5baf *state, int id)
937 {
938 	gpiod_set_value_cansleep(state->gpios[id], 0);
939 }
940 
941 static int s5k5baf_power_on(struct s5k5baf *state)
942 {
943 	int ret;
944 
945 	ret = regulator_bulk_enable(S5K5BAF_NUM_SUPPLIES, state->supplies);
946 	if (ret < 0)
947 		goto err;
948 
949 	ret = clk_set_rate(state->clock, state->mclk_frequency);
950 	if (ret < 0)
951 		goto err_reg_dis;
952 
953 	ret = clk_prepare_enable(state->clock);
954 	if (ret < 0)
955 		goto err_reg_dis;
956 
957 	v4l2_dbg(1, debug, &state->sd, "clock frequency: %ld\n",
958 		 clk_get_rate(state->clock));
959 
960 	s5k5baf_gpio_deassert(state, STBY);
961 	usleep_range(50, 100);
962 	s5k5baf_gpio_deassert(state, RSET);
963 	return 0;
964 
965 err_reg_dis:
966 	regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES, state->supplies);
967 err:
968 	v4l2_err(&state->sd, "%s() failed (%d)\n", __func__, ret);
969 	return ret;
970 }
971 
972 static int s5k5baf_power_off(struct s5k5baf *state)
973 {
974 	int ret;
975 
976 	state->streaming = 0;
977 	state->apply_cfg = 0;
978 	state->apply_crop = 0;
979 
980 	s5k5baf_gpio_assert(state, RSET);
981 	s5k5baf_gpio_assert(state, STBY);
982 
983 	if (!IS_ERR(state->clock))
984 		clk_disable_unprepare(state->clock);
985 
986 	ret = regulator_bulk_disable(S5K5BAF_NUM_SUPPLIES,
987 					state->supplies);
988 	if (ret < 0)
989 		v4l2_err(&state->sd, "failed to disable regulators\n");
990 
991 	return 0;
992 }
993 
994 static void s5k5baf_hw_init(struct s5k5baf *state)
995 {
996 	s5k5baf_i2c_write(state, AHB_MSB_ADDR_PTR, PAGE_IF_HW);
997 	s5k5baf_i2c_write(state, REG_CLEAR_HOST_INT, 0);
998 	s5k5baf_i2c_write(state, REG_SW_LOAD_COMPLETE, 1);
999 	s5k5baf_i2c_write(state, REG_CMDRD_PAGE, PAGE_IF_SW);
1000 	s5k5baf_i2c_write(state, REG_CMDWR_PAGE, PAGE_IF_SW);
1001 }
1002 
1003 /*
1004  * V4L2 subdev core and video operations
1005  */
1006 
1007 static void s5k5baf_initialize_data(struct s5k5baf *state)
1008 {
1009 	state->pixfmt = 0;
1010 	state->req_fiv = 10000 / 15;
1011 	state->fiv = state->req_fiv;
1012 	state->valid_auto_alg = 0;
1013 }
1014 
1015 static int s5k5baf_load_setfile(struct s5k5baf *state)
1016 {
1017 	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
1018 	const struct firmware *fw;
1019 	int ret;
1020 
1021 	ret = request_firmware(&fw, S5K5BAF_FW_FILENAME, &c->dev);
1022 	if (ret < 0) {
1023 		dev_warn(&c->dev, "firmware file (%s) not loaded\n",
1024 			 S5K5BAF_FW_FILENAME);
1025 		return ret;
1026 	}
1027 
1028 	ret = s5k5baf_fw_parse(&c->dev, &state->fw, fw->size / 2,
1029 			       (__le16 *)fw->data);
1030 
1031 	release_firmware(fw);
1032 
1033 	return ret;
1034 }
1035 
1036 static int s5k5baf_set_power(struct v4l2_subdev *sd, int on)
1037 {
1038 	struct s5k5baf *state = to_s5k5baf(sd);
1039 	int ret = 0;
1040 
1041 	mutex_lock(&state->lock);
1042 
1043 	if (state->power != !on)
1044 		goto out;
1045 
1046 	if (on) {
1047 		if (state->fw == NULL)
1048 			s5k5baf_load_setfile(state);
1049 
1050 		s5k5baf_initialize_data(state);
1051 		ret = s5k5baf_power_on(state);
1052 		if (ret < 0)
1053 			goto out;
1054 
1055 		s5k5baf_hw_init(state);
1056 		s5k5baf_hw_patch(state);
1057 		s5k5baf_i2c_write(state, REG_SET_HOST_INT, 1);
1058 		s5k5baf_hw_set_clocks(state);
1059 
1060 		ret = s5k5baf_hw_set_video_bus(state);
1061 		if (ret < 0)
1062 			goto out;
1063 
1064 		s5k5baf_hw_set_cis(state);
1065 		s5k5baf_hw_set_ccm(state);
1066 
1067 		ret = s5k5baf_clear_error(state);
1068 		if (!ret)
1069 			state->power++;
1070 	} else {
1071 		s5k5baf_power_off(state);
1072 		state->power--;
1073 	}
1074 
1075 out:
1076 	mutex_unlock(&state->lock);
1077 
1078 	if (!ret && on)
1079 		ret = v4l2_ctrl_handler_setup(&state->ctrls.handler);
1080 
1081 	return ret;
1082 }
1083 
1084 static void s5k5baf_hw_set_stream(struct s5k5baf *state, int enable)
1085 {
1086 	s5k5baf_write_seq(state, REG_G_ENABLE_PREV, enable, 1);
1087 }
1088 
1089 static int s5k5baf_s_stream(struct v4l2_subdev *sd, int on)
1090 {
1091 	struct s5k5baf *state = to_s5k5baf(sd);
1092 	int ret;
1093 
1094 	mutex_lock(&state->lock);
1095 
1096 	if (state->streaming == !!on) {
1097 		ret = 0;
1098 		goto out;
1099 	}
1100 
1101 	if (on) {
1102 		s5k5baf_hw_set_config(state);
1103 		ret = s5k5baf_hw_set_crop_rects(state);
1104 		if (ret < 0)
1105 			goto out;
1106 		s5k5baf_hw_set_stream(state, 1);
1107 		s5k5baf_i2c_write(state, 0xb0cc, 0x000b);
1108 	} else {
1109 		s5k5baf_hw_set_stream(state, 0);
1110 	}
1111 	ret = s5k5baf_clear_error(state);
1112 	if (!ret)
1113 		state->streaming = !state->streaming;
1114 
1115 out:
1116 	mutex_unlock(&state->lock);
1117 
1118 	return ret;
1119 }
1120 
1121 static int s5k5baf_g_frame_interval(struct v4l2_subdev *sd,
1122 				   struct v4l2_subdev_frame_interval *fi)
1123 {
1124 	struct s5k5baf *state = to_s5k5baf(sd);
1125 
1126 	mutex_lock(&state->lock);
1127 	fi->interval.numerator = state->fiv;
1128 	fi->interval.denominator = 10000;
1129 	mutex_unlock(&state->lock);
1130 
1131 	return 0;
1132 }
1133 
1134 static void s5k5baf_set_frame_interval(struct s5k5baf *state,
1135 				       struct v4l2_subdev_frame_interval *fi)
1136 {
1137 	struct v4l2_fract *i = &fi->interval;
1138 
1139 	if (fi->interval.denominator == 0)
1140 		state->req_fiv = S5K5BAF_MAX_FR_TIME;
1141 	else
1142 		state->req_fiv = clamp_t(u32,
1143 					 i->numerator * 10000 / i->denominator,
1144 					 S5K5BAF_MIN_FR_TIME,
1145 					 S5K5BAF_MAX_FR_TIME);
1146 
1147 	state->fiv = state->req_fiv;
1148 	if (state->apply_cfg) {
1149 		s5k5baf_hw_set_fiv(state, state->req_fiv);
1150 		s5k5baf_hw_validate_cfg(state);
1151 	}
1152 	*i = (struct v4l2_fract){ state->fiv, 10000 };
1153 	if (state->fiv == state->req_fiv)
1154 		v4l2_info(&state->sd, "frame interval changed to %d00us\n",
1155 			  state->fiv);
1156 }
1157 
1158 static int s5k5baf_s_frame_interval(struct v4l2_subdev *sd,
1159 				   struct v4l2_subdev_frame_interval *fi)
1160 {
1161 	struct s5k5baf *state = to_s5k5baf(sd);
1162 
1163 	mutex_lock(&state->lock);
1164 	s5k5baf_set_frame_interval(state, fi);
1165 	mutex_unlock(&state->lock);
1166 	return 0;
1167 }
1168 
1169 /*
1170  * V4L2 subdev pad level and video operations
1171  */
1172 static int s5k5baf_enum_frame_interval(struct v4l2_subdev *sd,
1173 			      struct v4l2_subdev_state *sd_state,
1174 			      struct v4l2_subdev_frame_interval_enum *fie)
1175 {
1176 	if (fie->index > S5K5BAF_MAX_FR_TIME - S5K5BAF_MIN_FR_TIME ||
1177 	    fie->pad != PAD_CIS)
1178 		return -EINVAL;
1179 
1180 	v4l_bound_align_image(&fie->width, S5K5BAF_WIN_WIDTH_MIN,
1181 			      S5K5BAF_CIS_WIDTH, 1,
1182 			      &fie->height, S5K5BAF_WIN_HEIGHT_MIN,
1183 			      S5K5BAF_CIS_HEIGHT, 1, 0);
1184 
1185 	fie->interval.numerator = S5K5BAF_MIN_FR_TIME + fie->index;
1186 	fie->interval.denominator = 10000;
1187 
1188 	return 0;
1189 }
1190 
1191 static int s5k5baf_enum_mbus_code(struct v4l2_subdev *sd,
1192 				 struct v4l2_subdev_state *sd_state,
1193 				 struct v4l2_subdev_mbus_code_enum *code)
1194 {
1195 	if (code->pad == PAD_CIS) {
1196 		if (code->index > 0)
1197 			return -EINVAL;
1198 		code->code = MEDIA_BUS_FMT_FIXED;
1199 		return 0;
1200 	}
1201 
1202 	if (code->index >= ARRAY_SIZE(s5k5baf_formats))
1203 		return -EINVAL;
1204 
1205 	code->code = s5k5baf_formats[code->index].code;
1206 	return 0;
1207 }
1208 
1209 static int s5k5baf_enum_frame_size(struct v4l2_subdev *sd,
1210 				  struct v4l2_subdev_state *sd_state,
1211 				  struct v4l2_subdev_frame_size_enum *fse)
1212 {
1213 	int i;
1214 
1215 	if (fse->index > 0)
1216 		return -EINVAL;
1217 
1218 	if (fse->pad == PAD_CIS) {
1219 		fse->code = MEDIA_BUS_FMT_FIXED;
1220 		fse->min_width = S5K5BAF_CIS_WIDTH;
1221 		fse->max_width = S5K5BAF_CIS_WIDTH;
1222 		fse->min_height = S5K5BAF_CIS_HEIGHT;
1223 		fse->max_height = S5K5BAF_CIS_HEIGHT;
1224 		return 0;
1225 	}
1226 
1227 	i = ARRAY_SIZE(s5k5baf_formats);
1228 	while (--i)
1229 		if (fse->code == s5k5baf_formats[i].code)
1230 			break;
1231 	fse->code = s5k5baf_formats[i].code;
1232 	fse->min_width = S5K5BAF_WIN_WIDTH_MIN;
1233 	fse->max_width = S5K5BAF_CIS_WIDTH;
1234 	fse->max_height = S5K5BAF_WIN_HEIGHT_MIN;
1235 	fse->min_height = S5K5BAF_CIS_HEIGHT;
1236 
1237 	return 0;
1238 }
1239 
1240 static void s5k5baf_try_cis_format(struct v4l2_mbus_framefmt *mf)
1241 {
1242 	mf->width = S5K5BAF_CIS_WIDTH;
1243 	mf->height = S5K5BAF_CIS_HEIGHT;
1244 	mf->code = MEDIA_BUS_FMT_FIXED;
1245 	mf->colorspace = V4L2_COLORSPACE_JPEG;
1246 	mf->field = V4L2_FIELD_NONE;
1247 }
1248 
1249 static int s5k5baf_try_isp_format(struct v4l2_mbus_framefmt *mf)
1250 {
1251 	int pixfmt;
1252 
1253 	v4l_bound_align_image(&mf->width, S5K5BAF_WIN_WIDTH_MIN,
1254 			      S5K5BAF_CIS_WIDTH, 1,
1255 			      &mf->height, S5K5BAF_WIN_HEIGHT_MIN,
1256 			      S5K5BAF_CIS_HEIGHT, 1, 0);
1257 
1258 	pixfmt = s5k5baf_find_pixfmt(mf);
1259 
1260 	mf->colorspace = s5k5baf_formats[pixfmt].colorspace;
1261 	mf->code = s5k5baf_formats[pixfmt].code;
1262 	mf->field = V4L2_FIELD_NONE;
1263 
1264 	return pixfmt;
1265 }
1266 
1267 static int s5k5baf_get_fmt(struct v4l2_subdev *sd,
1268 			   struct v4l2_subdev_state *sd_state,
1269 			   struct v4l2_subdev_format *fmt)
1270 {
1271 	struct s5k5baf *state = to_s5k5baf(sd);
1272 	const struct s5k5baf_pixfmt *pixfmt;
1273 	struct v4l2_mbus_framefmt *mf;
1274 
1275 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1276 		mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
1277 		fmt->format = *mf;
1278 		return 0;
1279 	}
1280 
1281 	mf = &fmt->format;
1282 	if (fmt->pad == PAD_CIS) {
1283 		s5k5baf_try_cis_format(mf);
1284 		return 0;
1285 	}
1286 	mf->field = V4L2_FIELD_NONE;
1287 	mutex_lock(&state->lock);
1288 	pixfmt = &s5k5baf_formats[state->pixfmt];
1289 	mf->width = state->crop_source.width;
1290 	mf->height = state->crop_source.height;
1291 	mf->code = pixfmt->code;
1292 	mf->colorspace = pixfmt->colorspace;
1293 	mutex_unlock(&state->lock);
1294 
1295 	return 0;
1296 }
1297 
1298 static int s5k5baf_set_fmt(struct v4l2_subdev *sd,
1299 			   struct v4l2_subdev_state *sd_state,
1300 			   struct v4l2_subdev_format *fmt)
1301 {
1302 	struct v4l2_mbus_framefmt *mf = &fmt->format;
1303 	struct s5k5baf *state = to_s5k5baf(sd);
1304 	const struct s5k5baf_pixfmt *pixfmt;
1305 	int ret = 0;
1306 
1307 	mf->field = V4L2_FIELD_NONE;
1308 
1309 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1310 		*v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) = *mf;
1311 		return 0;
1312 	}
1313 
1314 	if (fmt->pad == PAD_CIS) {
1315 		s5k5baf_try_cis_format(mf);
1316 		return 0;
1317 	}
1318 
1319 	mutex_lock(&state->lock);
1320 
1321 	if (state->streaming) {
1322 		mutex_unlock(&state->lock);
1323 		return -EBUSY;
1324 	}
1325 
1326 	state->pixfmt = s5k5baf_try_isp_format(mf);
1327 	pixfmt = &s5k5baf_formats[state->pixfmt];
1328 	mf->code = pixfmt->code;
1329 	mf->colorspace = pixfmt->colorspace;
1330 	mf->width = state->crop_source.width;
1331 	mf->height = state->crop_source.height;
1332 
1333 	mutex_unlock(&state->lock);
1334 	return ret;
1335 }
1336 
1337 enum selection_rect { R_CIS, R_CROP_SINK, R_COMPOSE, R_CROP_SOURCE, R_INVALID };
1338 
1339 static enum selection_rect s5k5baf_get_sel_rect(u32 pad, u32 target)
1340 {
1341 	switch (target) {
1342 	case V4L2_SEL_TGT_CROP_BOUNDS:
1343 		return pad ? R_COMPOSE : R_CIS;
1344 	case V4L2_SEL_TGT_CROP:
1345 		return pad ? R_CROP_SOURCE : R_CROP_SINK;
1346 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1347 		return pad ? R_INVALID : R_CROP_SINK;
1348 	case V4L2_SEL_TGT_COMPOSE:
1349 		return pad ? R_INVALID : R_COMPOSE;
1350 	default:
1351 		return R_INVALID;
1352 	}
1353 }
1354 
1355 static int s5k5baf_is_bound_target(u32 target)
1356 {
1357 	return target == V4L2_SEL_TGT_CROP_BOUNDS ||
1358 		target == V4L2_SEL_TGT_COMPOSE_BOUNDS;
1359 }
1360 
1361 static int s5k5baf_get_selection(struct v4l2_subdev *sd,
1362 				 struct v4l2_subdev_state *sd_state,
1363 				 struct v4l2_subdev_selection *sel)
1364 {
1365 	enum selection_rect rtype;
1366 	struct s5k5baf *state = to_s5k5baf(sd);
1367 
1368 	rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
1369 
1370 	switch (rtype) {
1371 	case R_INVALID:
1372 		return -EINVAL;
1373 	case R_CIS:
1374 		sel->r = s5k5baf_cis_rect;
1375 		return 0;
1376 	default:
1377 		break;
1378 	}
1379 
1380 	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1381 		if (rtype == R_COMPOSE)
1382 			sel->r = *v4l2_subdev_get_try_compose(sd, sd_state,
1383 							      sel->pad);
1384 		else
1385 			sel->r = *v4l2_subdev_get_try_crop(sd, sd_state,
1386 							   sel->pad);
1387 		return 0;
1388 	}
1389 
1390 	mutex_lock(&state->lock);
1391 	switch (rtype) {
1392 	case R_CROP_SINK:
1393 		sel->r = state->crop_sink;
1394 		break;
1395 	case R_COMPOSE:
1396 		sel->r = state->compose;
1397 		break;
1398 	case R_CROP_SOURCE:
1399 		sel->r = state->crop_source;
1400 		break;
1401 	default:
1402 		break;
1403 	}
1404 	if (s5k5baf_is_bound_target(sel->target)) {
1405 		sel->r.left = 0;
1406 		sel->r.top = 0;
1407 	}
1408 	mutex_unlock(&state->lock);
1409 
1410 	return 0;
1411 }
1412 
1413 /* bounds range [start, start+len) to [0, max) and aligns to 2 */
1414 static void s5k5baf_bound_range(u32 *start, u32 *len, u32 max)
1415 {
1416 	if (*len > max)
1417 		*len = max;
1418 	if (*start + *len > max)
1419 		*start = max - *len;
1420 	*start &= ~1;
1421 	*len &= ~1;
1422 	if (*len < S5K5BAF_WIN_WIDTH_MIN)
1423 		*len = S5K5BAF_WIN_WIDTH_MIN;
1424 }
1425 
1426 static void s5k5baf_bound_rect(struct v4l2_rect *r, u32 width, u32 height)
1427 {
1428 	s5k5baf_bound_range(&r->left, &r->width, width);
1429 	s5k5baf_bound_range(&r->top, &r->height, height);
1430 }
1431 
1432 static void s5k5baf_set_rect_and_adjust(struct v4l2_rect **rects,
1433 					enum selection_rect first,
1434 					struct v4l2_rect *v)
1435 {
1436 	struct v4l2_rect *r, *br;
1437 	enum selection_rect i = first;
1438 
1439 	*rects[first] = *v;
1440 	do {
1441 		r = rects[i];
1442 		br = rects[i - 1];
1443 		s5k5baf_bound_rect(r, br->width, br->height);
1444 	} while (++i != R_INVALID);
1445 	*v = *rects[first];
1446 }
1447 
1448 static bool s5k5baf_cmp_rect(const struct v4l2_rect *r1,
1449 			     const struct v4l2_rect *r2)
1450 {
1451 	return !memcmp(r1, r2, sizeof(*r1));
1452 }
1453 
1454 static int s5k5baf_set_selection(struct v4l2_subdev *sd,
1455 				 struct v4l2_subdev_state *sd_state,
1456 				 struct v4l2_subdev_selection *sel)
1457 {
1458 	static enum selection_rect rtype;
1459 	struct s5k5baf *state = to_s5k5baf(sd);
1460 	struct v4l2_rect **rects;
1461 	int ret = 0;
1462 
1463 	rtype = s5k5baf_get_sel_rect(sel->pad, sel->target);
1464 	if (rtype == R_INVALID || s5k5baf_is_bound_target(sel->target))
1465 		return -EINVAL;
1466 
1467 	/* allow only scaling on compose */
1468 	if (rtype == R_COMPOSE) {
1469 		sel->r.left = 0;
1470 		sel->r.top = 0;
1471 	}
1472 
1473 	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
1474 		rects = (struct v4l2_rect * []) {
1475 				&s5k5baf_cis_rect,
1476 				v4l2_subdev_get_try_crop(sd, sd_state,
1477 							 PAD_CIS),
1478 				v4l2_subdev_get_try_compose(sd, sd_state,
1479 							    PAD_CIS),
1480 				v4l2_subdev_get_try_crop(sd, sd_state,
1481 							 PAD_OUT)
1482 			};
1483 		s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
1484 		return 0;
1485 	}
1486 
1487 	rects = (struct v4l2_rect * []) {
1488 			&s5k5baf_cis_rect,
1489 			&state->crop_sink,
1490 			&state->compose,
1491 			&state->crop_source
1492 		};
1493 	mutex_lock(&state->lock);
1494 	if (state->streaming) {
1495 		/* adjust sel->r to avoid output resolution change */
1496 		if (rtype < R_CROP_SOURCE) {
1497 			if (sel->r.width < state->crop_source.width)
1498 				sel->r.width = state->crop_source.width;
1499 			if (sel->r.height < state->crop_source.height)
1500 				sel->r.height = state->crop_source.height;
1501 		} else {
1502 			sel->r.width = state->crop_source.width;
1503 			sel->r.height = state->crop_source.height;
1504 		}
1505 	}
1506 	s5k5baf_set_rect_and_adjust(rects, rtype, &sel->r);
1507 	if (!s5k5baf_cmp_rect(&state->crop_sink, &s5k5baf_cis_rect) ||
1508 	    !s5k5baf_cmp_rect(&state->compose, &s5k5baf_cis_rect))
1509 		state->apply_crop = 1;
1510 	if (state->streaming)
1511 		ret = s5k5baf_hw_set_crop_rects(state);
1512 	mutex_unlock(&state->lock);
1513 
1514 	return ret;
1515 }
1516 
1517 static const struct v4l2_subdev_pad_ops s5k5baf_cis_pad_ops = {
1518 	.enum_mbus_code		= s5k5baf_enum_mbus_code,
1519 	.enum_frame_size	= s5k5baf_enum_frame_size,
1520 	.get_fmt		= s5k5baf_get_fmt,
1521 	.set_fmt		= s5k5baf_set_fmt,
1522 };
1523 
1524 static const struct v4l2_subdev_pad_ops s5k5baf_pad_ops = {
1525 	.enum_mbus_code		= s5k5baf_enum_mbus_code,
1526 	.enum_frame_size	= s5k5baf_enum_frame_size,
1527 	.enum_frame_interval	= s5k5baf_enum_frame_interval,
1528 	.get_fmt		= s5k5baf_get_fmt,
1529 	.set_fmt		= s5k5baf_set_fmt,
1530 	.get_selection		= s5k5baf_get_selection,
1531 	.set_selection		= s5k5baf_set_selection,
1532 };
1533 
1534 static const struct v4l2_subdev_video_ops s5k5baf_video_ops = {
1535 	.g_frame_interval	= s5k5baf_g_frame_interval,
1536 	.s_frame_interval	= s5k5baf_s_frame_interval,
1537 	.s_stream		= s5k5baf_s_stream,
1538 };
1539 
1540 /*
1541  * V4L2 subdev controls
1542  */
1543 
1544 static int s5k5baf_s_ctrl(struct v4l2_ctrl *ctrl)
1545 {
1546 	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
1547 	struct s5k5baf *state = to_s5k5baf(sd);
1548 	int ret;
1549 
1550 	v4l2_dbg(1, debug, sd, "ctrl: %s, value: %d\n", ctrl->name, ctrl->val);
1551 
1552 	mutex_lock(&state->lock);
1553 
1554 	if (state->power == 0)
1555 		goto unlock;
1556 
1557 	switch (ctrl->id) {
1558 	case V4L2_CID_AUTO_WHITE_BALANCE:
1559 		s5k5baf_hw_set_awb(state, ctrl->val);
1560 		break;
1561 
1562 	case V4L2_CID_BRIGHTNESS:
1563 		s5k5baf_write(state, REG_USER_BRIGHTNESS, ctrl->val);
1564 		break;
1565 
1566 	case V4L2_CID_COLORFX:
1567 		s5k5baf_hw_set_colorfx(state, ctrl->val);
1568 		break;
1569 
1570 	case V4L2_CID_CONTRAST:
1571 		s5k5baf_write(state, REG_USER_CONTRAST, ctrl->val);
1572 		break;
1573 
1574 	case V4L2_CID_EXPOSURE_AUTO:
1575 		s5k5baf_hw_set_auto_exposure(state, ctrl->val);
1576 		break;
1577 
1578 	case V4L2_CID_HFLIP:
1579 		s5k5baf_hw_set_mirror(state);
1580 		break;
1581 
1582 	case V4L2_CID_POWER_LINE_FREQUENCY:
1583 		s5k5baf_hw_set_anti_flicker(state, ctrl->val);
1584 		break;
1585 
1586 	case V4L2_CID_SATURATION:
1587 		s5k5baf_write(state, REG_USER_SATURATION, ctrl->val);
1588 		break;
1589 
1590 	case V4L2_CID_SHARPNESS:
1591 		s5k5baf_write(state, REG_USER_SHARPBLUR, ctrl->val);
1592 		break;
1593 
1594 	case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
1595 		s5k5baf_write(state, REG_P_COLORTEMP(0), ctrl->val);
1596 		if (state->apply_cfg)
1597 			s5k5baf_hw_sync_cfg(state);
1598 		break;
1599 
1600 	case V4L2_CID_TEST_PATTERN:
1601 		s5k5baf_hw_set_test_pattern(state, ctrl->val);
1602 		break;
1603 	}
1604 unlock:
1605 	ret = s5k5baf_clear_error(state);
1606 	mutex_unlock(&state->lock);
1607 	return ret;
1608 }
1609 
1610 static const struct v4l2_ctrl_ops s5k5baf_ctrl_ops = {
1611 	.s_ctrl	= s5k5baf_s_ctrl,
1612 };
1613 
1614 static const char * const s5k5baf_test_pattern_menu[] = {
1615 	"Disabled",
1616 	"Blank",
1617 	"Bars",
1618 	"Gradients",
1619 	"Textile",
1620 	"Textile2",
1621 	"Squares"
1622 };
1623 
1624 static int s5k5baf_initialize_ctrls(struct s5k5baf *state)
1625 {
1626 	const struct v4l2_ctrl_ops *ops = &s5k5baf_ctrl_ops;
1627 	struct s5k5baf_ctrls *ctrls = &state->ctrls;
1628 	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1629 	int ret;
1630 
1631 	ret = v4l2_ctrl_handler_init(hdl, 16);
1632 	if (ret < 0) {
1633 		v4l2_err(&state->sd, "cannot init ctrl handler (%d)\n", ret);
1634 		return ret;
1635 	}
1636 
1637 	/* Auto white balance cluster */
1638 	ctrls->awb = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE,
1639 				       0, 1, 1, 1);
1640 	ctrls->gain_red = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
1641 					    0, 255, 1, S5K5BAF_GAIN_RED_DEF);
1642 	ctrls->gain_blue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
1643 					     0, 255, 1, S5K5BAF_GAIN_BLUE_DEF);
1644 	v4l2_ctrl_auto_cluster(3, &ctrls->awb, 0, false);
1645 
1646 	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1647 	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1648 	v4l2_ctrl_cluster(2, &ctrls->hflip);
1649 
1650 	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
1651 				V4L2_CID_EXPOSURE_AUTO,
1652 				V4L2_EXPOSURE_MANUAL, 0, V4L2_EXPOSURE_AUTO);
1653 	/* Exposure time: x 1 us */
1654 	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
1655 					    0, 6000000U, 1, 100000U);
1656 	/* Total gain: 256 <=> 1x */
1657 	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1658 					0, 256, 1, 256);
1659 	v4l2_ctrl_auto_cluster(3, &ctrls->auto_exp, 0, false);
1660 
1661 	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_POWER_LINE_FREQUENCY,
1662 			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
1663 			       V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
1664 
1665 	v4l2_ctrl_new_std_menu(hdl, ops, V4L2_CID_COLORFX,
1666 			       V4L2_COLORFX_SKY_BLUE, ~0x6f, V4L2_COLORFX_NONE);
1667 
1668 	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_WHITE_BALANCE_TEMPERATURE,
1669 			  0, 256, 1, 0);
1670 
1671 	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION, -127, 127, 1, 0);
1672 	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -127, 127, 1, 0);
1673 	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -127, 127, 1, 0);
1674 	v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS, -127, 127, 1, 0);
1675 
1676 	v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1677 				     ARRAY_SIZE(s5k5baf_test_pattern_menu) - 1,
1678 				     0, 0, s5k5baf_test_pattern_menu);
1679 
1680 	if (hdl->error) {
1681 		v4l2_err(&state->sd, "error creating controls (%d)\n",
1682 			 hdl->error);
1683 		ret = hdl->error;
1684 		v4l2_ctrl_handler_free(hdl);
1685 		return ret;
1686 	}
1687 
1688 	state->sd.ctrl_handler = hdl;
1689 	return 0;
1690 }
1691 
1692 /*
1693  * V4L2 subdev internal operations
1694  */
1695 static int s5k5baf_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1696 {
1697 	struct v4l2_mbus_framefmt *mf;
1698 
1699 	mf = v4l2_subdev_get_try_format(sd, fh->state, PAD_CIS);
1700 	s5k5baf_try_cis_format(mf);
1701 
1702 	if (s5k5baf_is_cis_subdev(sd))
1703 		return 0;
1704 
1705 	mf = v4l2_subdev_get_try_format(sd, fh->state, PAD_OUT);
1706 	mf->colorspace = s5k5baf_formats[0].colorspace;
1707 	mf->code = s5k5baf_formats[0].code;
1708 	mf->width = s5k5baf_cis_rect.width;
1709 	mf->height = s5k5baf_cis_rect.height;
1710 	mf->field = V4L2_FIELD_NONE;
1711 
1712 	*v4l2_subdev_get_try_crop(sd, fh->state, PAD_CIS) = s5k5baf_cis_rect;
1713 	*v4l2_subdev_get_try_compose(sd, fh->state, PAD_CIS) = s5k5baf_cis_rect;
1714 	*v4l2_subdev_get_try_crop(sd, fh->state, PAD_OUT) = s5k5baf_cis_rect;
1715 
1716 	return 0;
1717 }
1718 
1719 static int s5k5baf_check_fw_revision(struct s5k5baf *state)
1720 {
1721 	u16 api_ver = 0, fw_rev = 0, s_id = 0;
1722 	int ret;
1723 
1724 	api_ver = s5k5baf_read(state, REG_FW_APIVER);
1725 	fw_rev = s5k5baf_read(state, REG_FW_REVISION) & 0xff;
1726 	s_id = s5k5baf_read(state, REG_FW_SENSOR_ID);
1727 	ret = s5k5baf_clear_error(state);
1728 	if (ret < 0)
1729 		return ret;
1730 
1731 	v4l2_info(&state->sd, "FW API=%#x, revision=%#x sensor_id=%#x\n",
1732 		  api_ver, fw_rev, s_id);
1733 
1734 	if (api_ver != S5K5BAF_FW_APIVER) {
1735 		v4l2_err(&state->sd, "FW API version not supported\n");
1736 		return -ENODEV;
1737 	}
1738 
1739 	return 0;
1740 }
1741 
1742 static int s5k5baf_registered(struct v4l2_subdev *sd)
1743 {
1744 	struct s5k5baf *state = to_s5k5baf(sd);
1745 	int ret;
1746 
1747 	ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->cis_sd);
1748 	if (ret < 0)
1749 		v4l2_err(sd, "failed to register subdev %s\n",
1750 			 state->cis_sd.name);
1751 	else
1752 		ret = media_create_pad_link(&state->cis_sd.entity, PAD_CIS,
1753 					       &state->sd.entity, PAD_CIS,
1754 					       MEDIA_LNK_FL_IMMUTABLE |
1755 					       MEDIA_LNK_FL_ENABLED);
1756 	return ret;
1757 }
1758 
1759 static void s5k5baf_unregistered(struct v4l2_subdev *sd)
1760 {
1761 	struct s5k5baf *state = to_s5k5baf(sd);
1762 	v4l2_device_unregister_subdev(&state->cis_sd);
1763 }
1764 
1765 static const struct v4l2_subdev_ops s5k5baf_cis_subdev_ops = {
1766 	.pad	= &s5k5baf_cis_pad_ops,
1767 };
1768 
1769 static const struct v4l2_subdev_internal_ops s5k5baf_cis_subdev_internal_ops = {
1770 	.open = s5k5baf_open,
1771 };
1772 
1773 static const struct v4l2_subdev_internal_ops s5k5baf_subdev_internal_ops = {
1774 	.registered = s5k5baf_registered,
1775 	.unregistered = s5k5baf_unregistered,
1776 	.open = s5k5baf_open,
1777 };
1778 
1779 static const struct v4l2_subdev_core_ops s5k5baf_core_ops = {
1780 	.s_power = s5k5baf_set_power,
1781 	.log_status = v4l2_ctrl_subdev_log_status,
1782 };
1783 
1784 static const struct v4l2_subdev_ops s5k5baf_subdev_ops = {
1785 	.core = &s5k5baf_core_ops,
1786 	.pad = &s5k5baf_pad_ops,
1787 	.video = &s5k5baf_video_ops,
1788 };
1789 
1790 static int s5k5baf_configure_gpios(struct s5k5baf *state)
1791 {
1792 	static const char * const name[] = { "stbyn", "rstn" };
1793 	static const char * const label[] = { "S5K5BAF_STBY", "S5K5BAF_RST" };
1794 	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
1795 	struct gpio_desc *gpio;
1796 	int ret, i;
1797 
1798 	for (i = 0; i < NUM_GPIOS; ++i) {
1799 		gpio = devm_gpiod_get(&c->dev, name[i], GPIOD_OUT_HIGH);
1800 		ret = PTR_ERR_OR_ZERO(gpio);
1801 		if (ret) {
1802 			v4l2_err(c, "failed to request gpio %s: %d\n",
1803 				 name[i], ret);
1804 			return ret;
1805 		}
1806 
1807 		ret = gpiod_set_consumer_name(gpio, label[i]);
1808 		if (ret) {
1809 			v4l2_err(c, "failed to set up name for gpio %s: %d\n",
1810 				 name[i], ret);
1811 			return ret;
1812 		}
1813 
1814 		state->gpios[i] = gpio;
1815 	}
1816 	return 0;
1817 }
1818 
1819 static int s5k5baf_parse_device_node(struct s5k5baf *state, struct device *dev)
1820 {
1821 	struct device_node *node = dev->of_node;
1822 	struct device_node *node_ep;
1823 	struct v4l2_fwnode_endpoint ep = { .bus_type = 0 };
1824 	int ret;
1825 
1826 	if (!node) {
1827 		dev_err(dev, "no device-tree node provided\n");
1828 		return -EINVAL;
1829 	}
1830 
1831 	ret = of_property_read_u32(node, "clock-frequency",
1832 				   &state->mclk_frequency);
1833 	if (ret < 0) {
1834 		state->mclk_frequency = S5K5BAF_DEFAULT_MCLK_FREQ;
1835 		dev_info(dev, "using default %u Hz clock frequency\n",
1836 			 state->mclk_frequency);
1837 	}
1838 
1839 	node_ep = of_graph_get_next_endpoint(node, NULL);
1840 	if (!node_ep) {
1841 		dev_err(dev, "no endpoint defined at node %pOF\n", node);
1842 		return -EINVAL;
1843 	}
1844 
1845 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
1846 	of_node_put(node_ep);
1847 	if (ret)
1848 		return ret;
1849 
1850 	state->bus_type = ep.bus_type;
1851 
1852 	switch (state->bus_type) {
1853 	case V4L2_MBUS_CSI2_DPHY:
1854 		state->nlanes = ep.bus.mipi_csi2.num_data_lanes;
1855 		break;
1856 	case V4L2_MBUS_PARALLEL:
1857 		break;
1858 	default:
1859 		dev_err(dev, "unsupported bus in endpoint defined at node %pOF\n",
1860 			node);
1861 		return -EINVAL;
1862 	}
1863 
1864 	return 0;
1865 }
1866 
1867 static int s5k5baf_configure_subdevs(struct s5k5baf *state,
1868 				     struct i2c_client *c)
1869 {
1870 	struct v4l2_subdev *sd;
1871 	int ret;
1872 
1873 	sd = &state->cis_sd;
1874 	v4l2_subdev_init(sd, &s5k5baf_cis_subdev_ops);
1875 	sd->owner = THIS_MODULE;
1876 	v4l2_set_subdevdata(sd, state);
1877 	snprintf(sd->name, sizeof(sd->name), "S5K5BAF-CIS %d-%04x",
1878 		 i2c_adapter_id(c->adapter), c->addr);
1879 
1880 	sd->internal_ops = &s5k5baf_cis_subdev_internal_ops;
1881 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1882 
1883 	state->cis_pad.flags = MEDIA_PAD_FL_SOURCE;
1884 	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1885 	ret = media_entity_pads_init(&sd->entity, NUM_CIS_PADS, &state->cis_pad);
1886 	if (ret < 0)
1887 		goto err;
1888 
1889 	sd = &state->sd;
1890 	v4l2_i2c_subdev_init(sd, c, &s5k5baf_subdev_ops);
1891 	snprintf(sd->name, sizeof(sd->name), "S5K5BAF-ISP %d-%04x",
1892 		 i2c_adapter_id(c->adapter), c->addr);
1893 
1894 	sd->internal_ops = &s5k5baf_subdev_internal_ops;
1895 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1896 
1897 	state->pads[PAD_CIS].flags = MEDIA_PAD_FL_SINK;
1898 	state->pads[PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
1899 	sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
1900 	ret = media_entity_pads_init(&sd->entity, NUM_ISP_PADS, state->pads);
1901 
1902 	if (!ret)
1903 		return 0;
1904 
1905 	media_entity_cleanup(&state->cis_sd.entity);
1906 err:
1907 	dev_err(&c->dev, "cannot init media entity %s\n", sd->name);
1908 	return ret;
1909 }
1910 
1911 static int s5k5baf_configure_regulators(struct s5k5baf *state)
1912 {
1913 	struct i2c_client *c = v4l2_get_subdevdata(&state->sd);
1914 	int ret;
1915 	int i;
1916 
1917 	for (i = 0; i < S5K5BAF_NUM_SUPPLIES; i++)
1918 		state->supplies[i].supply = s5k5baf_supply_names[i];
1919 
1920 	ret = devm_regulator_bulk_get(&c->dev, S5K5BAF_NUM_SUPPLIES,
1921 				      state->supplies);
1922 	if (ret < 0)
1923 		v4l2_err(c, "failed to get regulators\n");
1924 	return ret;
1925 }
1926 
1927 static int s5k5baf_probe(struct i2c_client *c)
1928 {
1929 	struct s5k5baf *state;
1930 	int ret;
1931 
1932 	state = devm_kzalloc(&c->dev, sizeof(*state), GFP_KERNEL);
1933 	if (!state)
1934 		return -ENOMEM;
1935 
1936 	mutex_init(&state->lock);
1937 	state->crop_sink = s5k5baf_cis_rect;
1938 	state->compose = s5k5baf_cis_rect;
1939 	state->crop_source = s5k5baf_cis_rect;
1940 
1941 	ret = s5k5baf_parse_device_node(state, &c->dev);
1942 	if (ret < 0)
1943 		return ret;
1944 
1945 	ret = s5k5baf_configure_subdevs(state, c);
1946 	if (ret < 0)
1947 		return ret;
1948 
1949 	ret = s5k5baf_configure_gpios(state);
1950 	if (ret < 0)
1951 		goto err_me;
1952 
1953 	ret = s5k5baf_configure_regulators(state);
1954 	if (ret < 0)
1955 		goto err_me;
1956 
1957 	state->clock = devm_clk_get(state->sd.dev, S5K5BAF_CLK_NAME);
1958 	if (IS_ERR(state->clock)) {
1959 		ret = -EPROBE_DEFER;
1960 		goto err_me;
1961 	}
1962 
1963 	ret = s5k5baf_power_on(state);
1964 	if (ret < 0) {
1965 		ret = -EPROBE_DEFER;
1966 		goto err_me;
1967 	}
1968 	s5k5baf_hw_init(state);
1969 	ret = s5k5baf_check_fw_revision(state);
1970 
1971 	s5k5baf_power_off(state);
1972 	if (ret < 0)
1973 		goto err_me;
1974 
1975 	ret = s5k5baf_initialize_ctrls(state);
1976 	if (ret < 0)
1977 		goto err_me;
1978 
1979 	ret = v4l2_async_register_subdev(&state->sd);
1980 	if (ret < 0)
1981 		goto err_ctrl;
1982 
1983 	return 0;
1984 
1985 err_ctrl:
1986 	v4l2_ctrl_handler_free(state->sd.ctrl_handler);
1987 err_me:
1988 	media_entity_cleanup(&state->sd.entity);
1989 	media_entity_cleanup(&state->cis_sd.entity);
1990 	return ret;
1991 }
1992 
1993 static void s5k5baf_remove(struct i2c_client *c)
1994 {
1995 	struct v4l2_subdev *sd = i2c_get_clientdata(c);
1996 	struct s5k5baf *state = to_s5k5baf(sd);
1997 
1998 	v4l2_async_unregister_subdev(sd);
1999 	v4l2_ctrl_handler_free(sd->ctrl_handler);
2000 	media_entity_cleanup(&sd->entity);
2001 
2002 	sd = &state->cis_sd;
2003 	v4l2_device_unregister_subdev(sd);
2004 	media_entity_cleanup(&sd->entity);
2005 }
2006 
2007 static const struct i2c_device_id s5k5baf_id[] = {
2008 	{ S5K5BAF_DRIVER_NAME, 0 },
2009 	{ },
2010 };
2011 MODULE_DEVICE_TABLE(i2c, s5k5baf_id);
2012 
2013 static const struct of_device_id s5k5baf_of_match[] = {
2014 	{ .compatible = "samsung,s5k5baf" },
2015 	{ }
2016 };
2017 MODULE_DEVICE_TABLE(of, s5k5baf_of_match);
2018 
2019 static struct i2c_driver s5k5baf_i2c_driver = {
2020 	.driver = {
2021 		.of_match_table = s5k5baf_of_match,
2022 		.name = S5K5BAF_DRIVER_NAME
2023 	},
2024 	.probe_new	= s5k5baf_probe,
2025 	.remove		= s5k5baf_remove,
2026 	.id_table	= s5k5baf_id,
2027 };
2028 
2029 module_i2c_driver(s5k5baf_i2c_driver);
2030 
2031 MODULE_DESCRIPTION("Samsung S5K5BAF(X) UXGA camera driver");
2032 MODULE_AUTHOR("Andrzej Hajda <a.hajda@samsung.com>");
2033 MODULE_LICENSE("GPL v2");
2034