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