WhyCan Forum(哇酷开发者社区)

/*
 * A V4L2 driver for gc2023_mipi_2lane Raw cameras.
 *
 */
#include "rn6752V1_configuration.h"





MODULE_AUTHOR("lth");
MODULE_DESCRIPTION("A low-level driver for rm6752V1_mipi_2lane Raw sensors");
MODULE_LICENSE("GPL");

//for internel driver debug
#define DEV_DBG_EN      1
#if(DEV_DBG_EN == 1)    
#define vfe_dev_dbg(x,arg...) printk("[rm6752V1_mipi_2lane Raw]"x,##arg)
#else
#define vfe_dev_dbg(x,arg...) 
#endif

#define vfe_dev_err(x,arg...) printk("[rm6752V1_mipi_2lane Raw]"x,##arg)
#define vfe_dev_print(x,arg...) printk("[rm6752V1_mipi_2lane Raw]"x,##arg)

#define LOG_ERR_RET(x)  { \
                          int ret;  \
                          ret = x; \
                          if(ret < 0) {\
                            vfe_dev_err("error at %s\n",__func__);  \
                            return ret; \
                          } \
                        }

						
						
						
						
//define module timing
#define MCLK              (24*1000*1000)
#define VREF_POL          V4L2_MBUS_VSYNC_ACTIVE_HIGH
#define HREF_POL          V4L2_MBUS_HSYNC_ACTIVE_HIGH
#define CLK_POL           V4L2_MBUS_PCLK_SAMPLE_RISING
#define V4L2_IDENT_SENSOR  0x6752

/*
 *Our nominal (default) frame rate.
 */

#define SENSOR_FRAME_RATE 60


#define  I2C_ADDR 0x2c
#define  SENSOR_NAME "rn6752V1_mipi_2lane"



//static struct delayed_work sensor_s_ae_ratio_work;
static struct v4l2_subdev *glb_sd;

/*
 * Information we maintain about a known sensor.
 */
struct sensor_format_struct;  /* coming later */

struct cfg_array { /* coming later */
	struct regval_list * regs;
	int size; 
};

static inline struct sensor_info *to_state(struct v4l2_subdev *sd)
{
    return container_of(sd, struct sensor_info, sd);
}

static struct regval_list uyvy[] = {
    //{REG_TERM,VAL_TERM},
};

/*
 * Store information about the video data format. 
 */
static struct sensor_format_struct {
    __u8 *desc;
    //__u32 pixelformat;
    enum v4l2_mbus_pixelcode mbus_code;
    struct regval_list *regs;
    int regs_size;
    int bpp;   /* Bytes per pixel */
}sensor_formats[] = {
	{
		.desc   = "UYVY 4:2:2",
		.mbus_code  = V4L2_MBUS_FMT_UYVY8_2X8,
		.regs 		= uyvy,
		.regs_size  = ARRAY_SIZE(uyvy),
		.bpp		= 1
	},
}; 
 
#define N_FMTS ARRAY_SIZE(sensor_formats)

static struct regval_list sensor_720p30_regs[] = {
    //{REG_TERM,VAL_TERM},
};

/*
 * Then there is the issue of window sizes.  Try to capture the info here.
 */
static struct sensor_win_size sensor_win_sizes[] = {

   /* 720P */
	{
		.width      = 1280,
		.height     = 720,
        .hoffset    = 0,
        .voffset    = 0,
        .hts        = 1920,
        .vts        = 750,
        .mipi_bps	= 324*1000*1000,
        .fps_fixed  = 25,
        .bin_factor = 1,
        .intg_min   = 1<<4,
        .intg_max   = 750<<4,//
        .gain_min   = 1<<4,
        .gain_max   = 64<<4,
		.regs       = sensor_720p30_regs,
        .regs_size  = ARRAY_SIZE(sensor_720p30_regs),
        .set_size   = NULL,
	},
};

/*
 * Low-level register I/O.
 *
 */
