marvell/linux/drivers/media/i2c/gc032a.c

// SPDX-License-Identifier: GPL-2.0
/*
 * GC032A CMOS Image Sensor driver
 *
 * Copyright (C) 2023 ASR Mirco Co., Ltd.
 */

#include <linux/clk.h>
#include <linux/delay.h>
#include <linux/err.h>
#include <linux/gpio/consumer.h>
#include <linux/init.h>
#include <linux/interrupt.h>
#include <linux/io.h>
#include <linux/i2c.h>
#include <linux/regmap.h>
#include <linux/kernel.h>
#include <linux/media.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_graph.h>
#include <linux/regulator/consumer.h>
#include <linux/slab.h>
#include <linux/uaccess.h>
#include <linux/videodev2.h>
#include <linux/version.h>
#include <media/media-entity.h>
#include <media/v4l2-common.h>
#include <media/v4l2-ctrls.h>
#include <media/v4l2-device.h>
#include <media/v4l2-event.h>
#include <media/v4l2-fwnode.h>
#include <media/v4l2-image-sizes.h>
#include <media/v4l2-mediabus.h>
#include <media/v4l2-subdev.h>

#define DRIVER_VERSION			KERNEL_VERSION(0, 0x01, 0x1)
#define DRIVER_NAME "gc032a"
#define GC032A_PIXEL_RATE		(96 * 1000 * 1000)

/*
 * GC032A register definitions
 */
#define REG_SOFTWARE_STANDBY		0xf3

#define REG_SC_CHIP_ID_H		0xf0
#define REG_SC_CHIP_ID_L		0xf1

#define REG_NULL			0xFFFF	/* Array end token */

#define SENSOR_ID(_msb, _lsb)		((_msb) << 8 | (_lsb))
#define GC032A_ID			0x232a

/* gc isp nead extra 6 lines with raw sensor */
#define EXTRA_LINES_RAW_SENSOR 6

struct sensor_register {
	u16 addr;
	u8 value;
};

enum cam_interface {
	INF_SPI1LAN = 0x0,
	INF_SPI2LAN,
	INF_SPI4LAN,
	INF_MIPI1LAN,
	INF_MIPI2LAN,
	INF_MIPI4LAN,
	INF_DVP,
	INF_MAX,
};

struct spi_param {
	u8 spi_sdr;		 //0x0:no sdr	0x1:sdr
	u8 spi_crc;		 //0x0:no crc	0x1:crc
	u8 spi_manual_enable;	 //0x0:not enable	0x1:enable
	u8 spi_manual_mode;
	u8 spi_manual_height_enable;	//0x0:not enable   0x1:enable
	u8 spi_manual_width_enable;	//0x0:not enable   0x1:enable
	u16 spi_manual_height;
	u16 spi_manual_width;
	u8 spi_ignore_line_id;
};

struct gc032a_framesize {
	u16 width;
	u16 height;
	u16 max_exp_lines;
	struct v4l2_fract max_fps;
	const struct sensor_register *regs;
};

struct  gc032a_spi_config {
	enum cam_interface inf;
	union {
		struct spi_param inf_spi;
	} u;
};

struct gc032a_pll_ctrl {
	u8 ctrl1;
	u8 ctrl2;
	u8 ctrl3;
};

struct gc032a_pixfmt {
	u32 code;
	/* Output format Register Value (REG_FORMAT_CTRL00) */
	struct sensor_register *format_ctrl_regs;
};

struct pll_ctrl_reg {
	unsigned int div;
	unsigned char reg;
};

static const char * const gc032a_supply_names[] = {
	"avdd",		/* Analog power */
	"iovdd",	/* Digital I/O power */
};

#define GC032A_NUM_SUPPLIES ARRAY_SIZE(gc032a_supply_names)

struct gc032a {
	struct v4l2_subdev sd;
	struct media_pad pad;
	struct v4l2_mbus_framefmt format;
	struct gpio_desc *pwdn_gpio;
	struct gpio_desc *power_gpio;
	struct regulator_bulk_data supplies[GC032A_NUM_SUPPLIES];
	struct mutex lock;
	struct i2c_client *client;
	struct v4l2_ctrl_handler ctrls;
	struct v4l2_ctrl *link_frequency;
	const struct gc032a_framesize *frame_size;
	int streaming;
	bool power_on;
};

#define NUM_BR_LEVELS	9

#define GC032A_SET_QUICK_STREAM   \
    _IOW('V', BASE_VIDIOC_PRIVATE + 0, __u32)

