static void set_balance(struct v4l2_subdev *sd)
{
struct mt9v011 *core = to_mt9v011(sd);
u16 green_gain, blue_gain, red_gain;
u16 exposure;
s16 bal;
exposure = core->exposure;
green_gain = calc_mt9v011_gain(core->global_gain);
bal = core->global_gain;
bal += (core->blue_bal * core->global_gain / (1 << 7));
blue_gain = calc_mt9v011_gain(bal);
bal = core->global_gain;
bal += (core->red_bal * core->global_gain / (1 << 7));
red_gain = calc_mt9v011_gain(bal);
mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green_gain);
mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN, green_gain);
mt9v011_write(sd, R2C_MT9V011_BLUE_GAIN, blue_gain);
mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain);
mt9v011_write(sd, R09_MT9V011_SHUTTER_WIDTH, exposure);
}
static void set_balance(struct v4l2_subdev *sd)
{
struct mt9v011 *core = to_mt9v011(sd);
u16 green1_gain, green2_gain, blue_gain, red_gain;
green1_gain = core->global_gain;
green2_gain = core->global_gain;
blue_gain = core->global_gain +
core->global_gain * core->blue_bal / (1 << 9);
red_gain = core->global_gain +
core->global_gain * core->blue_bal / (1 << 9);
mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green1_gain);
mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN, green1_gain);
mt9v011_write(sd, R2C_MT9V011_BLUE_GAIN, blue_gain);
mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain);
}
static void set_res(struct v4l2_subdev *sd)
{
struct mt9v011 *core = to_mt9v011(sd);
unsigned vstart, hstart;
/*
* The mt9v011 doesn't have scaling. So, in order to select the desired
* resolution, we're cropping at the middle of the sensor.
* hblank and vblank should be adjusted, in order to warrant that
* we'll preserve the line timings for 30 fps, no matter what resolution
* is selected.
* NOTE: datasheet says that width (and height) should be filled with
* width-1. However, this doesn't work, since one pixel per line will
* be missing.
*/
hstart = 20 + (640 - core->width) / 2;
mt9v011_write(sd, R02_MT9V011_COLSTART, hstart);
mt9v011_write(sd, R04_MT9V011_WIDTH, core->width);
mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width);
vstart = 8 + (480 - core->height) / 2;
mt9v011_write(sd, R01_MT9V011_ROWSTART, vstart);
mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height);
mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height);
calc_fps(sd, NULL, NULL);
};
static void set_read_mode(struct v4l2_subdev *sd)
{
struct mt9v011 *core = to_mt9v011(sd);
unsigned mode = 0x1000;
if (core->hflip)
mode |= 0x4000;
if (core->vflip)
mode |= 0x8000;
mt9v011_write(sd, R20_MT9V011_READ_MODE, mode);
}
static int mt9v011_s_register(struct v4l2_subdev *sd,
struct v4l2_dbg_register *reg)
{
struct i2c_client *client = v4l2_get_subdevdata(sd);
if (!v4l2_chip_match_i2c_client(client, ®->match))
return -EINVAL;
if (!capable(CAP_SYS_ADMIN))
return -EPERM;
mt9v011_write(sd, reg->reg & 0xff, reg->val & 0xffff);
return 0;
}
static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
{
int i;
for (i = 0; i < ARRAY_SIZE(mt9v011_init_default); i++)
mt9v011_write(sd, mt9v011_init_default[i].reg,
mt9v011_init_default[i].value);
set_balance(sd);
set_res(sd);
set_read_mode(sd);
return 0;
};
static int mt9v011_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
{
struct v4l2_captureparm *cp = &parms->parm.capture;
struct v4l2_fract *tpf = &cp->timeperframe;
u16 speed;
if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
return -EINVAL;
if (cp->extendedmode != 0)
return -EINVAL;
speed = calc_speed(sd, tpf->numerator, tpf->denominator);
mt9v011_write(sd, R0A_MT9V011_CLK_SPEED, speed);
v4l2_dbg(1, debug, sd, "Setting speed to %d\n", speed);
/* Recalculate and update fps info */
calc_fps(sd, &tpf->numerator, &tpf->denominator);
return 0;
}
static void set_res(struct v4l2_subdev *sd)
{
struct mt9v011 *core = to_mt9v011(sd);
unsigned vstart, hstart;
hstart = 20 + (640 - core->width) / 2;
mt9v011_write(sd, R02_MT9V011_COLSTART, hstart);
mt9v011_write(sd, R04_MT9V011_WIDTH, core->width);
mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width);
vstart = 8 + (480 - core->height) / 2;
mt9v011_write(sd, R01_MT9V011_ROWSTART, vstart);
mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height);
mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height);
calc_fps(sd, NULL, NULL);
};