static int sensor_read(struct v4l2_subdev *sd, unsigned char reg,
    unsigned char *value) //!!!!be careful of the para type!!!
{
	int ret=0;
	int cnt=0;
	 
  ret = cci_read_a8_d8(sd,reg,value);
  vfe_dev_dbg("cci_read_a8_d81 addr = 0x%2x, value = 0x%2x \n",reg,value);
  while(ret!=0&&cnt<2)
  {
  	ret = cci_read_a8_d8(sd,reg,value);
  	cnt++;
  }
  if(cnt>0)
  	vfe_dev_dbg("sensor read retry=%d\n",cnt);
  
  return ret;
}

static int sensor_write(struct v4l2_subdev *sd, unsigned char reg,
    unsigned char value)
{
	int ret=0;
	int cnt=0;
	
  ret = cci_write_a8_d8(sd,reg,value);
  while(ret!=0&&cnt<2)
  {
  	ret = cci_write_a8_d8(sd,reg,value);
  	cnt++;
  }
  if(cnt>0)
  	vfe_dev_dbg("sensor write retry=%d\n",cnt);
  
  return ret;
}

/*
 * Write a list of register settings;
 */
static int sensor_write_array(struct v4l2_subdev *sd, struct regval_list *regs, int array_size)
{
    int i=0;

    if(!regs)
    	return -EINVAL;

    while(i<array_size)
    {
        if(regs->addr == REG_DLY) {
            msleep(regs->data);
        } 
		else {
			LOG_ERR_RET(sensor_write(sd, regs->addr, regs->data))
        }
        i++;
        regs++;
    }
    return 0;
}

/*
 * Stuff that knows about the sensor.
 */
 
static int sensor_power(struct v4l2_subdev *sd, int on)
{
  vfe_dev_dbg("sensor_power!\n");
  int ret;
  ret = 0;
  switch(on)
  {
    case CSI_SUBDEV_STBY_ON:
      vfe_dev_dbg("CSI_SUBDEV_STBY_ON!\n");
      cci_lock(sd);
      vfe_gpio_write(sd,PWDN,0);
	  usleep_range(10000,12000);
      cci_unlock(sd);
      break;
    case CSI_SUBDEV_STBY_OFF:
      vfe_dev_dbg("CSI_SUBDEV_STBY_OFF!\n");
	  cci_lock(sd);
      vfe_gpio_write(sd,PWDN,1);
	  usleep_range(10000,12000);
	   cci_unlock(sd);
      break;
	  
    case CSI_SUBDEV_PWR_ON:
       vfe_dev_dbg("CSI_SUBDEV_PWR_ON!\n");
       cci_lock(sd);
	   vfe_gpio_set_status(sd,PWDN,1);
	   vfe_gpio_set_status(sd,RESET,1);
	   
	   vfe_gpio_write(sd,RESET,1);
	   usleep_range(10000,10000);
	   vfe_gpio_write(sd,PWDN,1);
	   
	   usleep_range(50000,50000);
	   vfe_gpio_write(sd,RESET,0);	  
	   usleep_range(20000,20000);
	   vfe_gpio_write(sd,RESET,1);
	   usleep_range(200000,300000);
       cci_unlock(sd);
      break;
    case CSI_SUBDEV_PWR_OFF:
      vfe_dev_dbg("CSI_SUBDEV_PWR_OFF!\n");
      cci_lock(sd);
      vfe_gpio_write(sd,PWDN,0);
	  vfe_gpio_write(sd,RESET,1);	   
      cci_unlock(sd);
      break;
    default:
      return -EINVAL;
  }

	return 0;
}
 
static int sensor_reset(struct v4l2_subdev *sd, u32 val)
{
    switch(val)
    {
        case 0:
            vfe_gpio_write(sd,RESET,1);
            usleep_range(10000,12000);
            break;
        case 1:
            vfe_gpio_write(sd,RESET,0);
            usleep_range(10000,12000);
            break;
        default:
            return -EINVAL;
    }
    return 0;
}