extern void asr_camera_mclk_ctrl(int on);
/* yuv422 640 * 480 spi 2 lan */
static const struct sensor_register gc032a_init_yuv422_640x480_spi2lan_crc_sdr[] = 
{
    {0xf3, 0x83},    /*System*/
    {0xf5, 0x0c},
    {0xf7, 0x13},    //{0xf7, 0x11},
    {0xf8, 0x0f},    //{0xf8, 0x07}, pclk 24M
    {0xf9, 0x4e},
    {0xfa, 0x31},    //{0xfa, 0x10},
    {0xfc, 0x02},
    {0xfe, 0x02},
    {0x81, 0x03},
    {0xfe, 0x00},    /*Analog&Cisctl*/
    {0x03, 0x01},
    {0x04, 0xc2},
    {0x05, 0x01},
    {0x06, 0xa3},
    {0x07, 0x00},
    {0x08, 0x08},
    {0x0a, 0x04},
    {0x0c, 0x04},
    {0x0d, 0x01},
    {0x0e, 0xe8},
    {0x0f, 0x02},
    {0x10, 0x88},
    {0x17, 0x54},
    {0x19, 0x04},
    {0x1a, 0x0a},
    {0x1f, 0x40},
    {0x20, 0x30},
    {0x2e, 0x80},
    {0x2f, 0x2b},
    {0x30, 0x1a},
    {0xfe, 0x02},
    {0x03, 0x02},    //[4:0]post_tx_width
    {0x06, 0x60},    //[5:4]stsbp_mode
    {0x05, 0xd7},    //drv
    {0x12, 0x89},    //[7:6]init_ramp_mode
    {0xfe, 0x03},    /*SPI*/
    {0x51, 0x00},    //0x01	 can not stream on
    {0x52, 0xda},
    {0x53, 0xa4},    //no crc 0x24     crc 0xa4
    {0x54, 0x20},
    {0x55, 0x00},
    {0x59, 0x10},    //	{0x59, 0x30},
    {0x5a, 0x00},    //0x01  type
    {0x5b, 0x80},
    {0x5c, 0x02},
    {0x5d, 0xe0},    // e0
    {0x5e, 0x01},
    {0x64, 0x06},
    {0xfe, 0x00},    /*blk*/
    {0x18, 0x02},
    {0xfe, 0x02},
    {0x40, 0x22},
    {0x45, 0x00},
    {0x46, 0x00},
    {0x49, 0x20},
    {0x4b, 0x3c},
    {0x50, 0x20},
    {0x42, 0x10},
    {0xfe, 0x01},    /*isp*/
    {0x0a, 0xc5},
    {0x45, 0x00},    //[6]darksun_en
    {0xfe, 0x00},
    {0x40, 0xff},
    {0x41, 0x25},
    {0x42, 0xcf},
    {0x43, 0x10},
    {0x44, 0x83},
    {0x46, 0x26},
    {0x49, 0x03},
    {0x4f, 0x01},   //[0]AEC_en
    {0xde, 0x84},
    {0xfe, 0x02},
    {0x22, 0xf6},    //CISCTL_SUN_TH_R
    {0xfe, 0x01},    /*Shading*/ 
    {0xc1, 0x3c},
    {0xc2, 0x50},
    {0xc3, 0x00},
    {0xc4, 0x32},
    {0xc5, 0x24},
    {0xc6, 0x16},
    {0xc7, 0x08},
    {0xc8, 0x08},
    {0xc9, 0x00},
    {0xca, 0x20},
    {0xdc, 0x8a},
    {0xdd, 0xa0},
    {0xde, 0xa6},
    {0xdf, 0x75},
    {0xfe, 0x01},    /*AWB*/
    {0x7c, 0x09},
    {0x65, 0x06},
    {0x7c, 0x08},
    {0x56, 0xf4}, 
    {0x66, 0x0f}, 
    {0x67, 0x84}, 
    {0x6b, 0x80},
    {0x6d, 0x12},
    {0x6e, 0xb0}, 
    {0x86, 0x00},
    {0x87, 0x00},
    {0x88, 0x00},
    {0x89, 0x00},
    {0x8a, 0x00},
    {0x8b, 0x00},
    {0x8c, 0x00},
    {0x8d, 0x00},
    {0x8e, 0x00},
    {0x8f, 0x00},
    {0x90, 0xef},
    {0x91, 0xe1},
    {0x92, 0x0c},
    {0x93, 0xef},
    {0x94, 0x65},
    {0x95, 0x1f},
    {0x96, 0x0c},
    {0x97, 0x2d},
    {0x98, 0x20},
    {0x99, 0xaa},
    {0x9a, 0x3f},
    {0x9b, 0x2c},
    {0x9c, 0x5f},
    {0x9d, 0x3e},
    {0x9e, 0xaa},
    {0x9f, 0x67},
    {0xa0, 0x60},
    {0xa1, 0x00},
    {0xa2, 0x00},
    {0xa3, 0x0a},
    {0xa4, 0xb6},
    {0xa5, 0xac},
    {0xa6, 0xc1},
    {0xa7, 0xac},
    {0xa8, 0x55},
    {0xa9, 0xc3},
    {0xaa, 0xa4},
    {0xab, 0xba},
    {0xac, 0xa8},
    {0xad, 0x55},
    {0xae, 0xc8},
    {0xaf, 0xb9},
    {0xb0, 0xd4},
    {0xb1, 0xc3},
    {0xb2, 0x55},
    {0xb3, 0xd8},
    {0xb4, 0xce},
    {0xb5, 0x00},
    {0xb6, 0x00},
    {0xb7, 0x05},
    {0xb8, 0xd6},
    {0xb9, 0x8c},
    {0xfe, 0x01},    /*CC*/
    {0xd0, 0x40},    //3a
    {0xd1, 0xf8},
    {0xd2, 0x00},
    {0xd3, 0xfa},
    {0xd4, 0x45},
    {0xd5, 0x02},
    {0xd6, 0x30},
    {0xd7, 0xfa},
    {0xd8, 0x08},
    {0xd9, 0x08},
    {0xda, 0x58},
    {0xdb, 0x02},
    {0xfe, 0x00},
    {0xfe, 0x00},    /*Gamma*/
    {0xba, 0x00},
    {0xbb, 0x04},
    {0xbc, 0x0a},
    {0xbd, 0x0e},
    {0xbe, 0x22},
    {0xbf, 0x30},
    {0xc0, 0x3d},
    {0xc1, 0x4a},
    {0xc2, 0x5d},
    {0xc3, 0x6b},
    {0xc4, 0x7a},
    {0xc5, 0x85},
    {0xc6, 0x90},
    {0xc7, 0xa5},
    {0xc8, 0xb5},
    {0xc9, 0xc2},
    {0xca, 0xcc},
    {0xcb, 0xd5},
    {0xcc, 0xde},
    {0xcd, 0xea},
    {0xce, 0xf5},
    {0xcf, 0xff},
    {0xfe, 0x00},    /*Auto Gamma*/
    {0x5a, 0x08},
    {0x5b, 0x0f},
    {0x5c, 0x15},
    {0x5d, 0x1c},
    {0x5e, 0x28},
    {0x5f, 0x36},
    {0x60, 0x45},
    {0x61, 0x51},
    {0x62, 0x6a},
    {0x63, 0x7d},
    {0x64, 0x8d},
    {0x65, 0x98},
    {0x66, 0xa2},
    {0x67, 0xb5},
    {0x68, 0xc3},
    {0x69, 0xcd},
    {0x6a, 0xd4},
    {0x6b, 0xdc},
    {0x6c, 0xe3},
    {0x6d, 0xf0},
    {0x6e, 0xf9},
    {0x6f, 0xff},
    {0xfe, 0x00},    /*Gain*/
    {0x70, 0x50},
    {0xfe, 0x00},    /*AEC*/
    {0x4f, 0x01},
    {0xfe, 0x01},
    {0x44, 0x04},
    {0x1f, 0x30},
    {0x20, 0x40},
    {0x26, 0x4e},
    {0x27, 0x01},
    {0x28, 0xd4}, 
    {0x29, 0x03},
    {0x2a, 0x0c},
    {0x2b, 0x03},
    {0x2c, 0xe9},
    {0x2d, 0x07},
    {0x2e, 0xd2},
    {0x2f, 0x0b},
    {0x30, 0x6e},
    {0x31, 0x0e},
    {0x32, 0x70},
    {0x33, 0x12},
    {0x34, 0x0c},
    {0x3c, 0x10},    //[5:4] Max level setting
    {0x3e, 0x20},
    {0x3f, 0x2d},
    {0x40, 0x40},
    {0x41, 0x5b},
    {0x42, 0x82},
    {0x43, 0xb7},
    {0x04, 0x0a},
    {0x02, 0x79},
    {0x03, 0xc0},
    {0xcc, 0x08},    /*measure window*/
    {0xcd, 0x08},
    {0xce, 0xa4},
    {0xcf, 0xec},
    {0xfe, 0x00},    /*DNDD*/
    {0x81, 0xb8},    //f8
    {0x82, 0x12},
    {0x83, 0x0a},
    {0x84, 0x01},
    {0x86, 0x50},
    {0x87, 0x18},
    {0x88, 0x10},
    {0x89, 0x70},
    {0x8a, 0x20},
    {0x8b, 0x10},
    {0x8c, 0x08},
    {0x8d, 0x0a},
    {0xfe, 0x00},    /*Intpee*/
    {0x8f, 0xaa},
    {0x90, 0x9c},
    {0x91, 0x52},
    {0x92, 0x03},
    {0x93, 0x03},
    {0x94, 0x08},
    {0x95, 0x44},
    {0x97, 0x00},
    {0x98, 0x00},
    {0xfe, 0x00},    /*ASDE*/ 
    {0xa1, 0x30},
    {0xa2, 0x41},
    {0xa4, 0x30},
    {0xa5, 0x20},
    {0xaa, 0x30},
    {0xac, 0x32},
    {0xfe, 0x00},    /*YCP*/
    {0xd1, 0x3c},
    {0xd2, 0x3c},
    {0xd3, 0x38},
    {0xd6, 0xf4},
    {0xd7, 0x1d},
    {0xdd, 0x73},
    {0xde, 0x84},
#if 0
    /* sensor crop */
    {0x50, 0x01},
    // 320x240
    {0x55, 0x00}, // height
    {0x56, 0xf0},
    {0x57, 0x01}, // width
    {0x58, 0x40},

    {0xfe, 0x03},
    {0x5b, 0x40}, //spi width
    {0x5c, 0x01},
    {0x5d, 0xf0}, // spi height
    {0x5e, 0x00},
    {0xfe, 0x00},

    /* sensor crop */
    {0x50, 0x01},
    // 480x480
    {0x55, 0x01}, // height
    {0x56, 0xe0},
    {0x57, 0x01}, // width
    {0x58, 0xe0},

    {0xfe, 0x03},
    {0x5b, 0xe0}, //spi width
    {0x5c, 0x01},
    {0x5d, 0xe0}, // spi height
    {0x5e, 0x01},
    {0xfe, 0x00},
#endif
    //{0x4c, 0x08},   //for color bar
    {REG_NULL, 0x00},
};

