1 /*
2  * Samsung LSI S5C73M3 8M pixel camera driver
3  *
4  * Copyright (C) 2012, Samsung Electronics, Co., Ltd.
5  * Sylwester Nawrocki <s.nawrocki@samsung.com>
6  * Andrzej Hajda <a.hajda@samsung.com>
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * version 2 as published by the Free Software Foundation.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  * GNU General Public License for more details.
16  */
17 
18 #include <linux/clk.h>
19 #include <linux/delay.h>
20 #include <linux/firmware.h>
21 #include <linux/gpio.h>
22 #include <linux/i2c.h>
23 #include <linux/init.h>
24 #include <linux/media.h>
25 #include <linux/module.h>
26 #include <linux/of_gpio.h>
27 #include <linux/of_graph.h>
28 #include <linux/regulator/consumer.h>
29 #include <linux/sizes.h>
30 #include <linux/slab.h>
31 #include <linux/spi/spi.h>
32 #include <linux/videodev2.h>
33 #include <media/media-entity.h>
34 #include <media/v4l2-ctrls.h>
35 #include <media/v4l2-device.h>
36 #include <media/v4l2-subdev.h>
37 #include <media/v4l2-mediabus.h>
38 #include <media/i2c/s5c73m3.h>
39 #include <media/v4l2-fwnode.h>
40 
41 #include "s5c73m3.h"
42 
43 int s5c73m3_dbg;
44 module_param_named(debug, s5c73m3_dbg, int, 0644);
45 
46 static int boot_from_rom = 1;
47 module_param(boot_from_rom, int, 0644);
48 
49 static int update_fw;
50 module_param(update_fw, int, 0644);
51 
52 #define S5C73M3_EMBEDDED_DATA_MAXLEN	SZ_4K
53 #define S5C73M3_MIPI_DATA_LANES		4
54 #define S5C73M3_CLK_NAME		"cis_extclk"
55 
56 static const char * const s5c73m3_supply_names[S5C73M3_MAX_SUPPLIES] = {
57 	"vdd-int",	/* Digital Core supply (1.2V), CAM_ISP_CORE_1.2V */
58 	"vdda",		/* Analog Core supply (1.2V), CAM_SENSOR_CORE_1.2V */
59 	"vdd-reg",	/* Regulator input supply (2.8V), CAM_SENSOR_A2.8V */
60 	"vddio-host",	/* Digital Host I/O power supply (1.8V...2.8V),
61 			   CAM_ISP_SENSOR_1.8V */
62 	"vddio-cis",	/* Digital CIS I/O power (1.2V...1.8V),
63 			   CAM_ISP_MIPI_1.2V */
64 	"vdd-af",	/* Lens, CAM_AF_2.8V */
65 };
66 
67 static const struct s5c73m3_frame_size s5c73m3_isp_resolutions[] = {
68 	{ 320,	240,	COMM_CHG_MODE_YUV_320_240 },
69 	{ 352,	288,	COMM_CHG_MODE_YUV_352_288 },
70 	{ 640,	480,	COMM_CHG_MODE_YUV_640_480 },
71 	{ 880,	720,	COMM_CHG_MODE_YUV_880_720 },
72 	{ 960,	720,	COMM_CHG_MODE_YUV_960_720 },
73 	{ 1008,	672,	COMM_CHG_MODE_YUV_1008_672 },
74 	{ 1184,	666,	COMM_CHG_MODE_YUV_1184_666 },
75 	{ 1280,	720,	COMM_CHG_MODE_YUV_1280_720 },
76 	{ 1536,	864,	COMM_CHG_MODE_YUV_1536_864 },
77 	{ 1600,	1200,	COMM_CHG_MODE_YUV_1600_1200 },
78 	{ 1632,	1224,	COMM_CHG_MODE_YUV_1632_1224 },
79 	{ 1920,	1080,	COMM_CHG_MODE_YUV_1920_1080 },
80 	{ 1920,	1440,	COMM_CHG_MODE_YUV_1920_1440 },
81 	{ 2304,	1296,	COMM_CHG_MODE_YUV_2304_1296 },
82 	{ 3264,	2448,	COMM_CHG_MODE_YUV_3264_2448 },
83 };
84 
85 static const struct s5c73m3_frame_size s5c73m3_jpeg_resolutions[] = {
86 	{ 640,	480,	COMM_CHG_MODE_JPEG_640_480 },
87 	{ 800,	450,	COMM_CHG_MODE_JPEG_800_450 },
88 	{ 800,	600,	COMM_CHG_MODE_JPEG_800_600 },
89 	{ 1024,	768,	COMM_CHG_MODE_JPEG_1024_768 },
90 	{ 1280,	720,	COMM_CHG_MODE_JPEG_1280_720 },
91 	{ 1280,	960,	COMM_CHG_MODE_JPEG_1280_960 },
92 	{ 1600,	900,	COMM_CHG_MODE_JPEG_1600_900 },
93 	{ 1600,	1200,	COMM_CHG_MODE_JPEG_1600_1200 },
94 	{ 2048,	1152,	COMM_CHG_MODE_JPEG_2048_1152 },
95 	{ 2048,	1536,	COMM_CHG_MODE_JPEG_2048_1536 },
96 	{ 2560,	1440,	COMM_CHG_MODE_JPEG_2560_1440 },
97 	{ 2560,	1920,	COMM_CHG_MODE_JPEG_2560_1920 },
98 	{ 3264,	1836,	COMM_CHG_MODE_JPEG_3264_1836 },
99 	{ 3264,	2176,	COMM_CHG_MODE_JPEG_3264_2176 },
100 	{ 3264,	2448,	COMM_CHG_MODE_JPEG_3264_2448 },
101 };
102 
103 static const struct s5c73m3_frame_size * const s5c73m3_resolutions[] = {
104 	[RES_ISP] = s5c73m3_isp_resolutions,
105 	[RES_JPEG] = s5c73m3_jpeg_resolutions
106 };
107 
108 static const int s5c73m3_resolutions_len[] = {
109 	[RES_ISP] = ARRAY_SIZE(s5c73m3_isp_resolutions),
110 	[RES_JPEG] = ARRAY_SIZE(s5c73m3_jpeg_resolutions)
111 };
112 
113 static const struct s5c73m3_interval s5c73m3_intervals[] = {
114 	{ COMM_FRAME_RATE_FIXED_7FPS, {142857, 1000000}, {3264, 2448} },
115 	{ COMM_FRAME_RATE_FIXED_15FPS, {66667, 1000000}, {3264, 2448} },
116 	{ COMM_FRAME_RATE_FIXED_20FPS, {50000, 1000000}, {2304, 1296} },
117 	{ COMM_FRAME_RATE_FIXED_30FPS, {33333, 1000000}, {2304, 1296} },
118 };
119 
120 #define S5C73M3_DEFAULT_FRAME_INTERVAL 3 /* 30 fps */
121 
122 static void s5c73m3_fill_mbus_fmt(struct v4l2_mbus_framefmt *mf,
123 				  const struct s5c73m3_frame_size *fs,
124 				  u32 code)
125 {
126 	mf->width = fs->width;
127 	mf->height = fs->height;
128 	mf->code = code;
129 	mf->colorspace = V4L2_COLORSPACE_JPEG;
130 	mf->field = V4L2_FIELD_NONE;
131 }
132 
133 static int s5c73m3_i2c_write(struct i2c_client *client, u16 addr, u16 data)
134 {
135 	u8 buf[4] = { addr >> 8, addr & 0xff, data >> 8, data & 0xff };
136 
137 	int ret = i2c_master_send(client, buf, sizeof(buf));
138 
139 	v4l_dbg(4, s5c73m3_dbg, client, "%s: addr 0x%04x, data 0x%04x\n",
140 		 __func__, addr, data);
141 
142 	if (ret == 4)
143 		return 0;
144 
145 	return ret < 0 ? ret : -EREMOTEIO;
146 }
147 
148 static int s5c73m3_i2c_read(struct i2c_client *client, u16 addr, u16 *data)
149 {
150 	int ret;
151 	u8 rbuf[2], wbuf[2] = { addr >> 8, addr & 0xff };
152 	struct i2c_msg msg[2] = {
153 		{
154 			.addr = client->addr,
155 			.flags = 0,
156 			.len = sizeof(wbuf),
157 			.buf = wbuf
158 		}, {
159 			.addr = client->addr,
160 			.flags = I2C_M_RD,
161 			.len = sizeof(rbuf),
162 			.buf = rbuf
163 		}
164 	};
165 	/*
166 	 * Issue repeated START after writing 2 address bytes and
167 	 * just one STOP only after reading the data bytes.
168 	 */
169 	ret = i2c_transfer(client->adapter, msg, 2);
170 	if (ret == 2) {
171 		*data = be16_to_cpup((__be16 *)rbuf);
172 		v4l2_dbg(4, s5c73m3_dbg, client,
173 			 "%s: addr: 0x%04x, data: 0x%04x\n",
174 			 __func__, addr, *data);
175 		return 0;
176 	}
177 
178 	v4l2_err(client, "I2C read failed: addr: %04x, (%d)\n", addr, ret);
179 
180 	return ret >= 0 ? -EREMOTEIO : ret;
181 }
182 
183 int s5c73m3_write(struct s5c73m3 *state, u32 addr, u16 data)
184 {
185 	struct i2c_client *client = state->i2c_client;
186 	int ret;
187 
188 	if ((addr ^ state->i2c_write_address) & 0xffff0000) {
189 		ret = s5c73m3_i2c_write(client, REG_CMDWR_ADDRH, addr >> 16);
190 		if (ret < 0) {
191 			state->i2c_write_address = 0;
192 			return ret;
193 		}
194 	}
195 
196 	if ((addr ^ state->i2c_write_address) & 0xffff) {
197 		ret = s5c73m3_i2c_write(client, REG_CMDWR_ADDRL, addr & 0xffff);
198 		if (ret < 0) {
199 			state->i2c_write_address = 0;
200 			return ret;
201 		}
202 	}
203 
204 	state->i2c_write_address = addr;
205 
206 	ret = s5c73m3_i2c_write(client, REG_CMDBUF_ADDR, data);
207 	if (ret < 0)
208 		return ret;
209 
210 	state->i2c_write_address += 2;
211 
212 	return ret;
213 }
214 
215 int s5c73m3_read(struct s5c73m3 *state, u32 addr, u16 *data)
216 {
217 	struct i2c_client *client = state->i2c_client;
218 	int ret;
219 
220 	if ((addr ^ state->i2c_read_address) & 0xffff0000) {
221 		ret = s5c73m3_i2c_write(client, REG_CMDRD_ADDRH, addr >> 16);
222 		if (ret < 0) {
223 			state->i2c_read_address = 0;
224 			return ret;
225 		}
226 	}
227 
228 	if ((addr ^ state->i2c_read_address) & 0xffff) {
229 		ret = s5c73m3_i2c_write(client, REG_CMDRD_ADDRL, addr & 0xffff);
230 		if (ret < 0) {
231 			state->i2c_read_address = 0;
232 			return ret;
233 		}
234 	}
235 
236 	state->i2c_read_address = addr;
237 
238 	ret = s5c73m3_i2c_read(client, REG_CMDBUF_ADDR, data);
239 	if (ret < 0)
240 		return ret;
241 
242 	state->i2c_read_address += 2;
243 
244 	return ret;
245 }
246 
247 static int s5c73m3_check_status(struct s5c73m3 *state, unsigned int value)
248 {
249 	unsigned long start = jiffies;
250 	unsigned long end = start + msecs_to_jiffies(2000);
251 	int ret = 0;
252 	u16 status;
253 	int count = 0;
254 
255 	while (time_is_after_jiffies(end)) {
256 		ret = s5c73m3_read(state, REG_STATUS, &status);
257 		if (ret < 0 || status == value)
258 			break;
259 		usleep_range(500, 1000);
260 		++count;
261 	}
262 
263 	if (count > 0)
264 		v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
265 			 "status check took %dms\n",
266 			 jiffies_to_msecs(jiffies - start));
267 
268 	if (ret == 0 && status != value) {
269 		u16 i2c_status = 0;
270 		u16 i2c_seq_status = 0;
271 
272 		s5c73m3_read(state, REG_I2C_STATUS, &i2c_status);
273 		s5c73m3_read(state, REG_I2C_SEQ_STATUS, &i2c_seq_status);
274 
275 		v4l2_err(&state->sensor_sd,
276 			 "wrong status %#x, expected: %#x, i2c_status: %#x/%#x\n",
277 			 status, value, i2c_status, i2c_seq_status);
278 
279 		return -ETIMEDOUT;
280 	}
281 
282 	return ret;
283 }
284 
285 int s5c73m3_isp_command(struct s5c73m3 *state, u16 command, u16 data)
286 {
287 	int ret;
288 
289 	ret = s5c73m3_check_status(state, REG_STATUS_ISP_COMMAND_COMPLETED);
290 	if (ret < 0)
291 		return ret;
292 
293 	ret = s5c73m3_write(state, 0x00095000, command);
294 	if (ret < 0)
295 		return ret;
296 
297 	ret = s5c73m3_write(state, 0x00095002, data);
298 	if (ret < 0)
299 		return ret;
300 
301 	return s5c73m3_write(state, REG_STATUS, 0x0001);
302 }
303 
304 static int s5c73m3_isp_comm_result(struct s5c73m3 *state, u16 command,
305 				   u16 *data)
306 {
307 	return s5c73m3_read(state, COMM_RESULT_OFFSET + command, data);
308 }
309 
310 static int s5c73m3_set_af_softlanding(struct s5c73m3 *state)
311 {
312 	unsigned long start = jiffies;
313 	u16 af_softlanding;
314 	int count = 0;
315 	int ret;
316 	const char *msg;
317 
318 	ret = s5c73m3_isp_command(state, COMM_AF_SOFTLANDING,
319 					COMM_AF_SOFTLANDING_ON);
320 	if (ret < 0) {
321 		v4l2_info(&state->sensor_sd, "AF soft-landing failed\n");
322 		return ret;
323 	}
324 
325 	for (;;) {
326 		ret = s5c73m3_isp_comm_result(state, COMM_AF_SOFTLANDING,
327 							&af_softlanding);
328 		if (ret < 0) {
329 			msg = "failed";
330 			break;
331 		}
332 		if (af_softlanding == COMM_AF_SOFTLANDING_RES_COMPLETE) {
333 			msg = "succeeded";
334 			break;
335 		}
336 		if (++count > 100) {
337 			ret = -ETIME;
338 			msg = "timed out";
339 			break;
340 		}
341 		msleep(25);
342 	}
343 
344 	v4l2_info(&state->sensor_sd, "AF soft-landing %s after %dms\n",
345 		  msg, jiffies_to_msecs(jiffies - start));
346 
347 	return ret;
348 }
349 
350 static int s5c73m3_load_fw(struct v4l2_subdev *sd)
351 {
352 	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
353 	struct i2c_client *client = state->i2c_client;
354 	const struct firmware *fw;
355 	int ret;
356 	char fw_name[20];
357 
358 	snprintf(fw_name, sizeof(fw_name), "SlimISP_%.2s.bin",
359 							state->fw_file_version);
360 	ret = request_firmware(&fw, fw_name, &client->dev);
361 	if (ret < 0) {
362 		v4l2_err(sd, "Firmware request failed (%s)\n", fw_name);
363 		return -EINVAL;
364 	}
365 
366 	v4l2_info(sd, "Loading firmware (%s, %zu B)\n", fw_name, fw->size);
367 
368 	ret = s5c73m3_spi_write(state, fw->data, fw->size, 64);
369 
370 	if (ret >= 0)
371 		state->isp_ready = 1;
372 	else
373 		v4l2_err(sd, "SPI write failed\n");
374 
375 	release_firmware(fw);
376 
377 	return ret;
378 }
379 
380 static int s5c73m3_set_frame_size(struct s5c73m3 *state)
381 {
382 	const struct s5c73m3_frame_size *prev_size =
383 					state->sensor_pix_size[RES_ISP];
384 	const struct s5c73m3_frame_size *cap_size =
385 					state->sensor_pix_size[RES_JPEG];
386 	unsigned int chg_mode;
387 
388 	v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
389 		 "Preview size: %dx%d, reg_val: 0x%x\n",
390 		 prev_size->width, prev_size->height, prev_size->reg_val);
391 
392 	chg_mode = prev_size->reg_val | COMM_CHG_MODE_NEW;
393 
394 	if (state->mbus_code == S5C73M3_JPEG_FMT) {
395 		v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
396 			 "Capture size: %dx%d, reg_val: 0x%x\n",
397 			 cap_size->width, cap_size->height, cap_size->reg_val);
398 		chg_mode |= cap_size->reg_val;
399 	}
400 
401 	return s5c73m3_isp_command(state, COMM_CHG_MODE, chg_mode);
402 }
403 
404 static int s5c73m3_set_frame_rate(struct s5c73m3 *state)
405 {
406 	int ret;
407 
408 	if (state->ctrls.stabilization->val)
409 		return 0;
410 
411 	if (WARN_ON(state->fiv == NULL))
412 		return -EINVAL;
413 
414 	ret = s5c73m3_isp_command(state, COMM_FRAME_RATE, state->fiv->fps_reg);
415 	if (!ret)
416 		state->apply_fiv = 0;
417 
418 	return ret;
419 }
420 
421 static int __s5c73m3_s_stream(struct s5c73m3 *state, struct v4l2_subdev *sd,
422 								int on)
423 {
424 	u16 mode;
425 	int ret;
426 
427 	if (on && state->apply_fmt) {
428 		if (state->mbus_code == S5C73M3_JPEG_FMT)
429 			mode = COMM_IMG_OUTPUT_INTERLEAVED;
430 		else
431 			mode = COMM_IMG_OUTPUT_YUV;
432 
433 		ret = s5c73m3_isp_command(state, COMM_IMG_OUTPUT, mode);
434 		if (!ret)
435 			ret = s5c73m3_set_frame_size(state);
436 		if (ret)
437 			return ret;
438 		state->apply_fmt = 0;
439 	}
440 
441 	ret = s5c73m3_isp_command(state, COMM_SENSOR_STREAMING, !!on);
442 	if (ret)
443 		return ret;
444 
445 	state->streaming = !!on;
446 
447 	if (!on)
448 		return ret;
449 
450 	if (state->apply_fiv) {
451 		ret = s5c73m3_set_frame_rate(state);
452 		if (ret < 0)
453 			v4l2_err(sd, "Error setting frame rate(%d)\n", ret);
454 	}
455 
456 	return s5c73m3_check_status(state, REG_STATUS_ISP_COMMAND_COMPLETED);
457 }
458 
459 static int s5c73m3_oif_s_stream(struct v4l2_subdev *sd, int on)
460 {
461 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
462 	int ret;
463 
464 	mutex_lock(&state->lock);
465 	ret = __s5c73m3_s_stream(state, sd, on);
466 	mutex_unlock(&state->lock);
467 
468 	return ret;
469 }
470 
471 static int s5c73m3_system_status_wait(struct s5c73m3 *state, u32 value,
472 				      unsigned int delay, unsigned int steps)
473 {
474 	u16 reg = 0;
475 
476 	while (steps-- > 0) {
477 		int ret = s5c73m3_read(state, 0x30100010, &reg);
478 		if (ret < 0)
479 			return ret;
480 		if (reg == value)
481 			return 0;
482 		usleep_range(delay, delay + 25);
483 	}
484 	return -ETIMEDOUT;
485 }
486 
487 static int s5c73m3_read_fw_version(struct s5c73m3 *state)
488 {
489 	struct v4l2_subdev *sd = &state->sensor_sd;
490 	int i, ret;
491 	u16 data[2];
492 	int offset;
493 
494 	offset = state->isp_ready ? 0x60 : 0;
495 
496 	for (i = 0; i < S5C73M3_SENSOR_FW_LEN / 2; i++) {
497 		ret = s5c73m3_read(state, offset + i * 2, data);
498 		if (ret < 0)
499 			return ret;
500 		state->sensor_fw[i * 2] = (char)(*data & 0xff);
501 		state->sensor_fw[i * 2 + 1] = (char)(*data >> 8);
502 	}
503 	state->sensor_fw[S5C73M3_SENSOR_FW_LEN] = '\0';
504 
505 
506 	for (i = 0; i < S5C73M3_SENSOR_TYPE_LEN / 2; i++) {
507 		ret = s5c73m3_read(state, offset + 6 + i * 2, data);
508 		if (ret < 0)
509 			return ret;
510 		state->sensor_type[i * 2] = (char)(*data & 0xff);
511 		state->sensor_type[i * 2 + 1] = (char)(*data >> 8);
512 	}
513 	state->sensor_type[S5C73M3_SENSOR_TYPE_LEN] = '\0';
514 
515 	ret = s5c73m3_read(state, offset + 0x14, data);
516 	if (ret >= 0) {
517 		ret = s5c73m3_read(state, offset + 0x16, data + 1);
518 		if (ret >= 0)
519 			state->fw_size = data[0] + (data[1] << 16);
520 	}
521 
522 	v4l2_info(sd, "Sensor type: %s, FW version: %s\n",
523 		  state->sensor_type, state->sensor_fw);
524 	return ret;
525 }
526 
527 static int s5c73m3_fw_update_from(struct s5c73m3 *state)
528 {
529 	struct v4l2_subdev *sd = &state->sensor_sd;
530 	u16 status = COMM_FW_UPDATE_NOT_READY;
531 	int ret;
532 	int count = 0;
533 
534 	v4l2_warn(sd, "Updating F-ROM firmware.\n");
535 	do {
536 		if (status == COMM_FW_UPDATE_NOT_READY) {
537 			ret = s5c73m3_isp_command(state, COMM_FW_UPDATE, 0);
538 			if (ret < 0)
539 				return ret;
540 		}
541 
542 		ret = s5c73m3_read(state, 0x00095906, &status);
543 		if (ret < 0)
544 			return ret;
545 		switch (status) {
546 		case COMM_FW_UPDATE_FAIL:
547 			v4l2_warn(sd, "Updating F-ROM firmware failed.\n");
548 			return -EIO;
549 		case COMM_FW_UPDATE_SUCCESS:
550 			v4l2_warn(sd, "Updating F-ROM firmware finished.\n");
551 			return 0;
552 		}
553 		++count;
554 		msleep(20);
555 	} while (count < 500);
556 
557 	v4l2_warn(sd, "Updating F-ROM firmware timed-out.\n");
558 	return -ETIMEDOUT;
559 }
560 
561 static int s5c73m3_spi_boot(struct s5c73m3 *state, bool load_fw)
562 {
563 	struct v4l2_subdev *sd = &state->sensor_sd;
564 	int ret;
565 
566 	/* Run ARM MCU */
567 	ret = s5c73m3_write(state, 0x30000004, 0xffff);
568 	if (ret < 0)
569 		return ret;
570 
571 	usleep_range(400, 500);
572 
573 	/* Check booting status */
574 	ret = s5c73m3_system_status_wait(state, 0x0c, 100, 3);
575 	if (ret < 0) {
576 		v4l2_err(sd, "booting failed: %d\n", ret);
577 		return ret;
578 	}
579 
580 	/* P,M,S and Boot Mode */
581 	ret = s5c73m3_write(state, 0x30100014, 0x2146);
582 	if (ret < 0)
583 		return ret;
584 
585 	ret = s5c73m3_write(state, 0x30100010, 0x210c);
586 	if (ret < 0)
587 		return ret;
588 
589 	usleep_range(200, 250);
590 
591 	/* Check SPI status */
592 	ret = s5c73m3_system_status_wait(state, 0x210d, 100, 300);
593 	if (ret < 0)
594 		v4l2_err(sd, "SPI not ready: %d\n", ret);
595 
596 	/* Firmware download over SPI */
597 	if (load_fw)
598 		s5c73m3_load_fw(sd);
599 
600 	/* MCU reset */
601 	ret = s5c73m3_write(state, 0x30000004, 0xfffd);
602 	if (ret < 0)
603 		return ret;
604 
605 	/* Remap */
606 	ret = s5c73m3_write(state, 0x301000a4, 0x0183);
607 	if (ret < 0)
608 		return ret;
609 
610 	/* MCU restart */
611 	ret = s5c73m3_write(state, 0x30000004, 0xffff);
612 	if (ret < 0 || !load_fw)
613 		return ret;
614 
615 	ret = s5c73m3_read_fw_version(state);
616 	if (ret < 0)
617 		return ret;
618 
619 	if (load_fw && update_fw) {
620 		ret = s5c73m3_fw_update_from(state);
621 		update_fw = 0;
622 	}
623 
624 	return ret;
625 }
626 
627 static int s5c73m3_set_timing_register_for_vdd(struct s5c73m3 *state)
628 {
629 	static const u32 regs[][2] = {
630 		{ 0x30100018, 0x0618 },
631 		{ 0x3010001c, 0x10c1 },
632 		{ 0x30100020, 0x249e }
633 	};
634 	int ret;
635 	int i;
636 
637 	for (i = 0; i < ARRAY_SIZE(regs); i++) {
638 		ret = s5c73m3_write(state, regs[i][0], regs[i][1]);
639 		if (ret < 0)
640 			return ret;
641 	}
642 
643 	return 0;
644 }
645 
646 static void s5c73m3_set_fw_file_version(struct s5c73m3 *state)
647 {
648 	switch (state->sensor_fw[0]) {
649 	case 'G':
650 	case 'O':
651 		state->fw_file_version[0] = 'G';
652 		break;
653 	case 'S':
654 	case 'Z':
655 		state->fw_file_version[0] = 'Z';
656 		break;
657 	}
658 
659 	switch (state->sensor_fw[1]) {
660 	case 'C'...'F':
661 		state->fw_file_version[1] = state->sensor_fw[1];
662 		break;
663 	}
664 }
665 
666 static int s5c73m3_get_fw_version(struct s5c73m3 *state)
667 {
668 	struct v4l2_subdev *sd = &state->sensor_sd;
669 	int ret;
670 
671 	/* Run ARM MCU */
672 	ret = s5c73m3_write(state, 0x30000004, 0xffff);
673 	if (ret < 0)
674 		return ret;
675 	usleep_range(400, 500);
676 
677 	/* Check booting status */
678 	ret = s5c73m3_system_status_wait(state, 0x0c, 100, 3);
679 	if (ret < 0) {
680 
681 		v4l2_err(sd, "%s: booting failed: %d\n", __func__, ret);
682 		return ret;
683 	}
684 
685 	/* Change I/O Driver Current in order to read from F-ROM */
686 	ret = s5c73m3_write(state, 0x30100120, 0x0820);
687 	ret = s5c73m3_write(state, 0x30100124, 0x0820);
688 
689 	/* Offset Setting */
690 	ret = s5c73m3_write(state, 0x00010418, 0x0008);
691 
692 	/* P,M,S and Boot Mode */
693 	ret = s5c73m3_write(state, 0x30100014, 0x2146);
694 	if (ret < 0)
695 		return ret;
696 	ret = s5c73m3_write(state, 0x30100010, 0x230c);
697 	if (ret < 0)
698 		return ret;
699 
700 	usleep_range(200, 250);
701 
702 	/* Check SPI status */
703 	ret = s5c73m3_system_status_wait(state, 0x230e, 100, 300);
704 	if (ret < 0)
705 		v4l2_err(sd, "SPI not ready: %d\n", ret);
706 
707 	/* ARM reset */
708 	ret = s5c73m3_write(state, 0x30000004, 0xfffd);
709 	if (ret < 0)
710 		return ret;
711 
712 	/* Remap */
713 	ret = s5c73m3_write(state, 0x301000a4, 0x0183);
714 	if (ret < 0)
715 		return ret;
716 
717 	s5c73m3_set_timing_register_for_vdd(state);
718 
719 	ret = s5c73m3_read_fw_version(state);
720 
721 	s5c73m3_set_fw_file_version(state);
722 
723 	return ret;
724 }
725 
726 static int s5c73m3_rom_boot(struct s5c73m3 *state, bool load_fw)
727 {
728 	static const u32 boot_regs[][2] = {
729 		{ 0x3100010c, 0x0044 },
730 		{ 0x31000108, 0x000d },
731 		{ 0x31000304, 0x0001 },
732 		{ 0x00010000, 0x5800 },
733 		{ 0x00010002, 0x0002 },
734 		{ 0x31000000, 0x0001 },
735 		{ 0x30100014, 0x1b85 },
736 		{ 0x30100010, 0x230c }
737 	};
738 	struct v4l2_subdev *sd = &state->sensor_sd;
739 	int i, ret;
740 
741 	/* Run ARM MCU */
742 	ret = s5c73m3_write(state, 0x30000004, 0xffff);
743 	if (ret < 0)
744 		return ret;
745 	usleep_range(400, 450);
746 
747 	/* Check booting status */
748 	ret = s5c73m3_system_status_wait(state, 0x0c, 100, 4);
749 	if (ret < 0) {
750 		v4l2_err(sd, "Booting failed: %d\n", ret);
751 		return ret;
752 	}
753 
754 	for (i = 0; i < ARRAY_SIZE(boot_regs); i++) {
755 		ret = s5c73m3_write(state, boot_regs[i][0], boot_regs[i][1]);
756 		if (ret < 0)
757 			return ret;
758 	}
759 	msleep(200);
760 
761 	/* Check the binary read status */
762 	ret = s5c73m3_system_status_wait(state, 0x230e, 1000, 150);
763 	if (ret < 0) {
764 		v4l2_err(sd, "Binary read failed: %d\n", ret);
765 		return ret;
766 	}
767 
768 	/* ARM reset */
769 	ret = s5c73m3_write(state, 0x30000004, 0xfffd);
770 	if (ret < 0)
771 		return ret;
772 	/* Remap */
773 	ret = s5c73m3_write(state, 0x301000a4, 0x0183);
774 	if (ret < 0)
775 		return ret;
776 	/* MCU re-start */
777 	ret = s5c73m3_write(state, 0x30000004, 0xffff);
778 	if (ret < 0)
779 		return ret;
780 
781 	state->isp_ready = 1;
782 
783 	return s5c73m3_read_fw_version(state);
784 }
785 
786 static int s5c73m3_isp_init(struct s5c73m3 *state)
787 {
788 	int ret;
789 
790 	state->i2c_read_address = 0;
791 	state->i2c_write_address = 0;
792 
793 	ret = s5c73m3_i2c_write(state->i2c_client, AHB_MSB_ADDR_PTR, 0x3310);
794 	if (ret < 0)
795 		return ret;
796 
797 	if (boot_from_rom)
798 		return s5c73m3_rom_boot(state, true);
799 	else
800 		return s5c73m3_spi_boot(state, true);
801 }
802 
803 static const struct s5c73m3_frame_size *s5c73m3_find_frame_size(
804 					struct v4l2_mbus_framefmt *fmt,
805 					enum s5c73m3_resolution_types idx)
806 {
807 	const struct s5c73m3_frame_size *fs;
808 	const struct s5c73m3_frame_size *best_fs;
809 	int best_dist = INT_MAX;
810 	int i;
811 
812 	fs = s5c73m3_resolutions[idx];
813 	best_fs = NULL;
814 	for (i = 0; i < s5c73m3_resolutions_len[idx]; ++i) {
815 		int dist = abs(fs->width - fmt->width) +
816 						abs(fs->height - fmt->height);
817 		if (dist < best_dist) {
818 			best_dist = dist;
819 			best_fs = fs;
820 		}
821 		++fs;
822 	}
823 
824 	return best_fs;
825 }
826 
827 static void s5c73m3_oif_try_format(struct s5c73m3 *state,
828 				   struct v4l2_subdev_pad_config *cfg,
829 				   struct v4l2_subdev_format *fmt,
830 				   const struct s5c73m3_frame_size **fs)
831 {
832 	struct v4l2_subdev *sd = &state->sensor_sd;
833 	u32 code;
834 
835 	switch (fmt->pad) {
836 	case OIF_ISP_PAD:
837 		*fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
838 		code = S5C73M3_ISP_FMT;
839 		break;
840 	case OIF_JPEG_PAD:
841 		*fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
842 		code = S5C73M3_JPEG_FMT;
843 		break;
844 	case OIF_SOURCE_PAD:
845 	default:
846 		if (fmt->format.code == S5C73M3_JPEG_FMT)
847 			code = S5C73M3_JPEG_FMT;
848 		else
849 			code = S5C73M3_ISP_FMT;
850 
851 		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
852 			*fs = state->oif_pix_size[RES_ISP];
853 		else
854 			*fs = s5c73m3_find_frame_size(
855 						v4l2_subdev_get_try_format(sd, cfg,
856 							OIF_ISP_PAD),
857 						RES_ISP);
858 		break;
859 	}
860 
861 	s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
862 }
863 
864 static void s5c73m3_try_format(struct s5c73m3 *state,
865 			      struct v4l2_subdev_pad_config *cfg,
866 			      struct v4l2_subdev_format *fmt,
867 			      const struct s5c73m3_frame_size **fs)
868 {
869 	u32 code;
870 
871 	if (fmt->pad == S5C73M3_ISP_PAD) {
872 		*fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
873 		code = S5C73M3_ISP_FMT;
874 	} else {
875 		*fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
876 		code = S5C73M3_JPEG_FMT;
877 	}
878 
879 	s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
880 }
881 
882 static int s5c73m3_oif_g_frame_interval(struct v4l2_subdev *sd,
883 				   struct v4l2_subdev_frame_interval *fi)
884 {
885 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
886 
887 	if (fi->pad != OIF_SOURCE_PAD)
888 		return -EINVAL;
889 
890 	mutex_lock(&state->lock);
891 	fi->interval = state->fiv->interval;
892 	mutex_unlock(&state->lock);
893 
894 	return 0;
895 }
896 
897 static int __s5c73m3_set_frame_interval(struct s5c73m3 *state,
898 					struct v4l2_subdev_frame_interval *fi)
899 {
900 	const struct s5c73m3_frame_size *prev_size =
901 						state->sensor_pix_size[RES_ISP];
902 	const struct s5c73m3_interval *fiv = &s5c73m3_intervals[0];
903 	unsigned int ret, min_err = UINT_MAX;
904 	unsigned int i, fr_time;
905 
906 	if (fi->interval.denominator == 0)
907 		return -EINVAL;
908 
909 	fr_time = fi->interval.numerator * 1000 / fi->interval.denominator;
910 
911 	for (i = 0; i < ARRAY_SIZE(s5c73m3_intervals); i++) {
912 		const struct s5c73m3_interval *iv = &s5c73m3_intervals[i];
913 
914 		if (prev_size->width > iv->size.width ||
915 		    prev_size->height > iv->size.height)
916 			continue;
917 
918 		ret = abs(iv->interval.numerator / 1000 - fr_time);
919 		if (ret < min_err) {
920 			fiv = iv;
921 			min_err = ret;
922 		}
923 	}
924 	state->fiv = fiv;
925 
926 	v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
927 		 "Changed frame interval to %u us\n", fiv->interval.numerator);
928 	return 0;
929 }
930 
931 static int s5c73m3_oif_s_frame_interval(struct v4l2_subdev *sd,
932 				   struct v4l2_subdev_frame_interval *fi)
933 {
934 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
935 	int ret;
936 
937 	if (fi->pad != OIF_SOURCE_PAD)
938 		return -EINVAL;
939 
940 	v4l2_dbg(1, s5c73m3_dbg, sd, "Setting %d/%d frame interval\n",
941 		 fi->interval.numerator, fi->interval.denominator);
942 
943 	mutex_lock(&state->lock);
944 
945 	ret = __s5c73m3_set_frame_interval(state, fi);
946 	if (!ret) {
947 		if (state->streaming)
948 			ret = s5c73m3_set_frame_rate(state);
949 		else
950 			state->apply_fiv = 1;
951 	}
952 	mutex_unlock(&state->lock);
953 	return ret;
954 }
955 
956 static int s5c73m3_oif_enum_frame_interval(struct v4l2_subdev *sd,
957 			      struct v4l2_subdev_pad_config *cfg,
958 			      struct v4l2_subdev_frame_interval_enum *fie)
959 {
960 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
961 	const struct s5c73m3_interval *fi;
962 	int ret = 0;
963 
964 	if (fie->pad != OIF_SOURCE_PAD)
965 		return -EINVAL;
966 	if (fie->index >= ARRAY_SIZE(s5c73m3_intervals))
967 		return -EINVAL;
968 
969 	mutex_lock(&state->lock);
970 	fi = &s5c73m3_intervals[fie->index];
971 	if (fie->width > fi->size.width || fie->height > fi->size.height)
972 		ret = -EINVAL;
973 	else
974 		fie->interval = fi->interval;
975 	mutex_unlock(&state->lock);
976 
977 	return ret;
978 }
979 
980 static int s5c73m3_oif_get_pad_code(int pad, int index)
981 {
982 	if (pad == OIF_SOURCE_PAD) {
983 		if (index > 1)
984 			return -EINVAL;
985 		return (index == 0) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
986 	}
987 
988 	if (index > 0)
989 		return -EINVAL;
990 
991 	return (pad == OIF_ISP_PAD) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
992 }
993 
994 static int s5c73m3_get_fmt(struct v4l2_subdev *sd,
995 			   struct v4l2_subdev_pad_config *cfg,
996 			   struct v4l2_subdev_format *fmt)
997 {
998 	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
999 	const struct s5c73m3_frame_size *fs;
1000 	u32 code;
1001 
1002 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1003 		fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1004 		return 0;
1005 	}
1006 
1007 	mutex_lock(&state->lock);
1008 
1009 	switch (fmt->pad) {
1010 	case S5C73M3_ISP_PAD:
1011 		code = S5C73M3_ISP_FMT;
1012 		fs = state->sensor_pix_size[RES_ISP];
1013 		break;
1014 	case S5C73M3_JPEG_PAD:
1015 		code = S5C73M3_JPEG_FMT;
1016 		fs = state->sensor_pix_size[RES_JPEG];
1017 		break;
1018 	default:
1019 		mutex_unlock(&state->lock);
1020 		return -EINVAL;
1021 	}
1022 	s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1023 
1024 	mutex_unlock(&state->lock);
1025 	return 0;
1026 }
1027 
1028 static int s5c73m3_oif_get_fmt(struct v4l2_subdev *sd,
1029 			   struct v4l2_subdev_pad_config *cfg,
1030 			   struct v4l2_subdev_format *fmt)
1031 {
1032 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1033 	const struct s5c73m3_frame_size *fs;
1034 	u32 code;
1035 
1036 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1037 		fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1038 		return 0;
1039 	}
1040 
1041 	mutex_lock(&state->lock);
1042 
1043 	switch (fmt->pad) {
1044 	case OIF_ISP_PAD:
1045 		code = S5C73M3_ISP_FMT;
1046 		fs = state->oif_pix_size[RES_ISP];
1047 		break;
1048 	case OIF_JPEG_PAD:
1049 		code = S5C73M3_JPEG_FMT;
1050 		fs = state->oif_pix_size[RES_JPEG];
1051 		break;
1052 	case OIF_SOURCE_PAD:
1053 		code = state->mbus_code;
1054 		fs = state->oif_pix_size[RES_ISP];
1055 		break;
1056 	default:
1057 		mutex_unlock(&state->lock);
1058 		return -EINVAL;
1059 	}
1060 	s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1061 
1062 	mutex_unlock(&state->lock);
1063 	return 0;
1064 }
1065 
1066 static int s5c73m3_set_fmt(struct v4l2_subdev *sd,
1067 			   struct v4l2_subdev_pad_config *cfg,
1068 			   struct v4l2_subdev_format *fmt)
1069 {
1070 	const struct s5c73m3_frame_size *frame_size = NULL;
1071 	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
1072 	struct v4l2_mbus_framefmt *mf;
1073 	int ret = 0;
1074 
1075 	mutex_lock(&state->lock);
1076 
1077 	s5c73m3_try_format(state, cfg, fmt, &frame_size);
1078 
1079 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1080 		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1081 		*mf = fmt->format;
1082 	} else {
1083 		switch (fmt->pad) {
1084 		case S5C73M3_ISP_PAD:
1085 			state->sensor_pix_size[RES_ISP] = frame_size;
1086 			break;
1087 		case S5C73M3_JPEG_PAD:
1088 			state->sensor_pix_size[RES_JPEG] = frame_size;
1089 			break;
1090 		default:
1091 			ret = -EBUSY;
1092 		}
1093 
1094 		if (state->streaming)
1095 			ret = -EBUSY;
1096 		else
1097 			state->apply_fmt = 1;
1098 	}
1099 
1100 	mutex_unlock(&state->lock);
1101 
1102 	return ret;
1103 }
1104 
1105 static int s5c73m3_oif_set_fmt(struct v4l2_subdev *sd,
1106 			 struct v4l2_subdev_pad_config *cfg,
1107 			 struct v4l2_subdev_format *fmt)
1108 {
1109 	const struct s5c73m3_frame_size *frame_size = NULL;
1110 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1111 	struct v4l2_mbus_framefmt *mf;
1112 	int ret = 0;
1113 
1114 	mutex_lock(&state->lock);
1115 
1116 	s5c73m3_oif_try_format(state, cfg, fmt, &frame_size);
1117 
1118 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1119 		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1120 		*mf = fmt->format;
1121 		if (fmt->pad == OIF_ISP_PAD) {
1122 			mf = v4l2_subdev_get_try_format(sd, cfg, OIF_SOURCE_PAD);
1123 			mf->width = fmt->format.width;
1124 			mf->height = fmt->format.height;
1125 		}
1126 	} else {
1127 		switch (fmt->pad) {
1128 		case OIF_ISP_PAD:
1129 			state->oif_pix_size[RES_ISP] = frame_size;
1130 			break;
1131 		case OIF_JPEG_PAD:
1132 			state->oif_pix_size[RES_JPEG] = frame_size;
1133 			break;
1134 		case OIF_SOURCE_PAD:
1135 			state->mbus_code = fmt->format.code;
1136 			break;
1137 		default:
1138 			ret = -EBUSY;
1139 		}
1140 
1141 		if (state->streaming)
1142 			ret = -EBUSY;
1143 		else
1144 			state->apply_fmt = 1;
1145 	}
1146 
1147 	mutex_unlock(&state->lock);
1148 
1149 	return ret;
1150 }
1151 
1152 static int s5c73m3_oif_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1153 				  struct v4l2_mbus_frame_desc *fd)
1154 {
1155 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1156 	int i;
1157 
1158 	if (pad != OIF_SOURCE_PAD || fd == NULL)
1159 		return -EINVAL;
1160 
1161 	mutex_lock(&state->lock);
1162 	fd->num_entries = 2;
1163 	for (i = 0; i < fd->num_entries; i++)
1164 		fd->entry[i] = state->frame_desc.entry[i];
1165 	mutex_unlock(&state->lock);
1166 
1167 	return 0;
1168 }
1169 
1170 static int s5c73m3_oif_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1171 				      struct v4l2_mbus_frame_desc *fd)
1172 {
1173 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1174 	struct v4l2_mbus_frame_desc *frame_desc = &state->frame_desc;
1175 	int i;
1176 
1177 	if (pad != OIF_SOURCE_PAD || fd == NULL)
1178 		return -EINVAL;
1179 
1180 	fd->entry[0].length = 10 * SZ_1M;
1181 	fd->entry[1].length = max_t(u32, fd->entry[1].length,
1182 				    S5C73M3_EMBEDDED_DATA_MAXLEN);
1183 	fd->num_entries = 2;
1184 
1185 	mutex_lock(&state->lock);
1186 	for (i = 0; i < fd->num_entries; i++)
1187 		frame_desc->entry[i] = fd->entry[i];
1188 	mutex_unlock(&state->lock);
1189 
1190 	return 0;
1191 }
1192 
1193 static int s5c73m3_enum_mbus_code(struct v4l2_subdev *sd,
1194 				  struct v4l2_subdev_pad_config *cfg,
1195 				  struct v4l2_subdev_mbus_code_enum *code)
1196 {
1197 	static const int codes[] = {
1198 			[S5C73M3_ISP_PAD] = S5C73M3_ISP_FMT,
1199 			[S5C73M3_JPEG_PAD] = S5C73M3_JPEG_FMT};
1200 
1201 	if (code->index > 0 || code->pad >= S5C73M3_NUM_PADS)
1202 		return -EINVAL;
1203 
1204 	code->code = codes[code->pad];
1205 
1206 	return 0;
1207 }
1208 
1209 static int s5c73m3_oif_enum_mbus_code(struct v4l2_subdev *sd,
1210 				struct v4l2_subdev_pad_config *cfg,
1211 				struct v4l2_subdev_mbus_code_enum *code)
1212 {
1213 	int ret;
1214 
1215 	ret = s5c73m3_oif_get_pad_code(code->pad, code->index);
1216 	if (ret < 0)
1217 		return ret;
1218 
1219 	code->code = ret;
1220 
1221 	return 0;
1222 }
1223 
1224 static int s5c73m3_enum_frame_size(struct v4l2_subdev *sd,
1225 				   struct v4l2_subdev_pad_config *cfg,
1226 				   struct v4l2_subdev_frame_size_enum *fse)
1227 {
1228 	int idx;
1229 
1230 	if (fse->pad == S5C73M3_ISP_PAD) {
1231 		if (fse->code != S5C73M3_ISP_FMT)
1232 			return -EINVAL;
1233 		idx = RES_ISP;
1234 	} else{
1235 		if (fse->code != S5C73M3_JPEG_FMT)
1236 			return -EINVAL;
1237 		idx = RES_JPEG;
1238 	}
1239 
1240 	if (fse->index >= s5c73m3_resolutions_len[idx])
1241 		return -EINVAL;
1242 
1243 	fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1244 	fse->max_width  = fse->min_width;
1245 	fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1246 	fse->min_height = fse->max_height;
1247 
1248 	return 0;
1249 }
1250 
1251 static int s5c73m3_oif_enum_frame_size(struct v4l2_subdev *sd,
1252 				   struct v4l2_subdev_pad_config *cfg,
1253 				   struct v4l2_subdev_frame_size_enum *fse)
1254 {
1255 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1256 	int idx;
1257 
1258 	if (fse->pad == OIF_SOURCE_PAD) {
1259 		if (fse->index > 0)
1260 			return -EINVAL;
1261 
1262 		switch (fse->code) {
1263 		case S5C73M3_JPEG_FMT:
1264 		case S5C73M3_ISP_FMT: {
1265 			unsigned w, h;
1266 
1267 			if (fse->which == V4L2_SUBDEV_FORMAT_TRY) {
1268 				struct v4l2_mbus_framefmt *mf;
1269 
1270 				mf = v4l2_subdev_get_try_format(sd, cfg,
1271 								OIF_ISP_PAD);
1272 
1273 				w = mf->width;
1274 				h = mf->height;
1275 			} else {
1276 				const struct s5c73m3_frame_size *fs;
1277 
1278 				fs = state->oif_pix_size[RES_ISP];
1279 				w = fs->width;
1280 				h = fs->height;
1281 			}
1282 			fse->max_width = fse->min_width = w;
1283 			fse->max_height = fse->min_height = h;
1284 			return 0;
1285 		}
1286 		default:
1287 			return -EINVAL;
1288 		}
1289 	}
1290 
1291 	if (fse->code != s5c73m3_oif_get_pad_code(fse->pad, 0))
1292 		return -EINVAL;
1293 
1294 	if (fse->pad == OIF_JPEG_PAD)
1295 		idx = RES_JPEG;
1296 	else
1297 		idx = RES_ISP;
1298 
1299 	if (fse->index >= s5c73m3_resolutions_len[idx])
1300 		return -EINVAL;
1301 
1302 	fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1303 	fse->max_width  = fse->min_width;
1304 	fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1305 	fse->min_height = fse->max_height;
1306 
1307 	return 0;
1308 }
1309 
1310 static int s5c73m3_oif_log_status(struct v4l2_subdev *sd)
1311 {
1312 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1313 
1314 	v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
1315 
1316 	v4l2_info(sd, "power: %d, apply_fmt: %d\n", state->power,
1317 							state->apply_fmt);
1318 
1319 	return 0;
1320 }
1321 
1322 static int s5c73m3_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1323 {
1324 	struct v4l2_mbus_framefmt *mf;
1325 
1326 	mf = v4l2_subdev_get_try_format(sd, fh->pad, S5C73M3_ISP_PAD);
1327 	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1328 						S5C73M3_ISP_FMT);
1329 
1330 	mf = v4l2_subdev_get_try_format(sd, fh->pad, S5C73M3_JPEG_PAD);
1331 	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1332 					S5C73M3_JPEG_FMT);
1333 
1334 	return 0;
1335 }
1336 
1337 static int s5c73m3_oif_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1338 {
1339 	struct v4l2_mbus_framefmt *mf;
1340 
1341 	mf = v4l2_subdev_get_try_format(sd, fh->pad, OIF_ISP_PAD);
1342 	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1343 						S5C73M3_ISP_FMT);
1344 
1345 	mf = v4l2_subdev_get_try_format(sd, fh->pad, OIF_JPEG_PAD);
1346 	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1347 					S5C73M3_JPEG_FMT);
1348 
1349 	mf = v4l2_subdev_get_try_format(sd, fh->pad, OIF_SOURCE_PAD);
1350 	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1351 						S5C73M3_ISP_FMT);
1352 	return 0;
1353 }
1354 
1355 static int s5c73m3_gpio_set_value(struct s5c73m3 *priv, int id, u32 val)
1356 {
1357 	if (!gpio_is_valid(priv->gpio[id].gpio))
1358 		return 0;
1359 	gpio_set_value(priv->gpio[id].gpio, !!val);
1360 	return 1;
1361 }
1362 
1363 static int s5c73m3_gpio_assert(struct s5c73m3 *priv, int id)
1364 {
1365 	return s5c73m3_gpio_set_value(priv, id, priv->gpio[id].level);
1366 }
1367 
1368 static int s5c73m3_gpio_deassert(struct s5c73m3 *priv, int id)
1369 {
1370 	return s5c73m3_gpio_set_value(priv, id, !priv->gpio[id].level);
1371 }
1372 
1373 static int __s5c73m3_power_on(struct s5c73m3 *state)
1374 {
1375 	int i, ret;
1376 
1377 	for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++) {
1378 		ret = regulator_enable(state->supplies[i].consumer);
1379 		if (ret)
1380 			goto err_reg_dis;
1381 	}
1382 
1383 	ret = clk_set_rate(state->clock, state->mclk_frequency);
1384 	if (ret < 0)
1385 		goto err_reg_dis;
1386 
1387 	ret = clk_prepare_enable(state->clock);
1388 	if (ret < 0)
1389 		goto err_reg_dis;
1390 
1391 	v4l2_dbg(1, s5c73m3_dbg, &state->oif_sd, "clock frequency: %ld\n",
1392 					clk_get_rate(state->clock));
1393 
1394 	s5c73m3_gpio_deassert(state, STBY);
1395 	usleep_range(100, 200);
1396 
1397 	s5c73m3_gpio_deassert(state, RST);
1398 	usleep_range(50, 100);
1399 
1400 	return 0;
1401 
1402 err_reg_dis:
1403 	for (--i; i >= 0; i--)
1404 		regulator_disable(state->supplies[i].consumer);
1405 	return ret;
1406 }
1407 
1408 static int __s5c73m3_power_off(struct s5c73m3 *state)
1409 {
1410 	int i, ret;
1411 
1412 	if (s5c73m3_gpio_assert(state, RST))
1413 		usleep_range(10, 50);
1414 
1415 	if (s5c73m3_gpio_assert(state, STBY))
1416 		usleep_range(100, 200);
1417 
1418 	clk_disable_unprepare(state->clock);
1419 
1420 	state->streaming = 0;
1421 	state->isp_ready = 0;
1422 
1423 	for (i = S5C73M3_MAX_SUPPLIES - 1; i >= 0; i--) {
1424 		ret = regulator_disable(state->supplies[i].consumer);
1425 		if (ret)
1426 			goto err;
1427 	}
1428 
1429 	return 0;
1430 err:
1431 	for (++i; i < S5C73M3_MAX_SUPPLIES; i++) {
1432 		int r = regulator_enable(state->supplies[i].consumer);
1433 		if (r < 0)
1434 			v4l2_err(&state->oif_sd, "Failed to reenable %s: %d\n",
1435 				 state->supplies[i].supply, r);
1436 	}
1437 
1438 	clk_prepare_enable(state->clock);
1439 	return ret;
1440 }
1441 
1442 static int s5c73m3_oif_set_power(struct v4l2_subdev *sd, int on)
1443 {
1444 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1445 	int ret = 0;
1446 
1447 	mutex_lock(&state->lock);
1448 
1449 	if (on && !state->power) {
1450 		ret = __s5c73m3_power_on(state);
1451 		if (!ret)
1452 			ret = s5c73m3_isp_init(state);
1453 		if (!ret) {
1454 			state->apply_fiv = 1;
1455 			state->apply_fmt = 1;
1456 		}
1457 	} else if (state->power == !on) {
1458 		ret = s5c73m3_set_af_softlanding(state);
1459 		if (!ret)
1460 			ret = __s5c73m3_power_off(state);
1461 		else
1462 			v4l2_err(sd, "Soft landing lens failed\n");
1463 	}
1464 	if (!ret)
1465 		state->power += on ? 1 : -1;
1466 
1467 	v4l2_dbg(1, s5c73m3_dbg, sd, "%s: power: %d\n",
1468 		 __func__, state->power);
1469 
1470 	mutex_unlock(&state->lock);
1471 	return ret;
1472 }
1473 
1474 static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
1475 {
1476 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1477 	int ret;
1478 
1479 	ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->sensor_sd);
1480 	if (ret) {
1481 		v4l2_err(sd->v4l2_dev, "Failed to register %s\n",
1482 							state->oif_sd.name);
1483 		return ret;
1484 	}
1485 
1486 	ret = media_create_pad_link(&state->sensor_sd.entity,
1487 			S5C73M3_ISP_PAD, &state->oif_sd.entity, OIF_ISP_PAD,
1488 			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1489 
1490 	ret = media_create_pad_link(&state->sensor_sd.entity,
1491 			S5C73M3_JPEG_PAD, &state->oif_sd.entity, OIF_JPEG_PAD,
1492 			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1493 
1494 	return ret;
1495 }
1496 
1497 static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
1498 {
1499 	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1500 	v4l2_device_unregister_subdev(&state->sensor_sd);
1501 }
1502 
1503 static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
1504 	.open		= s5c73m3_open,
1505 };
1506 
1507 static const struct v4l2_subdev_pad_ops s5c73m3_pad_ops = {
1508 	.enum_mbus_code		= s5c73m3_enum_mbus_code,
1509 	.enum_frame_size	= s5c73m3_enum_frame_size,
1510 	.get_fmt		= s5c73m3_get_fmt,
1511 	.set_fmt		= s5c73m3_set_fmt,
1512 };
1513 
1514 static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
1515 	.pad	= &s5c73m3_pad_ops,
1516 };
1517 
1518 static const struct v4l2_subdev_internal_ops oif_internal_ops = {
1519 	.registered	= s5c73m3_oif_registered,
1520 	.unregistered	= s5c73m3_oif_unregistered,
1521 	.open		= s5c73m3_oif_open,
1522 };
1523 
1524 static const struct v4l2_subdev_pad_ops s5c73m3_oif_pad_ops = {
1525 	.enum_mbus_code		= s5c73m3_oif_enum_mbus_code,
1526 	.enum_frame_size	= s5c73m3_oif_enum_frame_size,
1527 	.enum_frame_interval	= s5c73m3_oif_enum_frame_interval,
1528 	.get_fmt		= s5c73m3_oif_get_fmt,
1529 	.set_fmt		= s5c73m3_oif_set_fmt,
1530 	.get_frame_desc		= s5c73m3_oif_get_frame_desc,
1531 	.set_frame_desc		= s5c73m3_oif_set_frame_desc,
1532 };
1533 
1534 static const struct v4l2_subdev_core_ops s5c73m3_oif_core_ops = {
1535 	.s_power	= s5c73m3_oif_set_power,
1536 	.log_status	= s5c73m3_oif_log_status,
1537 };
1538 
1539 static const struct v4l2_subdev_video_ops s5c73m3_oif_video_ops = {
1540 	.s_stream		= s5c73m3_oif_s_stream,
1541 	.g_frame_interval	= s5c73m3_oif_g_frame_interval,
1542 	.s_frame_interval	= s5c73m3_oif_s_frame_interval,
1543 };
1544 
1545 static const struct v4l2_subdev_ops oif_subdev_ops = {
1546 	.core	= &s5c73m3_oif_core_ops,
1547 	.pad	= &s5c73m3_oif_pad_ops,
1548 	.video	= &s5c73m3_oif_video_ops,
1549 };
1550 
1551 static int s5c73m3_configure_gpios(struct s5c73m3 *state)
1552 {
1553 	static const char * const gpio_names[] = {
1554 		"S5C73M3_STBY", "S5C73M3_RST"
1555 	};
1556 	struct i2c_client *c = state->i2c_client;
1557 	struct s5c73m3_gpio *g = state->gpio;
1558 	int ret, i;
1559 
1560 	for (i = 0; i < GPIO_NUM; ++i) {
1561 		unsigned int flags = GPIOF_DIR_OUT;
1562 		if (g[i].level)
1563 			flags |= GPIOF_INIT_HIGH;
1564 		ret = devm_gpio_request_one(&c->dev, g[i].gpio, flags,
1565 					    gpio_names[i]);
1566 		if (ret) {
1567 			v4l2_err(c, "failed to request gpio %s\n",
1568 				 gpio_names[i]);
1569 			return ret;
1570 		}
1571 	}
1572 	return 0;
1573 }
1574 
1575 static int s5c73m3_parse_gpios(struct s5c73m3 *state)
1576 {
1577 	static const char * const prop_names[] = {
1578 		"standby-gpios", "xshutdown-gpios",
1579 	};
1580 	struct device *dev = &state->i2c_client->dev;
1581 	struct device_node *node = dev->of_node;
1582 	int ret, i;
1583 
1584 	for (i = 0; i < GPIO_NUM; ++i) {
1585 		enum of_gpio_flags of_flags;
1586 
1587 		ret = of_get_named_gpio_flags(node, prop_names[i],
1588 					      0, &of_flags);
1589 		if (ret < 0) {
1590 			dev_err(dev, "failed to parse %s DT property\n",
1591 				prop_names[i]);
1592 			return -EINVAL;
1593 		}
1594 		state->gpio[i].gpio = ret;
1595 		state->gpio[i].level = !(of_flags & OF_GPIO_ACTIVE_LOW);
1596 	}
1597 	return 0;
1598 }
1599 
1600 static int s5c73m3_get_platform_data(struct s5c73m3 *state)
1601 {
1602 	struct device *dev = &state->i2c_client->dev;
1603 	const struct s5c73m3_platform_data *pdata = dev->platform_data;
1604 	struct device_node *node = dev->of_node;
1605 	struct device_node *node_ep;
1606 	struct v4l2_fwnode_endpoint ep;
1607 	int ret;
1608 
1609 	if (!node) {
1610 		if (!pdata) {
1611 			dev_err(dev, "Platform data not specified\n");
1612 			return -EINVAL;
1613 		}
1614 
1615 		state->mclk_frequency = pdata->mclk_frequency;
1616 		state->gpio[STBY] = pdata->gpio_stby;
1617 		state->gpio[RST] = pdata->gpio_reset;
1618 		return 0;
1619 	}
1620 
1621 	state->clock = devm_clk_get(dev, S5C73M3_CLK_NAME);
1622 	if (IS_ERR(state->clock))
1623 		return PTR_ERR(state->clock);
1624 
1625 	if (of_property_read_u32(node, "clock-frequency",
1626 				 &state->mclk_frequency)) {
1627 		state->mclk_frequency = S5C73M3_DEFAULT_MCLK_FREQ;
1628 		dev_info(dev, "using default %u Hz clock frequency\n",
1629 					state->mclk_frequency);
1630 	}
1631 
1632 	ret = s5c73m3_parse_gpios(state);
1633 	if (ret < 0)
1634 		return -EINVAL;
1635 
1636 	node_ep = of_graph_get_next_endpoint(node, NULL);
1637 	if (!node_ep) {
1638 		dev_warn(dev, "no endpoint defined for node: %s\n",
1639 						node->full_name);
1640 		return 0;
1641 	}
1642 
1643 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
1644 	of_node_put(node_ep);
1645 	if (ret)
1646 		return ret;
1647 
1648 	if (ep.bus_type != V4L2_MBUS_CSI2) {
1649 		dev_err(dev, "unsupported bus type\n");
1650 		return -EINVAL;
1651 	}
1652 	/*
1653 	 * Number of MIPI CSI-2 data lanes is currently not configurable,
1654 	 * always a default value of 4 lanes is used.
1655 	 */
1656 	if (ep.bus.mipi_csi2.num_data_lanes != S5C73M3_MIPI_DATA_LANES)
1657 		dev_info(dev, "falling back to 4 MIPI CSI-2 data lanes\n");
1658 
1659 	return 0;
1660 }
1661 
1662 static int s5c73m3_probe(struct i2c_client *client,
1663 				const struct i2c_device_id *id)
1664 {
1665 	struct device *dev = &client->dev;
1666 	struct v4l2_subdev *sd;
1667 	struct v4l2_subdev *oif_sd;
1668 	struct s5c73m3 *state;
1669 	int ret, i;
1670 
1671 	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
1672 	if (!state)
1673 		return -ENOMEM;
1674 
1675 	state->i2c_client = client;
1676 	ret = s5c73m3_get_platform_data(state);
1677 	if (ret < 0)
1678 		return ret;
1679 
1680 	mutex_init(&state->lock);
1681 	sd = &state->sensor_sd;
1682 	oif_sd = &state->oif_sd;
1683 
1684 	v4l2_subdev_init(sd, &s5c73m3_subdev_ops);
1685 	sd->owner = client->dev.driver->owner;
1686 	v4l2_set_subdevdata(sd, state);
1687 	strlcpy(sd->name, "S5C73M3", sizeof(sd->name));
1688 
1689 	sd->internal_ops = &s5c73m3_internal_ops;
1690 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1691 
1692 	state->sensor_pads[S5C73M3_JPEG_PAD].flags = MEDIA_PAD_FL_SOURCE;
1693 	state->sensor_pads[S5C73M3_ISP_PAD].flags = MEDIA_PAD_FL_SOURCE;
1694 	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1695 
1696 	ret = media_entity_pads_init(&sd->entity, S5C73M3_NUM_PADS,
1697 							state->sensor_pads);
1698 	if (ret < 0)
1699 		return ret;
1700 
1701 	v4l2_i2c_subdev_init(oif_sd, client, &oif_subdev_ops);
1702 	strcpy(oif_sd->name, "S5C73M3-OIF");
1703 
1704 	oif_sd->internal_ops = &oif_internal_ops;
1705 	oif_sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1706 
1707 	state->oif_pads[OIF_ISP_PAD].flags = MEDIA_PAD_FL_SINK;
1708 	state->oif_pads[OIF_JPEG_PAD].flags = MEDIA_PAD_FL_SINK;
1709 	state->oif_pads[OIF_SOURCE_PAD].flags = MEDIA_PAD_FL_SOURCE;
1710 	oif_sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
1711 
1712 	ret = media_entity_pads_init(&oif_sd->entity, OIF_NUM_PADS,
1713 							state->oif_pads);
1714 	if (ret < 0)
1715 		return ret;
1716 
1717 	ret = s5c73m3_configure_gpios(state);
1718 	if (ret)
1719 		goto out_err;
1720 
1721 	for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++)
1722 		state->supplies[i].supply = s5c73m3_supply_names[i];
1723 
1724 	ret = devm_regulator_bulk_get(dev, S5C73M3_MAX_SUPPLIES,
1725 			       state->supplies);
1726 	if (ret) {
1727 		dev_err(dev, "failed to get regulators\n");
1728 		goto out_err;
1729 	}
1730 
1731 	ret = s5c73m3_init_controls(state);
1732 	if (ret)
1733 		goto out_err;
1734 
1735 	state->sensor_pix_size[RES_ISP] = &s5c73m3_isp_resolutions[1];
1736 	state->sensor_pix_size[RES_JPEG] = &s5c73m3_jpeg_resolutions[1];
1737 	state->oif_pix_size[RES_ISP] = state->sensor_pix_size[RES_ISP];
1738 	state->oif_pix_size[RES_JPEG] = state->sensor_pix_size[RES_JPEG];
1739 
1740 	state->mbus_code = S5C73M3_ISP_FMT;
1741 
1742 	state->fiv = &s5c73m3_intervals[S5C73M3_DEFAULT_FRAME_INTERVAL];
1743 
1744 	state->fw_file_version[0] = 'G';
1745 	state->fw_file_version[1] = 'C';
1746 
1747 	ret = s5c73m3_register_spi_driver(state);
1748 	if (ret < 0)
1749 		goto out_err;
1750 
1751 	oif_sd->dev = dev;
1752 
1753 	ret = __s5c73m3_power_on(state);
1754 	if (ret < 0)
1755 		goto out_err1;
1756 
1757 	ret = s5c73m3_get_fw_version(state);
1758 	__s5c73m3_power_off(state);
1759 
1760 	if (ret < 0) {
1761 		dev_err(dev, "Device detection failed: %d\n", ret);
1762 		goto out_err1;
1763 	}
1764 
1765 	ret = v4l2_async_register_subdev(oif_sd);
1766 	if (ret < 0)
1767 		goto out_err1;
1768 
1769 	v4l2_info(sd, "%s: completed successfully\n", __func__);
1770 	return 0;
1771 
1772 out_err1:
1773 	s5c73m3_unregister_spi_driver(state);
1774 out_err:
1775 	media_entity_cleanup(&sd->entity);
1776 	return ret;
1777 }
1778 
1779 static int s5c73m3_remove(struct i2c_client *client)
1780 {
1781 	struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
1782 	struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
1783 	struct v4l2_subdev *sensor_sd = &state->sensor_sd;
1784 
1785 	v4l2_async_unregister_subdev(oif_sd);
1786 
1787 	v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
1788 	media_entity_cleanup(&oif_sd->entity);
1789 
1790 	v4l2_device_unregister_subdev(sensor_sd);
1791 	media_entity_cleanup(&sensor_sd->entity);
1792 
1793 	s5c73m3_unregister_spi_driver(state);
1794 
1795 	return 0;
1796 }
1797 
1798 static const struct i2c_device_id s5c73m3_id[] = {
1799 	{ DRIVER_NAME, 0 },
1800 	{ }
1801 };
1802 MODULE_DEVICE_TABLE(i2c, s5c73m3_id);
1803 
1804 #ifdef CONFIG_OF
1805 static const struct of_device_id s5c73m3_of_match[] = {
1806 	{ .compatible = "samsung,s5c73m3" },
1807 	{ }
1808 };
1809 MODULE_DEVICE_TABLE(of, s5c73m3_of_match);
1810 #endif
1811 
1812 static struct i2c_driver s5c73m3_i2c_driver = {
1813 	.driver = {
1814 		.of_match_table = of_match_ptr(s5c73m3_of_match),
1815 		.name	= DRIVER_NAME,
1816 	},
1817 	.probe		= s5c73m3_probe,
1818 	.remove		= s5c73m3_remove,
1819 	.id_table	= s5c73m3_id,
1820 };
1821 
1822 module_i2c_driver(s5c73m3_i2c_driver);
1823 
1824 MODULE_DESCRIPTION("Samsung S5C73M3 camera driver");
1825 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
1826 MODULE_LICENSE("GPL");
1827