static int sensor_detect(struct v4l2_subdev *sd)
{
  unsigned char rdval;
  
  LOG_ERR_RET(sensor_read(sd, 0xfd, &rdval))
  vfe_dev_dbg("gc2023_mipi_2lane ID_VAL_HIGH = %2x, Done!\n", rdval);
  if(rdval != 0x01)
    return -ENODEV;
 

  LOG_ERR_RET(sensor_read(sd, 0xfe, &rdval))
  vfe_dev_dbg("gc2023_mipi_2lane ID_VAL_LOW = %2x, Done!\n", rdval);
  if(rdval != 0x26)
    return -ENODEV;
  

  return 0;
}

#if 1

#define RN6752V1_HDR_CONFIGURATION
static struct regval_list RN675x_init_cfg[] = {
	// 720P@25 with mipi 2 data lanes + 1 clock lane out
	// pin24/23 data lane0}, pin18/17 data lane1
	// pin16/15 data lane2}, pin12/11 data lane3
	// pin14/13 clock lane
	// Slave address is {0x58
	// Register}, data

	// if clock source(Xin) of RN6752 is 26MHz}, please add these procedures marked first
	//{0xD2,0x85}, // disable auto clock detect
	//{0xD6,0x37}, // 27MHz default
	//{0xD8,0x18}, // switch to 26MHz clock
	//delay(100)}, // delay 100ms

	{0x81,0x01}, // turn on video decoder
	{0xDF,0xFE}, // enable HD format

	{0x88,0x40}, // disable SCLK0B out
	{0xF6,0x40}, // disable SCLK3A out

	// ch0
	{0xFF,0x00}, // switch to ch0 (default; optional)
	{0x00,0x20}, // internal use*
	{0x06,0x08}, // internal use*
	{0x07,0x63}, // HD format
	{0x2A,0x01}, // filter control
	{0x3A,0x20}, // Insert Channel ID in SAV/EAV code
	{0x3F,0x10}, // channel ID
	{0x4C,0x37}, // equalizer
	{0x4F,0x03}, // sync control
	{0x50,0x02}, // 720p resolution
	{0x56,0x01}, // BT 72M mode
	{0x5F,0x40}, // blank level
	{0x63,0xF5}, // filter control
	{0x59,0x00}, // extended register access
	{0x5A,0x42}, // data for extended register
	{0x58,0x01}, // enable extended register write
	{0x59,0x33}, // extended register access
	{0x5A,0x23}, // data for extended register
	{0x58,0x01}, // enable extended register write
	{0x51,0xE1}, // scale factor1
	{0x52,0x88}, // scale factor2
	{0x53,0x12}, // scale factor3
	{0x5B,0x07}, // H-scaling control
	{0x5E,0x08}, // enable H-scaling control
	{0x6A,0x82}, // H-scaling control
	{0x28,0x92}, // cropping
	{0x03,0x80}, // saturation
	{0x04,0x80}, // hue
	{0x05,0x00}, // sharpness
	{0x57,0x23}, // black/white stretch
	{0x68,0x32}, // coring
	{0x37,0x33},
	{0x61,0x6C},

	#ifdef RN6752V1_HDR_CONFIGURATION
	{0x1D,0xFF},
	{0x28,0x90},
	{0x2D,0xFA},
	{0x4E,0x01},
	{0x67,0x66},
	{0x0B,0x80},
	{0x09,0x00},
	{0x0D,0x18},
	{0x49,0x84},
	{0x57,0x63},
	{0x59,0x20},
	{0x5A,0x6C},
	{0x58,0x01},
	#endif

	// mipi link1
	{0xFF,0x09}, // switch to mipi tx1
	{0x00,0x03}, // enable bias
	{0xFF,0x08}, // switch to mipi csi1
	{0x04,0x03}, // csi1 and tx1 reset
	{0x6C,0x11}, // disable ch output; turn on ch0
	{0x06,0x4C}, // 2 lanes
	{0x21,0x01}, // enable hs clock
	{0x78,0x80}, // Y/C counts for ch0
	{0x79,0x02}, // Y/C counts for ch0
	{0x6C,0x01}, // enable ch output
	{0x04,0x00}, // csi1 and tx1 reset finish
	{0x07,0x05}, //enable non-continuous clock
	//{0x20,0xAA}, // invert hs clock

	// mipi link3
	{0xFF,0x0A}, // switch to mipi csi3
	{0x6C,0x10}, // disable ch output; turn off ch0~3