static const struct gc032a_framesize gc032a_framesizes[] = {
	{
		.width		= 640,
		.height		= 480,
		.max_fps = {
			.numerator = 10000,
			.denominator = 200000,
		},
		.regs		= gc032a_init_yuv422_640x480_spi2lan_crc_sdr,
		.max_exp_lines	= 488,
	},
};

static const struct gc032a_spi_config gc032a_spi[] = {
	{
		.inf = INF_SPI2LAN,
		.u = {
			.inf_spi = {
				.spi_sdr = 0,		  //0x0:sdr
				.spi_crc = 1,		  //0x1:crc
				.spi_manual_enable = 0,   //0x0:not enable	 0x1:enable
				.spi_manual_mode = 0,
				.spi_manual_height_enable = 0,	 //0x0:not enable	0x1:enable
				.spi_manual_width_enable = 0,	 //0x0:not enable	0x1:enable
				.spi_manual_height = 0,
				.spi_manual_width = 0,
				.spi_ignore_line_id = 1,
			},
		},
	},
};

static const struct gc032a_pixfmt gc032a_formats[] = {
	{
	.code = MEDIA_BUS_FMT_YVYU8_2X8,
	},
};

static inline struct gc032a *to_gc032a(struct v4l2_subdev *sd)
{
	return container_of(sd, struct gc032a, sd);
}

