static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator)
{
struct mt9v011 *core = to_mt9v011(sd);
unsigned height, width, hblank, vblank, speed;
unsigned row_time, t_time;
u64 frames_per_ms;
unsigned tmp;
height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
width = mt9v011_read(sd, R04_MT9V011_WIDTH);
hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
speed = mt9v011_read(sd, R0A_MT9V011_CLK_SPEED);
row_time = (width + 113 + hblank) * (speed + 2);
t_time = row_time * (height + vblank + 1);
frames_per_ms = core->xtal * 1000l;
do_div(frames_per_ms, t_time);
tmp = frames_per_ms;
v4l2_dbg(1, debug, sd, "Programmed to %u.%03u fps (%d pixel clcks)\n",
tmp / 1000, tmp % 1000, t_time);
if (numerator && denominator) {
*numerator = 1000;
*denominator = (u32)frames_per_ms;
}
}
static int mt9v011_g_chip_ident(struct v4l2_subdev *sd,
struct v4l2_dbg_chip_ident *chip)
{
u16 version;
struct i2c_client *client = v4l2_get_subdevdata(sd);
version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_MT9V011,
version);
}
static u16 calc_speed(struct v4l2_subdev *sd, u32 numerator, u32 denominator)
{
struct mt9v011 *core = to_mt9v011(sd);
unsigned height, width, hblank, vblank;
unsigned row_time, line_time;
u64 t_time, speed;
/* Avoid bogus calculus */
if (!numerator || !denominator)
return 0;
height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
width = mt9v011_read(sd, R04_MT9V011_WIDTH);
hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
row_time = width + 113 + hblank;
line_time = height + vblank + 1;
t_time = core->xtal * ((u64)numerator);
/* round to the closest value */
t_time += denominator / 2;
do_div(t_time, denominator);
speed = t_time;
do_div(speed, row_time * line_time);
/* Avoid having a negative value for speed */
if (speed < 2)
speed = 0;
else
speed -= 2;
/* Avoid speed overflow */
if (speed > 15)
return 15;
return (u16)speed;
}
static u16 calc_speed(struct v4l2_subdev *sd, u32 numerator, u32 denominator)
{
struct mt9v011 *core = to_mt9v011(sd);
unsigned height, width, hblank, vblank;
unsigned row_time, line_time;
u64 t_time, speed;
if (!numerator || !denominator)
return 0;
height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
width = mt9v011_read(sd, R04_MT9V011_WIDTH);
hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
row_time = width + 113 + hblank;
line_time = height + vblank + 1;
t_time = core->xtal * ((u64)numerator);
t_time += denominator / 2;
do_div(t_time, denominator);
speed = t_time;
do_div(speed, row_time * line_time);
if (speed < 2)
speed = 0;
else
speed -= 2;
if (speed > 15)
return 15;
return (u16)speed;
}
static int mt9v011_g_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;
reg->val = mt9v011_read(sd, reg->reg & 0xff);
reg->size = 2;
return 0;
}
static int mt9v011_probe(struct i2c_client *c,
const struct i2c_device_id *id)
{
u16 version;
struct mt9v011 *core;
struct v4l2_subdev *sd;
/* Check if the adapter supports the needed features */
if (!i2c_check_functionality(c->adapter,
I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
return -EIO;
core = kzalloc(sizeof(struct mt9v011), GFP_KERNEL);
if (!core)
return -ENOMEM;
sd = &core->sd;
v4l2_i2c_subdev_init(sd, c, &mt9v011_ops);
/* Check if the sensor is really a MT9V011 */
version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
if ((version != MT9V011_VERSION) &&
(version != MT9V011_REV_B_VERSION)) {
v4l2_info(sd, "*** unknown micron chip detected (0x%04x).\n",
version);
kfree(core);
return -EINVAL;
}
core->global_gain = 0x0024;
core->exposure = 0x01fc;
core->width = 640;
core->height = 480;
core->xtal = 27000000; /* Hz */
if (c->dev.platform_data) {
struct mt9v011_platform_data *pdata = c->dev.platform_data;
core->xtal = pdata->xtal;
v4l2_dbg(1, debug, sd, "xtal set to %d.%03d MHz\n",
core->xtal / 1000000, (core->xtal / 1000) % 1000);
}
v4l_info(c, "chip found @ 0x%02x (%s - chip version 0x%04x)\n",
c->addr << 1, c->adapter->name, version);
return 0;
}
static int mt9v011_probe(struct i2c_client *c,
const struct i2c_device_id *id)
{
u16 version;
struct mt9v011 *core;
struct v4l2_subdev *sd;
/* Check if the adapter supports the needed features */
if (!i2c_check_functionality(c->adapter,
I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
return -EIO;
core = kzalloc(sizeof(struct mt9v011), GFP_KERNEL);
if (!core)
return -ENOMEM;
sd = &core->sd;
v4l2_i2c_subdev_init(sd, c, &mt9v011_ops);
/* Check if the sensor is really a MT9V011 */
version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
if ((version != MT9V011_VERSION) &&
(version != MT9V011_REV_B_VERSION)) {
v4l2_info(sd, "*** unknown micron chip detected (0x%04x).\n",
version);
kfree(core);
return -EINVAL;
}
v4l2_ctrl_handler_init(&core->ctrls, 5);
v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
V4L2_CID_GAIN, 0, (1 << 12) - 1 - 0x20, 1, 0x20);
v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
V4L2_CID_EXPOSURE, 0, 2047, 1, 0x01fc);
v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
V4L2_CID_RED_BALANCE, -(1 << 9), (1 << 9) - 1, 1, 0);
v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
V4L2_CID_BLUE_BALANCE, -(1 << 9), (1 << 9) - 1, 1, 0);
v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
V4L2_CID_HFLIP, 0, 1, 1, 0);
v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
V4L2_CID_VFLIP, 0, 1, 1, 0);
if (core->ctrls.error) {
int ret = core->ctrls.error;
v4l2_err(sd, "control initialization error %d\n", ret);
v4l2_ctrl_handler_free(&core->ctrls);
kfree(core);
return ret;
}
core->sd.ctrl_handler = &core->ctrls;
core->global_gain = 0x0024;
core->exposure = 0x01fc;
core->width = 640;
core->height = 480;
core->xtal = 27000000; /* Hz */
if (c->dev.platform_data) {
struct mt9v011_platform_data *pdata = c->dev.platform_data;
core->xtal = pdata->xtal;
v4l2_dbg(1, debug, sd, "xtal set to %d.%03d MHz\n",
core->xtal / 1000000, (core->xtal / 1000) % 1000);
}
v4l_info(c, "chip found @ 0x%02x (%s - chip version 0x%04x)\n",
c->addr << 1, c->adapter->name, version);
return 0;
}