	// switch bank
	{0xFF,0x00}, //to bank0
};

int RN675xM_Pre_initial(struct v4l2_subdev *sd) 
{	
	int ret = 0;
	unsigned char rom_byte1, rom_byte2, rom_byte3, rom_byte4, rom_byte5, rom_byte6;
	sensor_write(sd,0xE1, 0x80);
	sensor_write(sd,0xFA, 0x81);
	sensor_read(sd, 0xFB, &rom_byte1);
	sensor_read(sd, 0xFB, &rom_byte2);
	sensor_read(sd, 0xFB, &rom_byte3);
	sensor_read(sd, 0xFB, &rom_byte4);
	sensor_read(sd, 0xFB, &rom_byte5);
	sensor_read(sd, 0xFB, &rom_byte6);

	if ((rom_byte6 == 0x00) && (rom_byte5 == 0x00)) {
		sensor_write(sd,0xEF, 0xAA);  
		sensor_write(sd,0xE7, 0xFF);
		sensor_write(sd,0xFF, 0x09);
		sensor_write(sd,0x03, 0x0C);
		sensor_write(sd,0xFF, 0x0B);
		sensor_write(sd,0x03, 0x0C);
	}
	else if (((rom_byte6 == 0x34) && (rom_byte5 == 0xA9)) ||
         ((rom_byte6 == 0x2C) && (rom_byte5 == 0xA8))) {
		sensor_write(sd,0xEF, 0xAA);  
		sensor_write(sd,0xE7, 0xFF);
		sensor_write(sd,0xFC, 0x60);
		sensor_write(sd,0xFF, 0x09);
		sensor_write(sd,0x03, 0x18);
		sensor_write(sd,0xFF, 0x0B);
		sensor_write(sd,0x03, 0x18);
	}
	else {
		sensor_write(sd,0xEF, 0xAA);  
		sensor_write(sd,0xFC, 0x60);
		sensor_write(sd,0xFF, 0x09);
		sensor_write(sd,0x03, 0x18);
		sensor_write(sd,0xFF, 0x0B);
		sensor_write(sd,0x03, 0x18);	
	}
	usleep_range(50000,60000);
	sensor_win_sizes[0].width= 1280;
	sensor_win_sizes[0].height = 720;	
	sensor_win_sizes[0].fps_fixed=25;
	sensor_win_sizes[0].hts=1920;
	sensor_win_sizes[0].vts=750;
	
	ret = sensor_write_array(sd, RN675x_init_cfg, ARRAY_SIZE(RN675x_init_cfg));  
    if(ret < 0) {
        vfe_dev_err("write sensor_default_regs error\n");
        return ret;
    }
	return 0;
	
}

#endif