/* sensor register write */
static int gc032a_write(struct i2c_client *client, u8 reg, u8 val)
{
	struct i2c_msg msg;
	u8 buf[2];
	int ret;

	buf[0] = reg & 0xFF;
	buf[1] = val;

	msg.addr = client->addr;
	msg.flags = client->flags;
	msg.buf = buf;
	msg.len = sizeof(buf);

	ret = i2c_transfer(client->adapter, &msg, 1);
	if (ret >= 0)
		return 0;

	dev_err(&client->dev,
		"gc032a write reg(0x%x val:0x%x) failed !\n", reg, val);

	return ret;
}

/* sensor register read */
static int gc032a_read(struct i2c_client *client, u8 reg, u8 *val)
{
	struct i2c_msg msg[2];
	u8 buf[1];
	int ret;

	buf[0] = reg & 0xFF;

	msg[0].addr = client->addr;
	msg[0].flags = client->flags;
	msg[0].buf = buf;
	msg[0].len = sizeof(buf);

	msg[1].addr = client->addr;
	msg[1].flags = client->flags | I2C_M_RD;
	msg[1].buf = buf;
	msg[1].len = 1;

	ret = i2c_transfer(client->adapter, msg, 2);
	if (ret >= 0) {
		*val = buf[0];
		return 0;
	}

	dev_err(&client->dev,
		"gc032a read reg:0x%x failed !\n", reg);
	return ret;
}

static int gc032a_write_array(struct i2c_client *client,
				  const struct sensor_register *regs)
{
	int i, ret = 0;

	i = 0;
	while (regs[i].addr != REG_NULL) {
		ret = gc032a_write(client, regs[i].addr, regs[i].value);
		if (ret) {
			dev_err(&client->dev, "%s failed !, i=%d\n", __func__,i);
			break;
		}

		i++;
	}

	return ret;
}

static void gc032a_get_default_format(struct v4l2_mbus_framefmt *format)
{
	format->width = gc032a_framesizes[0].width;
	format->height = gc032a_framesizes[0].height;
	format->colorspace = V4L2_COLORSPACE_SRGB;	//TODO
	format->code = gc032a_formats[0].code;
	format->field = V4L2_FIELD_NONE;

	format->reserved[0] = gc032a_spi[0].inf;
	format->reserved[1] = gc032a_spi[0].u.inf_spi.spi_sdr;
	format->reserved[2] = gc032a_spi[0].u.inf_spi.spi_crc;
	format->reserved[3] = gc032a_spi[0].u.inf_spi.spi_manual_enable;
	format->reserved[4] = gc032a_spi[0].u.inf_spi.spi_manual_mode;
	format->reserved[5] = gc032a_spi[0].u.inf_spi.spi_manual_height_enable;
	format->reserved[6] = gc032a_spi[0].u.inf_spi.spi_manual_width_enable;
	format->reserved[7] = gc032a_spi[0].u.inf_spi.spi_manual_height;
	format->reserved[8] = gc032a_spi[0].u.inf_spi.spi_manual_width;
	format->reserved[9] = gc032a_spi[0].u.inf_spi.spi_ignore_line_id;
	pr_debug("%s: %x %dx%d lane%d sdr%d crc%d line_id%d\n", __func__,
		format->code, format->width,
		format->height, format->reserved[0],
		format->reserved[1], format->reserved[2], format->reserved[9]);
}

static void gc032a_set_streaming(struct gc032a *gc032a, int on)
{
	struct i2c_client *client = gc032a->client;
	int ret;

	dev_dbg(&client->dev, "%s: on: %d\n", __func__, on);

	ret = gc032a_write(client, 0xfe, 0x03); //page select
	if (ret)
		dev_err(&client->dev, "gc032a write 0xfe, 0x03 failed ret = %d\n",ret );
	ret = gc032a_write(client, 0x51, on);
	if (ret)
		dev_err(&client->dev, "gc032a write 0x51, 0x%x failed ret = %d\n", on, ret);
}

/*
 * V4L2 subdev video and pad level operations
 */
static int gc032a_enum_mbus_code(struct v4l2_subdev *sd,
				 struct v4l2_subdev_pad_config *cfg,
				 struct v4l2_subdev_mbus_code_enum *code)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);

	dev_dbg(&client->dev, "%s:\n", __func__);

	if (code->index >= ARRAY_SIZE(gc032a_formats))
		return -EINVAL;

	code->code = gc032a_formats[code->index].code;

	return 0;
}

static int gc032a_enum_frame_sizes(struct v4l2_subdev *sd,
				   struct v4l2_subdev_pad_config *cfg,
				   struct v4l2_subdev_frame_size_enum *fse)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	int i = ARRAY_SIZE(gc032a_formats);

	dev_dbg(&client->dev, "%s:\n", __func__);

	if (fse->index >= ARRAY_SIZE(gc032a_framesizes))
		return -EINVAL;

	while (--i)
		if (fse->code == gc032a_formats[i].code)
			break;

	fse->code = gc032a_formats[i].code;

	fse->min_width  = gc032a_framesizes[fse->index].width;
	fse->max_width  = fse->min_width;
	fse->max_height = gc032a_framesizes[fse->index].height;
	fse->min_height = fse->max_height;

	return 0;
}

