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