static int sensor_init(struct v4l2_subdev *sd, u32 val)
{
    int ret;
	int conut = 10;
	unsigned char rdval;
    struct sensor_info *info = to_state(sd);
	struct regval_list *regs;
	int regsLen = 0;
	
    vfe_dev_dbg("sensor_init\n");
	

    /*Make sure it is a target sensor*/
	ret = sensor_detect(sd);
    if (ret) {
        vfe_dev_err("chip found is not an target chip.\n");
		return ret;
    }
    vfe_get_standby_mode(sd,&info->stby_mode);

    if((info->stby_mode == HW_STBY || info->stby_mode == SW_STBY) \
            && info->init_first_flag == 0) {
        vfe_dev_print("stby_mode and init_first_flag = 0\n");
        return 0;
    } 
	
#if 0
	vfe_dev_dbg("sensor_init11\n");
	while(conut--){
		usleep_range(100000,120000);
		ret = sensor_read(sd, 0x00, &rdval);
		if(ret) {
			vfe_dev_err("sensor_read error\n");
		}
		if(!(rdval & (1<< 4))) {
			break;
		}
	}
	vfe_dev_dbg("sensor info %02x\n",rdval);

	if(rdval & (1<< 4)) {
		// 无信息按照P制处理
		vfe_dev_err("sensor no info\n");
		regs = cvbs_pal_video;
		regsLen = ARRAY_SIZE(cvbs_pal_video);
		sensor_win_sizes[0].fps_fixed=50;
		sensor_win_sizes[0].width= 720;
		sensor_win_sizes[0].height = 576;
	
	} else {
		if(rdval & (1<< 5)){
			sensor_win_sizes[0].width= 1280;
			sensor_win_sizes[0].height = 720;	
			if(rdval & (1 << 0)){			
				vfe_dev_dbg("sensor info 720 30ps\n");
				regs = HD_720P30_video;
				regsLen = ARRAY_SIZE(HD_720P30_video);
				sensor_win_sizes[0].fps_fixed=30;
			} else {
				vfe_dev_dbg("sensor info 720 25ps\n");
				regs = HD_720P25_video;
				sensor_win_sizes[0].fps_fixed=25;
				regsLen = ARRAY_SIZE(HD_720P25_video);
			}	
		}			
		else if(rdval & (1<< 6)){
			sensor_win_sizes[0].width= 1920;
			sensor_win_sizes[0].height = 1080;
			if(rdval & (1 << 0)){
				vfe_dev_dbg("sensor info 1080 30ps\n");
				regs = FHD_1080P30_video;
				sensor_win_sizes[0].fps_fixed=30;
				regsLen = ARRAY_SIZE(HD_720P30_video);
			} else {
				vfe_dev_dbg("sensor info 1080 25ps\n");
				regs = FHD_1080P25_video;
				sensor_win_sizes[0].fps_fixed=25;
				regsLen = ARRAY_SIZE(HD_720P30_video);
			}
		} else {
			if(rdval & (1 << 0)){
				vfe_dev_dbg("sensor info D1 ntsc 60\n");
				sensor_win_sizes[0].width= 720;
				sensor_win_sizes[0].height = 480;
				sensor_win_sizes[0].fps_fixed=60;
				regs = cvbs_ntsc_video;
				regsLen = ARRAY_SIZE(cvbs_ntsc_video);
			} else {
				vfe_dev_dbg("sensor info D1 pal 50\n");
				sensor_win_sizes[0].width= 720;
				sensor_win_sizes[0].height = 576;
				sensor_win_sizes[0].fps_fixed=50;
				regs = cvbs_pal_video;
				regsLen = ARRAY_SIZE(cvbs_pal_video);			
			}
		}
	}	
	
	vfe_dev_dbg("sensor_init55\n");
    ret = sensor_write_array(sd, regs, regsLen);  
    if(ret < 0) {
        vfe_dev_err("write sensor_default_regs error\n");
        return ret;
    }
	
#else 
	
	ret = RN675xM_Pre_initial(sd);  
    if(ret < 0) {
        vfe_dev_err("write RN675xM_Pre_initial error\n");
        return ret;
    }	
#endif
	
		
	info->focus_status = 0;
    info->low_speed = 0;
    info->hflip = 0;
    info->vflip = 0;
    info->gain = 0;
	info->fmt = &sensor_formats[0];	
	info->width = sensor_win_sizes[0].width;
	info->height = sensor_win_sizes[0].height;
	
	
    info->tpf.numerator = 1;            
    info->tpf.denominator = sensor_win_sizes[0].fps_fixed;    /* 30fps */ 

    if(info->stby_mode == 0)
		info->init_first_flag = 0;
    info->preview_first_flag = 1;
	vfe_dev_dbg("sensor_init ok\n");
    return 0;
}

static long sensor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
{
int ret=0;
    struct sensor_info *info = to_state(sd);
    switch(cmd) {
    case GET_CURRENT_WIN_CFG:
        if(info->current_wins != NULL)
        {
            memcpy( arg,info->current_wins,sizeof(struct sensor_win_size) );
            ret=0;
        }
        else
        {
            vfe_dev_err("empty wins!\n");
            ret=-1;
        }
        break;
    case SET_FPS:
        ret=0;
        break;
    default:
        return -EINVAL;
    }
    return ret;
}




#define N_WIN_SIZES (ARRAY_SIZE(sensor_win_sizes))