static int gc032a_get_fmt(struct v4l2_subdev *sd,
			  struct v4l2_subdev_pad_config *cfg,
			  struct v4l2_subdev_format *fmt)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct gc032a *gc032a = to_gc032a(sd);

	dev_dbg(&client->dev, "%s enter\n", __func__);

	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
		struct v4l2_mbus_framefmt *mf;

		mf = v4l2_subdev_get_try_format(sd, cfg, 0);
		mutex_lock(&gc032a->lock);
		fmt->format = *mf;
		mutex_unlock(&gc032a->lock);
		return 0;
#else
	return -ENOTTY;
#endif
	}

	mutex_lock(&gc032a->lock);
	fmt->format = gc032a->format;
	mutex_unlock(&gc032a->lock);

	dev_dbg(&client->dev, "%s: %x %dx%d lane%d sdr%d crc%d line_id%d\n", __func__,
		gc032a->format.code, gc032a->format.width,
		gc032a->format.height, gc032a->format.reserved[0],
		gc032a->format.reserved[1], gc032a->format.reserved[2], gc032a->format.reserved[9]);

	return 0;
}

static void __gc032a_try_frame_size(struct v4l2_mbus_framefmt *mf,
				    const struct gc032a_framesize **size)
{
	const struct gc032a_framesize *fsize = &gc032a_framesizes[0];
	const struct gc032a_framesize *match = NULL;
	int i = ARRAY_SIZE(gc032a_framesizes);
	unsigned int min_err = UINT_MAX;

	while (i--) {
		unsigned int err = abs(fsize->width - mf->width)
				+ abs(fsize->height - mf->height);
		if (err < min_err && fsize->regs[0].addr) {
			min_err = err;
			match = fsize;
		}
		fsize++;
	}

	if (!match)
		match = &gc032a_framesizes[0];

	mf->width  = match->width;
	mf->height = match->height;

	if (size)
		*size = match;
}

static int gc032a_set_fmt(struct v4l2_subdev *sd,
			  struct v4l2_subdev_pad_config *cfg,
			  struct v4l2_subdev_format *fmt)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	int index = ARRAY_SIZE(gc032a_formats);
	struct v4l2_mbus_framefmt *mf = &fmt->format;
	const struct gc032a_framesize *size = NULL;
	struct gc032a *gc032a = to_gc032a(sd);
	int ret = 0;

	dev_dbg(&client->dev, "%s enter, code:0x%x\n", __func__, mf->code);

	__gc032a_try_frame_size(mf, &size);

	while (--index >= 0)
		if (gc032a_formats[index].code == mf->code)
			break;

	if (index < 0)
		return -EINVAL;

	mf->colorspace = V4L2_COLORSPACE_SRGB;
	mf->code = gc032a_formats[index].code;
	mf->field = V4L2_FIELD_NONE;

	mutex_lock(&gc032a->lock);

	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
		*mf = fmt->format;
#else
		return -ENOTTY;
#endif
	} else {
		if (gc032a->streaming) {
			mutex_unlock(&gc032a->lock);
			return -EBUSY;
		}

		gc032a->frame_size = size;
		gc032a->format = fmt->format;
	}

	mutex_unlock(&gc032a->lock);
	return ret;
}

static long gc032a_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
{
	struct gc032a *gc032a = to_gc032a(sd);
	long ret = 0;
	u32 stream = 0;

	switch (cmd) {
	case GC032A_SET_QUICK_STREAM:

		stream = *((u32 *)arg);

		if (stream)
			gc032a_set_streaming(gc032a, 1);
		else
			gc032a_set_streaming(gc032a, 0);
		break;
	default:
		ret = -ENOIOCTLCMD;
		break;
	}

	return ret;
}

#ifdef CONFIG_COMPAT
static long gc032a_compat_ioctl32(struct v4l2_subdev *sd,
				  unsigned int cmd, unsigned long arg)
{
	void __user *up = compat_ptr(arg);
	long ret;
	u32 stream = 0;

	switch (cmd) {
	case GC032A_SET_QUICK_STREAM:
		ret = copy_from_user(&stream, up, sizeof(u32));
		if (!ret)
			ret = gc032a_ioctl(sd, cmd, &stream);
		break;
	default:
		ret = -ENOIOCTLCMD;
		break;
	}

	return ret;
}
#endif

static int gc032a_s_stream(struct v4l2_subdev *sd, int on)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct gc032a *gc032a = to_gc032a(sd);

	dev_dbg(&client->dev, "%s: on: %d\n", __func__, on);

	mutex_lock(&gc032a->lock);
	on = !!on;
	if (gc032a->streaming == on)
		goto unlock;

	if (!on) {
		/* Stop Streaming Sequence */
		gc032a_set_streaming(gc032a, 0);
		gc032a->streaming = on;
		goto unlock;
	}

	gc032a_set_streaming(gc032a, 1);
	gc032a->streaming = on;

unlock:
	mutex_unlock(&gc032a->lock);
	return 0;
}

static int gc032a_set_test_pattern(struct gc032a *gc032a, int value)
{
	return 0;
}

static int gc032a_yuv_set_banding(struct gc032a *gc032a, int value)
{
	struct i2c_client *client = gc032a->client;
	int ret;

	dev_dbg(&client->dev, "%s: value: %d\n", __func__, value);

	switch (value) {
		case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
			gc032a_write(client, 0xfe, 0x00);
			break;
		case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
			gc032a_write(client, 0xfe, 0x00);
			break;
		case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
			gc032a_write(client, 0xfe, 0x00);
			break;
		default:
			dev_err(&client->dev, "invalid banding mode %d", value);
			ret = -EINVAL;
	}
	return ret;
}

