s5c73m3-core.c

   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
  33int s5c73m3_dbg;
  34module_param_named(debug, s5c73m3_dbg, int, 0644);
  35
  36static int boot_from_rom = 1;
  37module_param(boot_from_rom, int, 0644);
  38
  39static int update_fw;
  40module_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
  46static 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
  57static 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
  75static 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
  93static const struct s5c73m3_frame_size * const s5c73m3_resolutions[] = {
  94	[RES_ISP] = s5c73m3_isp_resolutions,
  95	[RES_JPEG] = s5c73m3_jpeg_resolutions
  96};
  97
  98static const int s5c73m3_resolutions_len[] = {
  99	[RES_ISP] = ARRAY_SIZE(s5c73m3_isp_resolutions),
 100	[RES_JPEG] = ARRAY_SIZE(s5c73m3_jpeg_resolutions)
 101};
 102
 103static 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
 112static 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
 123static 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
 138static 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
 173int 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
 205int 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
 237static 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
 275int 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
 294static 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
 300static 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
 340static 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
 370static 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
 394static 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
 411static 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
 449static 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
 461static 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
 477static 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
 517static 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
 551static 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
 617static 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
 636static 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
 656static 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
 716static 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
 776static 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
 793static 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
 817static 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	u32 code;
 823
 824	switch (fmt->pad) {
 825	case OIF_ISP_PAD:
 826		*fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
 827		code = S5C73M3_ISP_FMT;
 828		break;
 829	case OIF_JPEG_PAD:
 830		*fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
 831		code = S5C73M3_JPEG_FMT;
 832		break;
 833	case OIF_SOURCE_PAD:
 834	default:
 835		if (fmt->format.code == S5C73M3_JPEG_FMT)
 836			code = S5C73M3_JPEG_FMT;
 837		else
 838			code = S5C73M3_ISP_FMT;
 839
 840		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 841			*fs = state->oif_pix_size[RES_ISP];
 842		else
 843			*fs = s5c73m3_find_frame_size(v4l2_subdev_state_get_format(sd_state, OIF_ISP_PAD),
 844						      RES_ISP);
 845		break;
 846	}
 847
 848	s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
 849}
 850
 851static void s5c73m3_try_format(struct s5c73m3 *state,
 852			      struct v4l2_subdev_state *sd_state,
 853			      struct v4l2_subdev_format *fmt,
 854			      const struct s5c73m3_frame_size **fs)
 855{
 856	u32 code;
 857
 858	if (fmt->pad == S5C73M3_ISP_PAD) {
 859		*fs = s5c73m3_find_frame_size(&fmt->format, RES_ISP);
 860		code = S5C73M3_ISP_FMT;
 861	} else {
 862		*fs = s5c73m3_find_frame_size(&fmt->format, RES_JPEG);
 863		code = S5C73M3_JPEG_FMT;
 864	}
 865
 866	s5c73m3_fill_mbus_fmt(&fmt->format, *fs, code);
 867}
 868
 869static int s5c73m3_oif_get_frame_interval(struct v4l2_subdev *sd,
 870					  struct v4l2_subdev_state *sd_state,
 871					  struct v4l2_subdev_frame_interval *fi)
 872{
 873	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 874
 875	/*
 876	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
 877	 * subdev active state API.
 878	 */
 879	if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 880		return -EINVAL;
 881
 882	if (fi->pad != OIF_SOURCE_PAD)
 883		return -EINVAL;
 884
 885	mutex_lock(&state->lock);
 886	fi->interval = state->fiv->interval;
 887	mutex_unlock(&state->lock);
 888
 889	return 0;
 890}
 891
 892static int __s5c73m3_set_frame_interval(struct s5c73m3 *state,
 893					struct v4l2_subdev_frame_interval *fi)
 894{
 895	const struct s5c73m3_frame_size *prev_size =
 896						state->sensor_pix_size[RES_ISP];
 897	const struct s5c73m3_interval *fiv = &s5c73m3_intervals[0];
 898	unsigned int ret, min_err = UINT_MAX;
 899	unsigned int i, fr_time;
 900
 901	if (fi->interval.denominator == 0)
 902		return -EINVAL;
 903
 904	fr_time = fi->interval.numerator * 1000 / fi->interval.denominator;
 905
 906	for (i = 0; i < ARRAY_SIZE(s5c73m3_intervals); i++) {
 907		const struct s5c73m3_interval *iv = &s5c73m3_intervals[i];
 908
 909		if (prev_size->width > iv->size.width ||
 910		    prev_size->height > iv->size.height)
 911			continue;
 912
 913		ret = abs(iv->interval.numerator / 1000 - fr_time);
 914		if (ret < min_err) {
 915			fiv = iv;
 916			min_err = ret;
 917		}
 918	}
 919	state->fiv = fiv;
 920
 921	v4l2_dbg(1, s5c73m3_dbg, &state->sensor_sd,
 922		 "Changed frame interval to %u us\n", fiv->interval.numerator);
 923	return 0;
 924}
 925
 926static int s5c73m3_oif_set_frame_interval(struct v4l2_subdev *sd,
 927					  struct v4l2_subdev_state *sd_state,
 928					  struct v4l2_subdev_frame_interval *fi)
 929{
 930	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 931	int ret;
 932
 933	/*
 934	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
 935	 * subdev active state API.
 936	 */
 937	if (fi->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 938		return -EINVAL;
 939
 940	if (fi->pad != OIF_SOURCE_PAD)
 941		return -EINVAL;
 942
 943	v4l2_dbg(1, s5c73m3_dbg, sd, "Setting %d/%d frame interval\n",
 944		 fi->interval.numerator, fi->interval.denominator);
 945
 946	mutex_lock(&state->lock);
 947
 948	ret = __s5c73m3_set_frame_interval(state, fi);
 949	if (!ret) {
 950		if (state->streaming)
 951			ret = s5c73m3_set_frame_rate(state);
 952		else
 953			state->apply_fiv = 1;
 954	}
 955	mutex_unlock(&state->lock);
 956	return ret;
 957}
 958
 959static int s5c73m3_oif_enum_frame_interval(struct v4l2_subdev *sd,
 960			      struct v4l2_subdev_state *sd_state,
 961			      struct v4l2_subdev_frame_interval_enum *fie)
 962{
 963	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
 964	const struct s5c73m3_interval *fi;
 965	int ret = 0;
 966
 967	if (fie->pad != OIF_SOURCE_PAD)
 968		return -EINVAL;
 969	if (fie->index >= ARRAY_SIZE(s5c73m3_intervals))
 970		return -EINVAL;
 971
 972	mutex_lock(&state->lock);
 973	fi = &s5c73m3_intervals[fie->index];
 974	if (fie->width > fi->size.width || fie->height > fi->size.height)
 975		ret = -EINVAL;
 976	else
 977		fie->interval = fi->interval;
 978	mutex_unlock(&state->lock);
 979
 980	return ret;
 981}
 982
 983static int s5c73m3_oif_get_pad_code(int pad, int index)
 984{
 985	if (pad == OIF_SOURCE_PAD) {
 986		if (index > 1)
 987			return -EINVAL;
 988		return (index == 0) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
 989	}
 990
 991	if (index > 0)
 992		return -EINVAL;
 993
 994	return (pad == OIF_ISP_PAD) ? S5C73M3_ISP_FMT : S5C73M3_JPEG_FMT;
 995}
 996
 997static int s5c73m3_get_fmt(struct v4l2_subdev *sd,
 998			   struct v4l2_subdev_state *sd_state,
 999			   struct v4l2_subdev_format *fmt)
1000{
1001	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
1002	const struct s5c73m3_frame_size *fs;
1003	u32 code;
1004
1005	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1006		fmt->format = *v4l2_subdev_state_get_format(sd_state,
1007							    fmt->pad);
1008		return 0;
1009	}
1010
1011	mutex_lock(&state->lock);
1012
1013	switch (fmt->pad) {
1014	case S5C73M3_ISP_PAD:
1015		code = S5C73M3_ISP_FMT;
1016		fs = state->sensor_pix_size[RES_ISP];
1017		break;
1018	case S5C73M3_JPEG_PAD:
1019		code = S5C73M3_JPEG_FMT;
1020		fs = state->sensor_pix_size[RES_JPEG];
1021		break;
1022	default:
1023		mutex_unlock(&state->lock);
1024		return -EINVAL;
1025	}
1026	s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1027
1028	mutex_unlock(&state->lock);
1029	return 0;
1030}
1031
1032static int s5c73m3_oif_get_fmt(struct v4l2_subdev *sd,
1033			   struct v4l2_subdev_state *sd_state,
1034			   struct v4l2_subdev_format *fmt)
1035{
1036	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1037	const struct s5c73m3_frame_size *fs;
1038	u32 code;
1039
1040	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1041		fmt->format = *v4l2_subdev_state_get_format(sd_state,
1042							    fmt->pad);
1043		return 0;
1044	}
1045
1046	mutex_lock(&state->lock);
1047
1048	switch (fmt->pad) {
1049	case OIF_ISP_PAD:
1050		code = S5C73M3_ISP_FMT;
1051		fs = state->oif_pix_size[RES_ISP];
1052		break;
1053	case OIF_JPEG_PAD:
1054		code = S5C73M3_JPEG_FMT;
1055		fs = state->oif_pix_size[RES_JPEG];
1056		break;
1057	case OIF_SOURCE_PAD:
1058		code = state->mbus_code;
1059		fs = state->oif_pix_size[RES_ISP];
1060		break;
1061	default:
1062		mutex_unlock(&state->lock);
1063		return -EINVAL;
1064	}
1065	s5c73m3_fill_mbus_fmt(&fmt->format, fs, code);
1066
1067	mutex_unlock(&state->lock);
1068	return 0;
1069}
1070
1071static int s5c73m3_set_fmt(struct v4l2_subdev *sd,
1072			   struct v4l2_subdev_state *sd_state,
1073			   struct v4l2_subdev_format *fmt)
1074{
1075	const struct s5c73m3_frame_size *frame_size = NULL;
1076	struct s5c73m3 *state = sensor_sd_to_s5c73m3(sd);
1077	struct v4l2_mbus_framefmt *mf;
1078	int ret = 0;
1079
1080	mutex_lock(&state->lock);
1081
1082	s5c73m3_try_format(state, sd_state, fmt, &frame_size);
1083
1084	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1085		mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
1086		*mf = fmt->format;
1087	} else {
1088		switch (fmt->pad) {
1089		case S5C73M3_ISP_PAD:
1090			state->sensor_pix_size[RES_ISP] = frame_size;
1091			break;
1092		case S5C73M3_JPEG_PAD:
1093			state->sensor_pix_size[RES_JPEG] = frame_size;
1094			break;
1095		default:
1096			ret = -EBUSY;
1097		}
1098
1099		if (state->streaming)
1100			ret = -EBUSY;
1101		else
1102			state->apply_fmt = 1;
1103	}
1104
1105	mutex_unlock(&state->lock);
1106
1107	return ret;
1108}
1109
1110static int s5c73m3_oif_set_fmt(struct v4l2_subdev *sd,
1111			 struct v4l2_subdev_state *sd_state,
1112			 struct v4l2_subdev_format *fmt)
1113{
1114	const struct s5c73m3_frame_size *frame_size = NULL;
1115	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1116	struct v4l2_mbus_framefmt *mf;
1117	int ret = 0;
1118
1119	mutex_lock(&state->lock);
1120
1121	s5c73m3_oif_try_format(state, sd_state, fmt, &frame_size);
1122
1123	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1124		mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
1125		*mf = fmt->format;
1126		if (fmt->pad == OIF_ISP_PAD) {
1127			mf = v4l2_subdev_state_get_format(sd_state,
1128							  OIF_SOURCE_PAD);
1129			mf->width = fmt->format.width;
1130			mf->height = fmt->format.height;
1131		}
1132	} else {
1133		switch (fmt->pad) {
1134		case OIF_ISP_PAD:
1135			state->oif_pix_size[RES_ISP] = frame_size;
1136			break;
1137		case OIF_JPEG_PAD:
1138			state->oif_pix_size[RES_JPEG] = frame_size;
1139			break;
1140		case OIF_SOURCE_PAD:
1141			state->mbus_code = fmt->format.code;
1142			break;
1143		default:
1144			ret = -EBUSY;
1145		}
1146
1147		if (state->streaming)
1148			ret = -EBUSY;
1149		else
1150			state->apply_fmt = 1;
1151	}
1152
1153	mutex_unlock(&state->lock);
1154
1155	return ret;
1156}
1157
1158static int s5c73m3_oif_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1159				  struct v4l2_mbus_frame_desc *fd)
1160{
1161	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1162	int i;
1163
1164	if (pad != OIF_SOURCE_PAD || fd == NULL)
1165		return -EINVAL;
1166
1167	mutex_lock(&state->lock);
1168	fd->num_entries = 2;
1169	for (i = 0; i < fd->num_entries; i++)
1170		fd->entry[i] = state->frame_desc.entry[i];
1171	mutex_unlock(&state->lock);
1172
1173	return 0;
1174}
1175
1176static int s5c73m3_oif_set_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
1177				      struct v4l2_mbus_frame_desc *fd)
1178{
1179	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1180	struct v4l2_mbus_frame_desc *frame_desc = &state->frame_desc;
1181	int i;
1182
1183	if (pad != OIF_SOURCE_PAD || fd == NULL)
1184		return -EINVAL;
1185
1186	fd->entry[0].length = 10 * SZ_1M;
1187	fd->entry[1].length = max_t(u32, fd->entry[1].length,
1188				    S5C73M3_EMBEDDED_DATA_MAXLEN);
1189	fd->num_entries = 2;
1190
1191	mutex_lock(&state->lock);
1192	for (i = 0; i < fd->num_entries; i++)
1193		frame_desc->entry[i] = fd->entry[i];
1194	mutex_unlock(&state->lock);
1195
1196	return 0;
1197}
1198
1199static int s5c73m3_enum_mbus_code(struct v4l2_subdev *sd,
1200				  struct v4l2_subdev_state *sd_state,
1201				  struct v4l2_subdev_mbus_code_enum *code)
1202{
1203	static const int codes[] = {
1204			[S5C73M3_ISP_PAD] = S5C73M3_ISP_FMT,
1205			[S5C73M3_JPEG_PAD] = S5C73M3_JPEG_FMT};
1206
1207	if (code->index > 0 || code->pad >= S5C73M3_NUM_PADS)
1208		return -EINVAL;
1209
1210	code->code = codes[code->pad];
1211
1212	return 0;
1213}
1214
1215static int s5c73m3_oif_enum_mbus_code(struct v4l2_subdev *sd,
1216				struct v4l2_subdev_state *sd_state,
1217				struct v4l2_subdev_mbus_code_enum *code)
1218{
1219	int ret;
1220
1221	ret = s5c73m3_oif_get_pad_code(code->pad, code->index);
1222	if (ret < 0)
1223		return ret;
1224
1225	code->code = ret;
1226
1227	return 0;
1228}
1229
1230static int s5c73m3_enum_frame_size(struct v4l2_subdev *sd,
1231				   struct v4l2_subdev_state *sd_state,
1232				   struct v4l2_subdev_frame_size_enum *fse)
1233{
1234	int idx;
1235
1236	if (fse->pad == S5C73M3_ISP_PAD) {
1237		if (fse->code != S5C73M3_ISP_FMT)
1238			return -EINVAL;
1239		idx = RES_ISP;
1240	} else{
1241		if (fse->code != S5C73M3_JPEG_FMT)
1242			return -EINVAL;
1243		idx = RES_JPEG;
1244	}
1245
1246	if (fse->index >= s5c73m3_resolutions_len[idx])
1247		return -EINVAL;
1248
1249	fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1250	fse->max_width  = fse->min_width;
1251	fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1252	fse->min_height = fse->max_height;
1253
1254	return 0;
1255}
1256
1257static int s5c73m3_oif_enum_frame_size(struct v4l2_subdev *sd,
1258				   struct v4l2_subdev_state *sd_state,
1259				   struct v4l2_subdev_frame_size_enum *fse)
1260{
1261	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1262	int idx;
1263
1264	if (fse->pad == OIF_SOURCE_PAD) {
1265		if (fse->index > 0)
1266			return -EINVAL;
1267
1268		switch (fse->code) {
1269		case S5C73M3_JPEG_FMT:
1270		case S5C73M3_ISP_FMT: {
1271			unsigned w, h;
1272
1273			if (fse->which == V4L2_SUBDEV_FORMAT_TRY) {
1274				struct v4l2_mbus_framefmt *mf;
1275
1276				mf = v4l2_subdev_state_get_format(sd_state,
1277								  OIF_ISP_PAD);
1278
1279				w = mf->width;
1280				h = mf->height;
1281			} else {
1282				const struct s5c73m3_frame_size *fs;
1283
1284				fs = state->oif_pix_size[RES_ISP];
1285				w = fs->width;
1286				h = fs->height;
1287			}
1288			fse->max_width = fse->min_width = w;
1289			fse->max_height = fse->min_height = h;
1290			return 0;
1291		}
1292		default:
1293			return -EINVAL;
1294		}
1295	}
1296
1297	if (fse->code != s5c73m3_oif_get_pad_code(fse->pad, 0))
1298		return -EINVAL;
1299
1300	if (fse->pad == OIF_JPEG_PAD)
1301		idx = RES_JPEG;
1302	else
1303		idx = RES_ISP;
1304
1305	if (fse->index >= s5c73m3_resolutions_len[idx])
1306		return -EINVAL;
1307
1308	fse->min_width  = s5c73m3_resolutions[idx][fse->index].width;
1309	fse->max_width  = fse->min_width;
1310	fse->max_height = s5c73m3_resolutions[idx][fse->index].height;
1311	fse->min_height = fse->max_height;
1312
1313	return 0;
1314}
1315
1316static int s5c73m3_oif_log_status(struct v4l2_subdev *sd)
1317{
1318	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1319
1320	v4l2_ctrl_handler_log_status(sd->ctrl_handler, sd->name);
1321
1322	v4l2_info(sd, "power: %d, apply_fmt: %d\n", state->power,
1323							state->apply_fmt);
1324
1325	return 0;
1326}
1327
1328static int s5c73m3_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1329{
1330	struct v4l2_mbus_framefmt *mf;
1331
1332	mf = v4l2_subdev_state_get_format(fh->state, S5C73M3_ISP_PAD);
1333	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1334						S5C73M3_ISP_FMT);
1335
1336	mf = v4l2_subdev_state_get_format(fh->state, S5C73M3_JPEG_PAD);
1337	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1338					S5C73M3_JPEG_FMT);
1339
1340	return 0;
1341}
1342
1343static int s5c73m3_oif_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1344{
1345	struct v4l2_mbus_framefmt *mf;
1346
1347	mf = v4l2_subdev_state_get_format(fh->state, OIF_ISP_PAD);
1348	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1349						S5C73M3_ISP_FMT);
1350
1351	mf = v4l2_subdev_state_get_format(fh->state, OIF_JPEG_PAD);
1352	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_jpeg_resolutions[1],
1353					S5C73M3_JPEG_FMT);
1354
1355	mf = v4l2_subdev_state_get_format(fh->state, OIF_SOURCE_PAD);
1356	s5c73m3_fill_mbus_fmt(mf, &s5c73m3_isp_resolutions[1],
1357						S5C73M3_ISP_FMT);
1358	return 0;
1359}
1360
1361static int __s5c73m3_power_on(struct s5c73m3 *state)
1362{
1363	int i, ret;
1364
1365	for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++) {
1366		ret = regulator_enable(state->supplies[i].consumer);
1367		if (ret)
1368			goto err_reg_dis;
1369	}
1370
1371	ret = clk_prepare_enable(state->clock);
1372	if (ret < 0)
1373		goto err_reg_dis;
1374
1375	v4l2_dbg(1, s5c73m3_dbg, &state->oif_sd, "clock frequency: %ld\n",
1376					clk_get_rate(state->clock));
1377
1378	gpiod_set_value(state->stby, 0);
1379	usleep_range(100, 200);
1380	gpiod_set_value(state->reset, 0);
1381	usleep_range(50, 100);
1382
1383	return 0;
1384
1385err_reg_dis:
1386	for (--i; i >= 0; i--)
1387		regulator_disable(state->supplies[i].consumer);
1388	return ret;
1389}
1390
1391/*
1392 * This function has been created just to avoid a smatch warning,
1393 * please do not merge into __s5c73m3_power_off() until you have
1394 * confirmed that it does not introduce a new warning.
1395 */
1396static void s5c73m3_enable_clk(struct s5c73m3 *state)
1397{
1398	clk_prepare_enable(state->clock);
1399}
1400
1401static int __s5c73m3_power_off(struct s5c73m3 *state)
1402{
1403	int i, ret;
1404
1405	gpiod_set_value(state->reset, 1);
1406	usleep_range(10, 50);
1407	gpiod_set_value(state->stby, 1);
1408	usleep_range(100, 200);
1409
1410	clk_disable_unprepare(state->clock);
1411
1412	state->streaming = 0;
1413	state->isp_ready = 0;
1414
1415	for (i = S5C73M3_MAX_SUPPLIES - 1; i >= 0; i--) {
1416		ret = regulator_disable(state->supplies[i].consumer);
1417		if (ret)
1418			goto err;
1419	}
1420
1421	return 0;
1422err:
1423	for (++i; i < S5C73M3_MAX_SUPPLIES; i++) {
1424		int r = regulator_enable(state->supplies[i].consumer);
1425		if (r < 0)
1426			v4l2_err(&state->oif_sd, "Failed to re-enable %s: %d\n",
1427				 state->supplies[i].supply, r);
1428	}
1429
1430	s5c73m3_enable_clk(state);
1431
1432	return ret;
1433}
1434
1435static int s5c73m3_oif_set_power(struct v4l2_subdev *sd, int on)
1436{
1437	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1438	int ret = 0;
1439
1440	mutex_lock(&state->lock);
1441
1442	if (on && !state->power) {
1443		ret = __s5c73m3_power_on(state);
1444		if (!ret)
1445			ret = s5c73m3_isp_init(state);
1446		if (!ret) {
1447			state->apply_fiv = 1;
1448			state->apply_fmt = 1;
1449		}
1450	} else if (state->power == !on) {
1451		ret = s5c73m3_set_af_softlanding(state);
1452		if (!ret)
1453			ret = __s5c73m3_power_off(state);
1454		else
1455			v4l2_err(sd, "Soft landing lens failed\n");
1456	}
1457	if (!ret)
1458		state->power += on ? 1 : -1;
1459
1460	v4l2_dbg(1, s5c73m3_dbg, sd, "%s: power: %d\n",
1461		 __func__, state->power);
1462
1463	mutex_unlock(&state->lock);
1464	return ret;
1465}
1466
1467static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
1468{
1469	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1470	int ret;
1471
1472	ret = v4l2_device_register_subdev(sd->v4l2_dev, &state->sensor_sd);
1473	if (ret) {
1474		v4l2_err(sd->v4l2_dev, "Failed to register %s\n",
1475							state->oif_sd.name);
1476		return ret;
1477	}
1478
1479	ret = media_create_pad_link(&state->sensor_sd.entity,
1480			S5C73M3_ISP_PAD, &state->oif_sd.entity, OIF_ISP_PAD,
1481			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1482
1483	ret = media_create_pad_link(&state->sensor_sd.entity,
1484			S5C73M3_JPEG_PAD, &state->oif_sd.entity, OIF_JPEG_PAD,
1485			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1486
1487	return ret;
1488}
1489
1490static void s5c73m3_oif_unregistered(struct v4l2_subdev *sd)
1491{
1492	struct s5c73m3 *state = oif_sd_to_s5c73m3(sd);
1493	v4l2_device_unregister_subdev(&state->sensor_sd);
1494}
1495
1496static const struct v4l2_subdev_internal_ops s5c73m3_internal_ops = {
1497	.open		= s5c73m3_open,
1498};
1499
1500static const struct v4l2_subdev_pad_ops s5c73m3_pad_ops = {
1501	.enum_mbus_code		= s5c73m3_enum_mbus_code,
1502	.enum_frame_size	= s5c73m3_enum_frame_size,
1503	.get_fmt		= s5c73m3_get_fmt,
1504	.set_fmt		= s5c73m3_set_fmt,
1505};
1506
1507static const struct v4l2_subdev_ops s5c73m3_subdev_ops = {
1508	.pad	= &s5c73m3_pad_ops,
1509};
1510
1511static const struct v4l2_subdev_internal_ops oif_internal_ops = {
1512	.registered	= s5c73m3_oif_registered,
1513	.unregistered	= s5c73m3_oif_unregistered,
1514	.open		= s5c73m3_oif_open,
1515};
1516
1517static const struct v4l2_subdev_pad_ops s5c73m3_oif_pad_ops = {
1518	.enum_mbus_code		= s5c73m3_oif_enum_mbus_code,
1519	.enum_frame_size	= s5c73m3_oif_enum_frame_size,
1520	.enum_frame_interval	= s5c73m3_oif_enum_frame_interval,
1521	.get_fmt		= s5c73m3_oif_get_fmt,
1522	.set_fmt		= s5c73m3_oif_set_fmt,
1523	.get_frame_interval	= s5c73m3_oif_get_frame_interval,
1524	.set_frame_interval	= s5c73m3_oif_set_frame_interval,
1525	.get_frame_desc		= s5c73m3_oif_get_frame_desc,
1526	.set_frame_desc		= s5c73m3_oif_set_frame_desc,
1527};
1528
1529static 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
1534static const struct v4l2_subdev_video_ops s5c73m3_oif_video_ops = {
1535	.s_stream		= s5c73m3_oif_s_stream,
1536};
1537
1538static const struct v4l2_subdev_ops oif_subdev_ops = {
1539	.core	= &s5c73m3_oif_core_ops,
1540	.pad	= &s5c73m3_oif_pad_ops,
1541	.video	= &s5c73m3_oif_video_ops,
1542};
1543
1544static int s5c73m3_get_dt_data(struct s5c73m3 *state)
1545{
1546	struct device *dev = &state->i2c_client->dev;
1547	struct device_node *node = dev->of_node;
1548	struct device_node *node_ep;
1549	struct v4l2_fwnode_endpoint ep = { .bus_type = 0 };
1550	int ret;
1551
1552	if (!node)
1553		return -EINVAL;
1554
1555	state->clock = devm_v4l2_sensor_clk_get_legacy(dev, S5C73M3_CLK_NAME,
1556						       false,
1557						       S5C73M3_DEFAULT_MCLK_FREQ);
1558	if (IS_ERR(state->clock))
1559		return dev_err_probe(dev, PTR_ERR(state->clock),
1560				     "Failed to get the clock %s\n",
1561				     S5C73M3_CLK_NAME);
1562
1563	/* Request GPIO lines asserted */
1564	state->stby = devm_gpiod_get(dev, "standby", GPIOD_OUT_HIGH);
1565	if (IS_ERR(state->stby))
1566		return dev_err_probe(dev, PTR_ERR(state->stby),
1567				     "failed to request gpio S5C73M3_STBY\n");
1568	gpiod_set_consumer_name(state->stby, "S5C73M3_STBY");
1569	state->reset = devm_gpiod_get(dev, "xshutdown", GPIOD_OUT_HIGH);
1570	if (IS_ERR(state->reset))
1571		return dev_err_probe(dev, PTR_ERR(state->reset),
1572				     "failed to request gpio S5C73M3_RST\n");
1573	gpiod_set_consumer_name(state->reset, "S5C73M3_RST");
1574
1575	node_ep = of_graph_get_endpoint_by_regs(node, 0, -1);
1576	if (!node_ep) {
1577		dev_warn(dev, "no endpoint defined for node: %pOF\n", node);
1578		return 0;
1579	}
1580
1581	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node_ep), &ep);
1582	of_node_put(node_ep);
1583	if (ret)
1584		return ret;
1585
1586	if (ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
1587		dev_err(dev, "unsupported bus type\n");
1588		return -EINVAL;
1589	}
1590	/*
1591	 * Number of MIPI CSI-2 data lanes is currently not configurable,
1592	 * always a default value of 4 lanes is used.
1593	 */
1594	if (ep.bus.mipi_csi2.num_data_lanes != S5C73M3_MIPI_DATA_LANES)
1595		dev_info(dev, "falling back to 4 MIPI CSI-2 data lanes\n");
1596
1597	return 0;
1598}
1599
1600static int s5c73m3_probe(struct i2c_client *client)
1601{
1602	struct device *dev = &client->dev;
1603	struct v4l2_subdev *sd;
1604	struct v4l2_subdev *oif_sd;
1605	struct s5c73m3 *state;
1606	int ret, i;
1607
1608	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
1609	if (!state)
1610		return -ENOMEM;
1611
1612	state->i2c_client = client;
1613	ret = s5c73m3_get_dt_data(state);
1614	if (ret < 0)
1615		return ret;
1616
1617	mutex_init(&state->lock);
1618	sd = &state->sensor_sd;
1619	oif_sd = &state->oif_sd;
1620
1621	v4l2_subdev_init(sd, &s5c73m3_subdev_ops);
1622	sd->owner = client->dev.driver->owner;
1623	v4l2_set_subdevdata(sd, state);
1624	strscpy(sd->name, "S5C73M3", sizeof(sd->name));
1625
1626	sd->internal_ops = &s5c73m3_internal_ops;
1627	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1628
1629	state->sensor_pads[S5C73M3_JPEG_PAD].flags = MEDIA_PAD_FL_SOURCE;
1630	state->sensor_pads[S5C73M3_ISP_PAD].flags = MEDIA_PAD_FL_SOURCE;
1631	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1632
1633	ret = media_entity_pads_init(&sd->entity, S5C73M3_NUM_PADS,
1634							state->sensor_pads);
1635	if (ret < 0)
1636		return ret;
1637
1638	v4l2_i2c_subdev_init(oif_sd, client, &oif_subdev_ops);
1639	/* Static name; NEVER use in new drivers! */
1640	strscpy(oif_sd->name, "S5C73M3-OIF", sizeof(oif_sd->name));
1641
1642	oif_sd->internal_ops = &oif_internal_ops;
1643	oif_sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1644
1645	state->oif_pads[OIF_ISP_PAD].flags = MEDIA_PAD_FL_SINK;
1646	state->oif_pads[OIF_JPEG_PAD].flags = MEDIA_PAD_FL_SINK;
1647	state->oif_pads[OIF_SOURCE_PAD].flags = MEDIA_PAD_FL_SOURCE;
1648	oif_sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
1649
1650	ret = media_entity_pads_init(&oif_sd->entity, OIF_NUM_PADS,
1651							state->oif_pads);
1652	if (ret < 0)
1653		return ret;
1654
1655	for (i = 0; i < S5C73M3_MAX_SUPPLIES; i++)
1656		state->supplies[i].supply = s5c73m3_supply_names[i];
1657
1658	ret = devm_regulator_bulk_get(dev, S5C73M3_MAX_SUPPLIES,
1659			       state->supplies);
1660	if (ret) {
1661		dev_err(dev, "failed to get regulators\n");
1662		goto out_err;
1663	}
1664
1665	ret = s5c73m3_init_controls(state);
1666	if (ret)
1667		goto out_err;
1668
1669	state->sensor_pix_size[RES_ISP] = &s5c73m3_isp_resolutions[1];
1670	state->sensor_pix_size[RES_JPEG] = &s5c73m3_jpeg_resolutions[1];
1671	state->oif_pix_size[RES_ISP] = state->sensor_pix_size[RES_ISP];
1672	state->oif_pix_size[RES_JPEG] = state->sensor_pix_size[RES_JPEG];
1673
1674	state->mbus_code = S5C73M3_ISP_FMT;
1675
1676	state->fiv = &s5c73m3_intervals[S5C73M3_DEFAULT_FRAME_INTERVAL];
1677
1678	state->fw_file_version[0] = 'G';
1679	state->fw_file_version[1] = 'C';
1680
1681	ret = s5c73m3_register_spi_driver(state);
1682	if (ret < 0)
1683		goto out_err;
1684
1685	oif_sd->dev = dev;
1686
1687	ret = __s5c73m3_power_on(state);
1688	if (ret < 0)
1689		goto out_err1;
1690
1691	ret = s5c73m3_get_fw_version(state);
1692	__s5c73m3_power_off(state);
1693
1694	if (ret < 0) {
1695		dev_err(dev, "Device detection failed: %d\n", ret);
1696		goto out_err1;
1697	}
1698
1699	ret = v4l2_async_register_subdev(oif_sd);
1700	if (ret < 0)
1701		goto out_err1;
1702
1703	v4l2_info(sd, "%s: completed successfully\n", __func__);
1704	return 0;
1705
1706out_err1:
1707	s5c73m3_unregister_spi_driver(state);
1708out_err:
1709	media_entity_cleanup(&sd->entity);
1710	return ret;
1711}
1712
1713static void s5c73m3_remove(struct i2c_client *client)
1714{
1715	struct v4l2_subdev *oif_sd = i2c_get_clientdata(client);
1716	struct s5c73m3 *state = oif_sd_to_s5c73m3(oif_sd);
1717	struct v4l2_subdev *sensor_sd = &state->sensor_sd;
1718
1719	v4l2_async_unregister_subdev(oif_sd);
1720
1721	v4l2_ctrl_handler_free(oif_sd->ctrl_handler);
1722	media_entity_cleanup(&oif_sd->entity);
1723
1724	v4l2_device_unregister_subdev(sensor_sd);
1725	media_entity_cleanup(&sensor_sd->entity);
1726
1727	s5c73m3_unregister_spi_driver(state);
1728}
1729
1730static const struct i2c_device_id s5c73m3_id[] = {
1731	{ DRIVER_NAME },
1732	{ }
1733};
1734MODULE_DEVICE_TABLE(i2c, s5c73m3_id);
1735
1736#ifdef CONFIG_OF
1737static const struct of_device_id s5c73m3_of_match[] = {
1738	{ .compatible = "samsung,s5c73m3" },
1739	{ }
1740};
1741MODULE_DEVICE_TABLE(of, s5c73m3_of_match);
1742#endif
1743
1744static struct i2c_driver s5c73m3_i2c_driver = {
1745	.driver = {
1746		.of_match_table = of_match_ptr(s5c73m3_of_match),
1747		.name	= DRIVER_NAME,
1748	},
1749	.probe		= s5c73m3_probe,
1750	.remove		= s5c73m3_remove,
1751	.id_table	= s5c73m3_id,
1752};
1753
1754module_i2c_driver(s5c73m3_i2c_driver);
1755
1756MODULE_DESCRIPTION("Samsung S5C73M3 camera driver");
1757MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
1758MODULE_LICENSE("GPL");