static int sensor_enum_fmt(struct v4l2_subdev *sd, unsigned index,
                 enum v4l2_mbus_pixelcode *code)
{
	vfe_dev_err("sensor_enum_fmt:%d\n",index);
    if (index >= N_FMTS)
        return -EINVAL;

    *code = sensor_formats[index].mbus_code;
    return 0;
}

static int sensor_enum_size(struct v4l2_subdev *sd,
                            struct v4l2_frmsizeenum *fsize)
{
    if(fsize->index > N_WIN_SIZES-1)
    	return -EINVAL;
	vfe_dev_err("sensor_enum_size\n");
    fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
    fsize->discrete.width = sensor_win_sizes[fsize->index].width;
    fsize->discrete.height = sensor_win_sizes[fsize->index].height;
  
    return 0;
}
 
static int sensor_try_fmt_internal(struct v4l2_subdev *sd,
    struct v4l2_mbus_framefmt *fmt,
    struct sensor_format_struct **ret_fmt,
    struct sensor_win_size **ret_wsize)
{
    int index;
    struct sensor_win_size *wsize;
    struct sensor_info *info = to_state(sd);
	vfe_dev_err("sensor_try_fmt_internal\n");
	
    for (index = 0; index < N_FMTS; index++)
        if (sensor_formats[index].mbus_code == fmt->code)
            break;
 
	vfe_dev_err("sensor_try_fmt_internal22:%d:%d\n",sensor_formats[0].mbus_code,fmt->code);
	vfe_dev_err("sensor_try_fmt_internal33:%d:%d\n",fmt->width,fmt->height);
	vfe_dev_err("sensor_try_fmt_internal44:%d\n",info->tpf.denominator);
	 
    if (index >= N_FMTS) 
	{
		vfe_dev_err("index >= N_FMTS \n");
        return -EINVAL;
	}
   
    if (ret_fmt != NULL)
        *ret_fmt = sensor_formats + index;
    
    /*
    * Fields: the sensor devices claim to be progressive.
    */
  
    fmt->field = V4L2_FIELD_NONE;
  
    /*
    * Round requested image size down to the nearest
    * we support, but not below the smallest.
    */
    for (wsize = sensor_win_sizes; wsize < sensor_win_sizes + N_WIN_SIZES;
       wsize++)
    if (fmt->width >= wsize->width && fmt->height >= wsize->height && info->tpf.denominator == wsize->fps_fixed)
            break;
    
    if (wsize >= sensor_win_sizes + N_WIN_SIZES)
        wsize--;   /* Take the smallest one */
    if (ret_wsize != NULL)
        *ret_wsize = wsize;
	
    /*
    * Note the size we'll actually handle.
    */
    fmt->width = wsize->width;
    fmt->height = wsize->height;
    info->current_wins = wsize;

    return 0;
}
/* 
 * Code for dealing with controls.
 * fill with different sensor module
 * different sensor module has different settings here
 * if not support the follow function ,retrun -EINVAL
 */
static int sensor_g_exp(struct v4l2_subdev *sd, __s32 *value)
{
	struct sensor_info *info = to_state(sd);
	
	*value = info->exp;
	vfe_dev_dbg("sensor_get_exposure = %d\n", info->exp);
	return 0;
}

static int sensor_s_exp(struct v4l2_subdev *sd, unsigned int exp_val)
{
    struct sensor_info *info = to_state(sd);

    vfe_dev_dbg("sensor_set_exposure = %d\n", exp_val);
    if(exp_val>0xffffff)
        exp_val=0xfffff0;
    if(exp_val<16)
        exp_val=16;
  
    exp_val=(exp_val)>>4;//rounding to 1
    info->exp = exp_val;
    return 0;
}

static int sensor_g_gain(struct v4l2_subdev *sd, __s32 *value)
{
    struct sensor_info *info = to_state(sd);
	
    *value = info->gain;
    vfe_dev_dbg("sensor_get_gain = %d\n", info->gain);
    return 0;
}