static int gc032a_yuv_set_brightness(struct gc032a *gc032a, int value)
{
	struct i2c_client *client = gc032a->client;
	static const u8 regs[NUM_BR_LEVELS + 1] = {
		0xfe,
		0x00, /* 1 */
		0x00, /* 2 */
		0x00, /* 3 */
		0x00, /* 4 */
		0x00, /* 5 */
		0x00, /* 6 */
		0x00, /* 7 */
		0x00, /* 8 */
		0x00, /* 9 */
	};
	int ret = 0;

	dev_dbg(&client->dev, "%s: value: %d\n", __func__, value);

	if (value > NUM_BR_LEVELS)
		return -EINVAL;

	ret = gc032a_write(client, regs[0], regs[value]);
	return ret;
}

static int gc032a_set_exposure(struct gc032a *gc032a, int value)
{
	struct i2c_client *client = gc032a->client;
	int ret;

	dev_dbg(&client->dev, "%s: value: %d\n", __func__, value);

	ret = gc032a_write(client, 0xfe, 0x00); //page select
	if (ret)
		dev_err(&client->dev, "gc032a write 0xfe, 0x00 failed ret = %d\n",ret );

	ret = gc032a_write(client, 0x04, value& 0xff); //exp_low
	if (ret < 0)
		dev_err(&client->dev, "%s: error!", __func__);

	ret = gc032a_write(client, 0x03, (value >> 8) & 0x0f); //exp_high
	if (ret < 0)
		dev_err(&client->dev, "%s: error!", __func__);

	return ret;
}

static int gc032a_set_gain(struct gc032a *gc032a, int value)
{
	struct i2c_client *client = gc032a->client;
	int ret;

	dev_info(&client->dev, "%s: value: %d\n", __func__, value);

	ret = gc032a_write(client, 0xfe, 0x00); //page select
	if (ret)
		dev_err(&client->dev, "gc032a write 0xfe, 0x00 failed ret = %d\n",ret );
	ret = gc032a_write(client, 0x48, value);
	if (ret < 0)
		dev_err(&client->dev, "%s: error!", __func__);

	return ret;
}

static int gc032a_s_ctrl(struct v4l2_ctrl *ctrl)
{
	struct gc032a *gc032a =
			container_of(ctrl->handler, struct gc032a, ctrls);
	int ret = -EINVAL;

	mutex_lock(&gc032a->lock);
	/*
	 * If the device is not powered up n ow postpone applying control's
	 * value to the hardware, until it is ready to accept commands.
	 */
	if (gc032a->power_on == 0) {
		mutex_unlock(&gc032a->lock);
		return 0;
	}

	switch (ctrl->id) {

	case V4L2_CID_BRIGHTNESS:
		ret = gc032a_yuv_set_brightness(gc032a, ctrl->val);
		break;
	case V4L2_CID_POWER_LINE_FREQUENCY:
		ret = gc032a_yuv_set_banding(gc032a, ctrl->val);
		break;
	case V4L2_CID_EXPOSURE:
		ret = gc032a_set_exposure(gc032a, ctrl->val);
		break;
	case V4L2_CID_GAIN:
		ret = gc032a_set_gain(gc032a, ctrl->val);
		break;
	case V4L2_CID_TEST_PATTERN:
		ret = gc032a_set_test_pattern(gc032a, ctrl->val);
		break;
	}

	mutex_unlock(&gc032a->lock);
	return ret;
}

static const struct v4l2_ctrl_ops gc032a_ctrl_ops = {
	.s_ctrl = gc032a_s_ctrl,
};

static const char * const gc032a_test_pattern_menu[] = {
	"Disabled",
	"Vertical Color Bars",
};

/* -----------------------------------------------------------------------------
 * V4L2 subdev internal operations
 */

#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
static int gc032a_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct v4l2_mbus_framefmt *format =
				v4l2_subdev_get_try_format(sd, fh->pad, 0);

	dev_dbg(&client->dev, "%s:\n", __func__);

	gc032a_get_default_format(format);

	return 0;
}
#endif

static int gc032a_g_mbus_config(struct v4l2_subdev *sd,
				struct v4l2_mbus_config *config)
{
	config->type = V4L2_MBUS_PARALLEL;
	config->flags = V4L2_MBUS_PCLK_SAMPLE_RISING;

	return 0;
}

static int __gc032a_power_on(struct gc032a *gc032a)
{
	int ret;
	struct device *dev = &gc032a->client->dev;

	if (gc032a->power_on)
		return 0;

	if (!IS_ERR(gc032a->power_gpio)) {
		gpiod_set_value_cansleep(gc032a->power_gpio, 1);
		usleep_range(2000, 5000);
	} else if (!IS_ERR(gc032a->supplies)) {
		ret = regulator_bulk_enable(GC032A_NUM_SUPPLIES,
			gc032a->supplies);
		if (ret < 0)
			dev_err(dev, "Failed to enable regulators\n");

		usleep_range(2000, 5000);
	}

	asr_camera_mclk_ctrl(1);
	usleep_range(2000, 5000);

	if (!IS_ERR(gc032a->pwdn_gpio)) {
		gpiod_set_value_cansleep(gc032a->pwdn_gpio, 1);
		usleep_range(2000, 5000);
	}

	if (!IS_ERR(gc032a->pwdn_gpio)) {
		gpiod_set_value_cansleep(gc032a->pwdn_gpio, 0);
		usleep_range(2000, 5000);
	}

	gc032a->power_on = true;
	return 0;
}