static int sensor_s_gain(struct v4l2_subdev *sd, int gain_val)
{
    struct sensor_info *info = to_state(sd);
	unsigned short dig_gain = 0x80;	// 1 times digital gain
    info->gain = gain_val;
    return 0;
}
static int sensor_try_fmt(struct v4l2_subdev *sd, 
             struct v4l2_mbus_framefmt *fmt)
{
	vfe_dev_err("sensor_try_fmt\n");
    return sensor_try_fmt_internal(sd, fmt, NULL, NULL);
}

static int sensor_g_mbus_config(struct v4l2_subdev *sd,
           struct v4l2_mbus_config *cfg)
{
	vfe_dev_err("sensor_g_mbus_config\n");
    cfg->type = V4L2_MBUS_CSI2;
	cfg->flags = 0|V4L2_MBUS_CSI2_2_LANE|V4L2_MBUS_CSI2_CHANNEL_0;
    return 0;
}

/*
 * Set a format.
 */
static int sensor_s_fmt(struct v4l2_subdev *sd, 
             struct v4l2_mbus_framefmt *fmt)
{
    int ret;
    struct sensor_format_struct *sensor_fmt;
    struct sensor_win_size *wsize;
    struct sensor_info *info = to_state(sd);

    vfe_dev_dbg("sensor_s_fmt\n");
    ret = sensor_try_fmt_internal(sd, fmt, &sensor_fmt, &wsize);
    if (ret)
        return ret;
    if(info->capture_mode == V4L2_MODE_VIDEO)
    {
		
    }
    else if(info->capture_mode == V4L2_MODE_IMAGE)
    {
		
    }
    LOG_ERR_RET(sensor_write_array(sd, sensor_fmt->regs, sensor_fmt->regs_size))
    ret = 0;
    if (wsize->regs)
        LOG_ERR_RET(sensor_write_array(sd, wsize->regs, wsize->regs_size))
    if (wsize->set_size)
        LOG_ERR_RET(wsize->set_size(sd))

    info->fmt = sensor_fmt;
    info->width = wsize->width;
    info->height = wsize->height;

    vfe_dev_print("s_fmt set width = %d, height = %d\n",wsize->width,wsize->height);
    if(info->capture_mode == V4L2_MODE_VIDEO)
    {
       
    } else {
        
    }
	printk("sensor_s_fmt: wsize.width = [%d], wsize.height = [%d]\n", wsize->width, wsize->height);		
    return 0;
}

/*
 * Implement G/S_PARM.  There is a "high quality" mode we could try
 * to do someday; for now, we just do the frame rate tweak.
 */
static int sensor_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
{
	vfe_dev_err("sensor_g_parm\n");
    struct v4l2_captureparm *cp = &parms->parm.capture;
    struct sensor_info *info = to_state(sd);

    if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
        return -EINVAL;
  
    memset(cp, 0, sizeof(struct v4l2_captureparm));
    cp->capability = V4L2_CAP_TIMEPERFRAME;
    cp->capturemode = info->capture_mode;
    return 0;
}

static int sensor_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
{
	vfe_dev_err("sensor_s_parm\n");
	  struct v4l2_captureparm *cp = &parms->parm.capture;
    //struct v4l2_fract *tpf = &cp->timeperframe;
    struct sensor_info *info = to_state(sd);
    //unsigned char div;
  
    vfe_dev_dbg("sensor_s_parm\n");
  
    if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
        return -EINVAL;
  
    if (info->tpf.numerator == 0)
        return -EINVAL;
    
    info->capture_mode = cp->capturemode;
  
    return 0;
}


static int sensor_queryctrl(struct v4l2_subdev *sd,
    struct v4l2_queryctrl *qc)
{
	vfe_dev_err("sensor_queryctrl\n");
	  switch (qc->id) {
    case V4L2_CID_GAIN:
    	return v4l2_ctrl_query_fill(qc, 1*16, 16*9-1, 1, 16);
    case V4L2_CID_EXPOSURE:
    	return v4l2_ctrl_query_fill(qc, 1, 65536*16, 1, 1);
    case V4L2_CID_FRAME_RATE:
    	return v4l2_ctrl_query_fill(qc, 15, 120, 1, 30);
    }
    return -EINVAL;
}