static void __gc032a_power_off(struct gc032a *gc032a)
{
	if (!gc032a->power_on)
		return;

	if (!IS_ERR(gc032a->pwdn_gpio)) {
		gpiod_set_value_cansleep(gc032a->pwdn_gpio, 1);
		usleep_range(2000, 5000);
	}

	asr_camera_mclk_ctrl(0);
	usleep_range(2000, 5000);

	if (!IS_ERR(gc032a->power_gpio)) {
		gpiod_set_value_cansleep(gc032a->power_gpio, 0);
		usleep_range(2000, 5000);
	} else if (!IS_ERR(gc032a->supplies)) {
		regulator_bulk_disable(GC032A_NUM_SUPPLIES, gc032a->supplies);
		usleep_range(2000, 5000);
	}

	if (!IS_ERR(gc032a->pwdn_gpio))
		gpiod_set_value_cansleep(gc032a->pwdn_gpio, 0);
	usleep_range(7000, 10000);
	gc032a->power_on = false;
}

static int gc032a_power(struct v4l2_subdev *sd, int on)
{
	int ret;
	struct gc032a *gc032a = to_gc032a(sd);
	struct i2c_client *client = gc032a->client;

	dev_dbg(&client->dev, "%s(%d) on(%d)\n", __func__, __LINE__, on);
	if (on) {
		__gc032a_power_on(gc032a);
		ret = gc032a_write_array(client, gc032a->frame_size->regs);
		if (ret)
			dev_err(&client->dev, "init error\n");
	} else {
		__gc032a_power_off(gc032a);
	}
	return 0;
}

static int gc032a_enum_frame_interval(struct v4l2_subdev *sd,
				       struct v4l2_subdev_pad_config *cfg,
				       struct v4l2_subdev_frame_interval_enum *fie)
{
	if (fie->index >= ARRAY_SIZE(gc032a_framesizes))
		return -EINVAL;

	if (fie->code != MEDIA_BUS_FMT_YVYU8_2X8)
		return -EINVAL;

	fie->width = gc032a_framesizes[fie->index].width;
	fie->height = gc032a_framesizes[fie->index].height;
	fie->interval = gc032a_framesizes[fie->index].max_fps;
	return 0;
}

static const struct v4l2_subdev_core_ops gc032a_subdev_core_ops = {
	.log_status = v4l2_ctrl_subdev_log_status,
	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
	.ioctl = gc032a_ioctl,
#ifdef CONFIG_COMPAT
	.compat_ioctl32 = gc032a_compat_ioctl32,
#endif
	.s_power = gc032a_power,
};

static const struct v4l2_subdev_video_ops gc032a_subdev_video_ops = {
	.s_stream = gc032a_s_stream,
	.g_mbus_config = gc032a_g_mbus_config,
};

static const struct v4l2_subdev_pad_ops gc032a_subdev_pad_ops = {
	.enum_mbus_code = gc032a_enum_mbus_code,
	.enum_frame_size = gc032a_enum_frame_sizes,
	.enum_frame_interval = gc032a_enum_frame_interval,
	.get_fmt = gc032a_get_fmt,
	.set_fmt = gc032a_set_fmt,
};

#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
static const struct v4l2_subdev_ops gc032a_subdev_ops = {
	.core  = &gc032a_subdev_core_ops,
	.video = &gc032a_subdev_video_ops,
	.pad   = &gc032a_subdev_pad_ops,
};

static const struct v4l2_subdev_internal_ops gc032a_subdev_internal_ops = {
	.open = gc032a_open,
};
#endif

static int gc032a_detect(struct gc032a *gc032a)
{
	struct i2c_client *client = gc032a->client;
	u8 pid, ver;
	int ret;

	dev_dbg(&client->dev, "%s:\n", __func__);

	/* Check sensor revision */
	ret = gc032a_read(client, REG_SC_CHIP_ID_H, &pid);
	if (!ret)
		ret = gc032a_read(client, REG_SC_CHIP_ID_L, &ver);

	if (!ret) {
		unsigned short id;

		id = SENSOR_ID(pid, ver);
		if (id != GC032A_ID) {
			ret = -1;
			dev_err(&client->dev,
				"Sensor detection failed (%04X, %d)\n", id, ret);
		} else
			dev_info(&client->dev, "Found GC%04X sensor\n", id);
	}

	return ret;
}

static int gc032a_configure_regulators(struct gc032a *gc032a)
{
	unsigned int i;

	for (i = 0; i < GC032A_NUM_SUPPLIES; i++)
		gc032a->supplies[i].supply = gc032a_supply_names[i];

	return devm_regulator_bulk_get(&gc032a->client->dev,
				       GC032A_NUM_SUPPLIES,
				       gc032a->supplies);
}

static int gc032a_parse_of(struct gc032a *gc032a)
{
	struct device *dev = &gc032a->client->dev;
	int ret;

	gc032a->pwdn_gpio = devm_gpiod_get(dev, "pwdn", GPIOD_OUT_LOW);
	if (IS_ERR(gc032a->pwdn_gpio))
		dev_err(dev, "Failed to get pwdn-gpios, maybe no used\n");

	gc032a->power_gpio = devm_gpiod_get(dev, "power", GPIOD_OUT_LOW);
	if (IS_ERR(gc032a->power_gpio)) {
		dev_err(dev, "Failed to get reset-gpios, maybe no used\n");

		ret = gc032a_configure_regulators(gc032a);
		if (ret)
			dev_err(dev, "Failed to get power regulators\n");
	}

	return __gc032a_power_on(gc032a);
}

static int gc032a_probe(struct i2c_client *client,
			const struct i2c_device_id *id)
{
	struct device *dev = &client->dev;
	struct v4l2_subdev *sd;
	struct gc032a *gc032a;
	int ret;

	dev_info(dev, "driver version: %02x.%02x.%02x",
		DRIVER_VERSION >> 16,
		(DRIVER_VERSION & 0xff00) >> 8,
		DRIVER_VERSION & 0x00ff);

	gc032a = devm_kzalloc(&client->dev, sizeof(*gc032a), GFP_KERNEL);
	if (!gc032a)
		return -ENOMEM;

	gc032a->client = client;

	gc032a_parse_of(gc032a);

	v4l2_ctrl_handler_init(&gc032a->ctrls, 6);
	gc032a->link_frequency =
			v4l2_ctrl_new_std(&gc032a->ctrls, &gc032a_ctrl_ops,
					  V4L2_CID_PIXEL_RATE, 0,
					  GC032A_PIXEL_RATE, 1,
					  GC032A_PIXEL_RATE);
	v4l2_ctrl_new_std_menu_items(&gc032a->ctrls, &gc032a_ctrl_ops,
				     V4L2_CID_TEST_PATTERN,
				     ARRAY_SIZE(gc032a_test_pattern_menu) - 1,
				     0, 0, gc032a_test_pattern_menu);
	v4l2_ctrl_new_std(&gc032a->ctrls, &gc032a_ctrl_ops,
							 V4L2_CID_EXPOSURE, 1, 32, 1, 32);
	v4l2_ctrl_new_std(&gc032a->ctrls, &gc032a_ctrl_ops,
						 V4L2_CID_GAIN, 16, 1023, 1, 16);
	v4l2_ctrl_new_std_menu(&gc032a->ctrls, &gc032a_ctrl_ops,
						   V4L2_CID_POWER_LINE_FREQUENCY,
						   V4L2_CID_POWER_LINE_FREQUENCY_60HZ, ~0x7,
						   V4L2_CID_POWER_LINE_FREQUENCY_50HZ);

	v4l2_ctrl_new_std(&gc032a->ctrls, &gc032a_ctrl_ops,
				V4L2_CID_BRIGHTNESS, 0, 9, 1, 0);

	gc032a->sd.ctrl_handler = &gc032a->ctrls;

	if (gc032a->ctrls.error) {
		dev_err(&client->dev, "%s: control initialization error %d\n",
			__func__, gc032a->ctrls.error);
		return  gc032a->ctrls.error;
	}

	sd = &gc032a->sd;
	//client->flags |= I2C_CLIENT_SCCB;
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
	v4l2_i2c_subdev_init(sd, client, &gc032a_subdev_ops);

	sd->internal_ops = &gc032a_subdev_internal_ops;
	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
		     V4L2_SUBDEV_FL_HAS_EVENTS;
#endif

#if defined(CONFIG_MEDIA_CONTROLLER)
	gc032a->pad.flags = MEDIA_PAD_FL_SOURCE;
	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
	ret = media_entity_pads_init(&sd->entity, 1, &gc032a->pad);
	if (ret < 0) {
		v4l2_ctrl_handler_free(&gc032a->ctrls);
		return ret;
	}
#endif

	mutex_init(&gc032a->lock);

	gc032a_get_default_format(&gc032a->format);
	gc032a->frame_size = &gc032a_framesizes[0];

	ret = gc032a_detect(gc032a);
	if (ret < 0)
		goto error;
	ret = gc032a_write_array(client, gc032a->frame_size->regs);
	if (ret)
		dev_err(&client->dev, "init error\n");
	snprintf(sd->name, sizeof(sd->name), "m_%s %s",
		 DRIVER_NAME, dev_name(sd->dev));

	ret = v4l2_async_register_subdev_sensor_common(sd);
	if (ret)
		goto error;

	dev_info(&client->dev, "%s sensor driver registered !!\n", sd->name);
	return 0;

error:
	v4l2_ctrl_handler_free(&gc032a->ctrls);
#if defined(CONFIG_MEDIA_CONTROLLER)
	media_entity_cleanup(&sd->entity);
#endif
	mutex_destroy(&gc032a->lock);
	__gc032a_power_off(gc032a);
	return ret;
}

static int gc032a_remove(struct i2c_client *client)
{
	struct v4l2_subdev *sd = i2c_get_clientdata(client);
	struct gc032a *gc032a = to_gc032a(sd);

	dev_dbg(&client->dev, "gc032a_remove...\n");

	v4l2_ctrl_handler_free(&gc032a->ctrls);
	v4l2_async_unregister_subdev(sd);
#if defined(CONFIG_MEDIA_CONTROLLER)
	media_entity_cleanup(&sd->entity);
#endif
	mutex_destroy(&gc032a->lock);

	__gc032a_power_off(gc032a);

	return 0;
}

static const struct i2c_device_id gc032a_id[] = {
	{ "gc032a", 0 },
	{ /* sentinel */ },
};
MODULE_DEVICE_TABLE(i2c, gc032a_id);

#if IS_ENABLED(CONFIG_OF)
static const struct of_device_id gc032a_of_match[] = {
	{ .compatible = "galaxycore,gc032a", },
	{ /* sentinel */ },
};
MODULE_DEVICE_TABLE(of, gc032a_of_match);
#endif

static struct i2c_driver gc032a_i2c_driver = {
	.driver = {
		.name	= DRIVER_NAME,
		.of_match_table = of_match_ptr(gc032a_of_match),
	},
	.probe		= gc032a_probe,
	.remove		= gc032a_remove,
	.id_table	= gc032a_id,
};

static int __init sensor_mod_init(void)
{
	return i2c_add_driver(&gc032a_i2c_driver);
}

static void __exit sensor_mod_exit(void)
{
	i2c_del_driver(&gc032a_i2c_driver);
}

device_initcall_sync(sensor_mod_init);
module_exit(sensor_mod_exit);
//module_i2c_driver(gc032a_i2c_driver);

MODULE_AUTHOR("ASR Inc.");
MODULE_DESCRIPTION("GC032A CMOS Image Sensor driver");
MODULE_LICENSE("GPL v2");