static int sensor_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
{
	vfe_dev_err("sensor_g_ctrl\n");
	switch (ctrl->id) {
    case V4L2_CID_GAIN:
    	return sensor_g_gain(sd, &ctrl->value);
    case V4L2_CID_EXPOSURE:
    	return sensor_g_exp(sd, &ctrl->value);
    }
    return -EINVAL;
    return 0;
}

static int sensor_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
{
	vfe_dev_err("sensor_s_ctrl\n");
	    struct v4l2_queryctrl qc;
    int ret;

    qc.id = ctrl->id;
    ret = sensor_queryctrl(sd, &qc);
    if (ret < 0) {
        return ret;
    }

    if (ctrl->value < qc.minimum || ctrl->value > qc.maximum) {
        return -ERANGE;
    }

    switch (ctrl->id) {
    case V4L2_CID_GAIN:
        return sensor_s_gain(sd, ctrl->value);      
    case V4L2_CID_EXPOSURE:
        return sensor_s_exp(sd, ctrl->value);
    }
    return -EINVAL;
    return 0;
}


static int sensor_g_chip_ident(struct v4l2_subdev *sd,
    struct v4l2_dbg_chip_ident *chip)
{
    struct i2c_client *client = v4l2_get_subdevdata(sd);

    return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_SENSOR, 0);
}


/* ----------------------------------------------------------------------- */

static const struct v4l2_subdev_core_ops sensor_core_ops = {
    .g_chip_ident = sensor_g_chip_ident,
    .g_ctrl = sensor_g_ctrl,
    .s_ctrl = sensor_s_ctrl,
    .queryctrl = sensor_queryctrl,
    .reset = sensor_reset,
    .init = sensor_init,
    .s_power = sensor_power,
    .ioctl = sensor_ioctl,
};

static const struct v4l2_subdev_video_ops sensor_video_ops = {
    .enum_mbus_fmt = sensor_enum_fmt,
    .enum_framesizes = sensor_enum_size,
    .try_mbus_fmt = sensor_try_fmt,
    .s_mbus_fmt = sensor_s_fmt,
    .s_parm = sensor_s_parm,
    .g_parm = sensor_g_parm,
    .g_mbus_config = sensor_g_mbus_config,
};


static const struct v4l2_subdev_ops sensor_ops = {
    .core = &sensor_core_ops,
    .video = &sensor_video_ops,
};

/* ----------------------------------------------------------------------- */
static struct cci_driver cci_drv = {
	.name = SENSOR_NAME,
	.addr_width = CCI_BITS_8,
	.data_width = CCI_BITS_8,
};

static int sensor_probe(struct i2c_client *client,
      const struct i2c_device_id *id)
{
    struct v4l2_subdev *sd;
    struct sensor_info *info;
	vfe_dev_dbg("sensor_probe\n");
    info = kzalloc(sizeof(struct sensor_info), GFP_KERNEL);
    if (info == NULL)
        return -ENOMEM;
    sd = &info->sd;
    glb_sd = sd;
    cci_dev_probe_helper(sd, client, &sensor_ops, &cci_drv);

    info->fmt = &sensor_formats[0];
    info->af_first_flag = 1;
    info->init_first_flag = 1;
    return 0;
}

static int sensor_remove(struct i2c_client *client)
{
    struct v4l2_subdev * sd;

    sd = cci_dev_remove_helper(client, &cci_drv);
    kfree(to_state(sd));
    return 0;
}

static const struct i2c_device_id sensor_id[] = {
    { SENSOR_NAME, 0 },
    { }
};
MODULE_DEVICE_TABLE(i2c, sensor_id);


static struct i2c_driver sensor_driver = {
    .driver = {
        .owner = THIS_MODULE,
        .name = SENSOR_NAME,
    },
    .probe = sensor_probe,
    .remove = sensor_remove,
    .id_table = sensor_id,
};
static __init int init_sensor(void)
{
	vfe_dev_dbg("init_sensor\n");
	return cci_dev_init_helper(&sensor_driver);
}

static __exit void exit_sensor(void)
{
	cci_dev_exit_helper(&sensor_driver);
}

module_init(init_sensor);
module_exit(exit_sensor);