*f)
+{
+ struct v4l2_pix_format *pix;
+ v4l2_std_id current_std;
+ int res;
+
+ if (f == NULL)
+ return -EINVAL;
+
+ if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ /* only capture is supported */
+ return -EINVAL;
+
+ pix = &f->fmt.pix;
+
+ /* Calculate height and width based on current standard */
+ res = tvp7002_get_video_mode(sd);
+ if (res < 0)
+ return -EINVAL;
+ current_std = res;
+
+ pix->width = NUM_ACTIVE_PIXELS(current_std);
+ pix->height = NUM_ACTIVE_LINES(current_std);
+
+ pix->pixelformat = V4L2_PIX_FMT_UYVY;
+
+ pix->field = V4L2_FIELD_INTERLACED;
+ pix->bytesperline = pix->width * 2;
+ pix->sizeimage = pix->bytesperline * pix->height;
+ pix->colorspace = V4L2_COLORSPACE_REC709;
+ pix->priv = 0;
+
+ v4l2_dbg(1, debug, sd, "Try FMT: pixelformat - %s, bytesperline -
%d"
+ "Width - %d, Height - %d",
+ "8-bit UYVY 4:2:2 Format", pix->bytesperline,
+ pix->width, pix->height);
+ return 0;
+}
+
+/**
+ * tvp7002_s_fmt_cap() - V4L2 decoder interface handler for s_fmt
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @f: pointer to standard V4L2 VIDIOC_S_FMT ioctl structure
+ *
+ * If the requested format is supported, configures the HW to use that
+ * format, returns error code if format not supported or HW can't be
+ * correctly configured.
+ */
+static int tvp7002_s_fmt_cap(struct v4l2_subdev *sd, struct v4l2_format
*f)
+{
+ struct tvp7002 *decoder = to_tvp7002(sd);
+ struct v4l2_pix_format *pix;
+ int rval;
+
+ if (f == NULL)
+ return -EINVAL;
+
+ if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ /* only capture is supported */
+ return -EINVAL;
+
+ pix = &f->fmt.pix;
+ rval = tvp7002_try_fmt_cap(sd, f);
+ if (rval)
+ return rval;
+
+ decoder->pix = *pix;
+
+ return rval;
+}
+
+/*
+ * tvp7002_g_fmt() - V4L2 decoder interface handler for tvp7002_g_fmt
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the decoder's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int tvp7002_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
+{
+ struct tvp7002 *decoder = to_tvp7002(sd);
+
+ if (f == NULL)
+ return -EINVAL;
+
+ if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ /* only capture is supported */
+ return -EINVAL;
+
+ f->fmt.pix = decoder->pix;
+
+ v4l2_dbg(1, debug, sd, "Current FMT: bytesperline - %d"
+ "Width - %d, Height - %d",
+ decoder->pix.bytesperline,
+ decoder->pix.width, decoder->pix.height);
+ return 0;
+}
+
+/*
+ * tvp7002_s_ctrl() - Set a control
+ * @sd: ptr to v4l2_subdev struct
+ * @ctrl: ptr to v4l2_control struct
+ *
+ * Set a control for a TVP7002 decoder device.
+ * Returns zero when successful or -EINVAL if register access fails.
+ */
+static int tvp7002_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+ struct tvp7002 *decoder = to_tvp7002(sd);
+ int vmd = 0;
+
+ decoder->video_mode = std;
+ vmd = tvp7002_from_std(std);
+
+ v4l2_dbg(1, debug, sd, "Set video std mode to %llx.\n", std);
+
+ return tvp7002_set_video_mode(sd, vmd);
+}
+
+/*
+ * tvp7002_g_ctrl() - Get a control
+ * @sd: ptr to v4l2_subdev struct
+ * @ctrl: ptr to v4l2_control struct
+ *
+ * Get a control for a TVP7002 decoder device.
+ * Returns zero when successful or -EINVAL if register access fails.
+ */
+static int tvp7002_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control
*ctrl)
+{
+ int rval, gval, bval;
+ int res;
+
+ v4l2_info(sd, "g_ctrl called\n");
+
+ switch (ctrl->id) {
+ case V4L2_CID_GAIN:
+ rval = tvp7002_read(sd, TVP7002_R_FINE_GAIN);
+ gval = tvp7002_read(sd, TVP7002_G_FINE_GAIN);
+ bval = tvp7002_read(sd, TVP7002_B_FINE_GAIN);
+
+ if (rval < 0 || gval < 0 || bval < 0) {
+ res = -1;
+ } else if (rval != gval || rval != bval) {
+ res = -1;
+ } else {
+ ctrl->value = rval & 0x0F;
+ res = ctrl->value;
+ }
+ break;
+ default:
+ res = -1;
+ break;
+ }
+
+ return res < 0 ? res : 0;
+}
+
+/*
+ * tvp7002_s_ctrl() - Set a control
+ * @sd: ptr to v4l2_subdev struct
+ * @ctrl: ptr to v4l2_control struct
+ *
+ * Set a control in TVP7002 decoder device.
+ * Returns zero when successful or -EINVAL if register access fails.
+ */
+static int tvp7002_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control
*ctrl)
+{
+ int rval, gval, bval;
+ int error;
+ u8 i, n;
+ n = ARRAY_SIZE(tvp7002_qctrl);
+
+ for (i = 0; i < n; i++)
+ if (ctrl->id == tvp7002_qctrl[i].id)
+ break;
+
+ if (i == n)
+ return -EINVAL;
+
+ if (ctrl->value < tvp7002_qctrl[i].minimum ||
+ ctrl->value > tvp7002_qctrl[i].maximum)
+ return -ERANGE;
+
+ switch (ctrl->id) {
+ case V4L2_CID_GAIN:
+ rval = tvp7002_write(sd, TVP7002_R_FINE_GAIN,
+ ctrl->value & 0xff);
+ gval = tvp7002_write(sd, TVP7002_G_FINE_GAIN,
+ ctrl->value & 0xff);
+ bval = tvp7002_write(sd, TVP7002_B_FINE_GAIN,
+ ctrl->value & 0xff);
+ if (rval < 0 || gval < 0 || bval < 0)
+ error = -1;
+ else
+ error = rval;
+ break;
+ default:
+ error = -1;
+ break;
+ }
+
+ return error < 0 ? -EINVAL : 0;
+}
+
+/*
+ * tvp7002_g_register() - Get the value of a register
+ * @sd: ptr to v4l2_subdev struct
+ * @vreg: ptr to v4l2_dbg_register struct
+ *
+ * Get the value of a TVP7002 decoder device register.
+ * Returns zero when successful, -EINVAL if register read fails or
+ * access to I2C client fails, -EPERM if the call is not allowed
+ * by diabled CAP_SYS_ADMIN.
+ */
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int tvp7002_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;
+
+ return reg->val < 0 ? -EINVAL : 0;
+}
+
+/*
+ * tvp7002_s_register() - set a control
+ * @sd: ptr to v4l2_subdev struct
+ * @ctrl: ptr to v4l2_control struct
+ *
+ * Get the value of a TVP7002 decoder device register.
+ * Returns zero when successful or -EINVAL if register read fails.
+ */
+static int tvp7002_s_register(struct v4l2_subdev *sd,
+ struct v4l2_dbg_register *reg)
+{
+ int wres;
+ 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;
+
+ wres = tvp7002_write(sd, reg->reg & 0xff, reg->val & 0xff);
+
+ return wres < 0 ? -EINVAL : 0;
+}
+#endif
+
+/*
+ * tvp7002_queryctrl() - Query a control
+ * @sd: ptr to v4l2_subdev struct
+ * @ctrl: ptr to v4l2_queryctrl struct
+ *
+ * Query a control of a TVP7002 decoder device.
+ * Returns zero when successful or -EINVAL if register read fails.
+ */
+static int tvp7002_queryctrl(struct v4l2_subdev *sd, struct
v4l2_queryctrl *qc)
+{
+ int i, error;
+ error = -EINVAL;
+
+ v4l2_info(sd, "queryctrl called\n");
+
+ for (i = 0; i < ARRAY_SIZE(tvp7002_qctrl); i++)
+ if (qc->id && qc->id == tvp7002_qctrl[i].id) {
+ v4l2_ctrl_query_fill(qc, tvp7002_qctrl[i].minimum,
+ tvp7002_qctrl[i].maximum,
+ tvp7002_qctrl[i].step,
+ tvp7002_qctrl[i].default_value);
+ error = 0;
+ break;
+ }
+
+ return error;
+}
+
+/*
+ * tvp7002_s_stream() - V4L2 decoder i/f handler for s_stream
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @enable: streaming enable or disable
+ *
+ * Sets streaming to enable or disable, if possible.
+ */
+static int tvp7002_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ int err = 0;
+ struct tvp7002 *decoder = to_tvp7002(sd);
+
+ if (decoder->streaming == enable)
+ return 0;
+
+ if (enable) {
+ /* Power Up Sequence */
+ err = tvp7002_write(sd, TVP7002_PWR_CTL, 0x00);
+ if (err) {
+ v4l2_err(sd, "Unable to turn on decoder\n");
+ err = -EINVAL;
+ }
+ err = tvp7002_write_inittab(sd, tvp7002_init_default);
+ if (err < 0) {
+ v4l2_err(sd, "Unable to initialize\n");
+ err = -EINVAL;
+ }
+ /* Detect if not already detected */
+ err = tvp7002_read(sd, TVP7002_CHIP_REV);
+ if (err < 0) {
+ v4l2_err(sd, "Unable to detect decoder\n");
+ err = -EINVAL;
+ }
+ decoder->streaming = enable;
+ } else {
+ /* Power Down Sequence */
+ err = tvp7002_write(sd, TVP7002_PWR_CTL, 0x40);
+ if (err) {
+ v4l2_err(sd, "Unable to turn off decoder\n");
+ return err;
+ }
+ decoder->streaming = enable;
+ }
+
+ return err;
+}
+
+/* Specific video subsystem operation handlers */
+static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
+ .querystd = tvp7002_querystd,
+ .s_stream = tvp7002_s_stream,
+ .g_fmt = tvp7002_g_fmt,
+};
+
+/* V4L2 Operations handlers */
+static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
+ .g_chip_ident = tvp7002_g_chip_ident,
+ .log_status = tvp7002_log_status,
+ .g_ctrl = tvp7002_g_ctrl,
+ .s_ctrl = tvp7002_s_ctrl,
+ .queryctrl = tvp7002_queryctrl,
+ .s_std = tvp7002_s_std,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+ .g_register = tvp7002_g_register,
+ .s_register = tvp7002_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops tvp7002_ops = {
+ .core = &tvp7002_core_ops,
+ .video = &tvp7002_video_ops,
+};
+
+/* Default driver settings */
+static struct tvp7002 tvp7002_dev = {
+ .streaming = 0,
+
+ .pix = {
+ .width = 720,
+ .height = 480,
+ .pixelformat = V4L2_PIX_FMT_UYVY,
+ .field = V4L2_FIELD_INTERLACED,
+ .bytesperline = 720 * 2,
+ .sizeimage = 720 * 2 * 480,
+ .colorspace = V4L2_COLORSPACE_SMPTE170M,
+ },
+
+ .video_mode = V4L2_STD_NTSC,
+};
+
+/*
+ * tvp7002_reset - Reset a TVP7002 device
+ * @sd: ptr to v4l2_subdev struct
+ * @val: unsigned integer (not used)
+ *
+ * Reset the TVP7002 device
+ * Returns zero when successful or -EINVAL if register read fails.
+ */
+static int tvp7002_reset(struct v4l2_subdev *sd, u32 val)
+{
+ struct tvp7002 *core = to_tvp7002(sd);
+ int polarity;
+ int error;
+
+ error = tvp7002_read(sd, TVP7002_CHIP_REV);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+ if (error == 0x02) {
+ v4l2_info(sd, "rev. %02x detected.\n", error);
+ } else {
+ v4l2_info(sd, "unknown revision detected.\n");
+ v4l2_info(sd, "revision number is %02x\n", error);
+ }
+
+ /* Initializes TVP7002 to its default values */
+ error = tvp7002_write_inittab(sd, tvp7002_init_default);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+ /* Set polarity information after registers have been set */
+ polarity = 0x5b | core->pdata->hs_polarity << 5
+ | core->pdata->vs_polarity << 2;
+ error = tvp7002_write(sd, TVP7002_SYNC_CTL_1, polarity);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+ polarity = 0x0 | core->pdata->fid_polarity << 2
+ | core->pdata->sog_polarity << 1
+ | core->pdata->clk_polarity;
+ error = tvp7002_write(sd, TVP7002_MISC_CTL_3, polarity);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+found_error:
+ return error;
+};
+
+/*
+ * tvp7002_probe - Reset a TVP7002 device
+ * @sd: ptr to v4l2_subdev struct
+ * @ctrl: ptr to i2c_device_id struct
+ *
+ * Reset the TVP7002 device
+ * Returns zero when successful or -EINVAL if register read fails.
+ */
+static int tvp7002_probe(struct i2c_client *c, const struct
*id)
+{
+ struct v4l2_subdev *sd;
+ struct tvp7002 *core;
+ int polarity;
+ int error;
+
+ /* 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;
+
+ if (!c->dev.platform_data) {
+ v4l2_err(c, "No platform data!!\n");
+ return -ENODEV;
+ }
+
+ core = kzalloc(sizeof(struct tvp7002), GFP_KERNEL);
+
+ if (!core)
+ return -ENOMEM;
+
+ *core = tvp7002_dev;
+ sd = &core->sd;
+ core->pdata = c->dev.platform_data;
+
+ v4l2_i2c_subdev_init(sd, c, &tvp7002_ops);
+ v4l_info(c, "tvp7002 found @ 0x%02x (%s)\n",
+ c->addr << 1, c->adapter->name);
+
+ /* Initializes TVP7002 to its default values */
+ error = tvp7002_write_inittab(sd, tvp7002_init_default);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+ /* Set polarity information after registers have been set */
+ polarity = 0x5b | core->pdata->hs_polarity << 5
+ | core->pdata->vs_polarity << 2;
+ error = tvp7002_write(sd, TVP7002_SYNC_CTL_1, polarity);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+ polarity = 0x0 | core->pdata->fid_polarity << 2
+ | core->pdata->sog_polarity << 1
+ | core->pdata->clk_polarity;
+ error = tvp7002_write(sd, TVP7002_MISC_CTL_3, polarity);
+ if (error < 0) {
+ error = -EINVAL;
+ goto found_error;
+ }
+
+ if (debug > 1)
+ tvp7002_log_status(sd);
+
+found_error:
+ if (error < 0)
+ kfree(core);
+
+ return error;
+}
+
+/*
+ * tvp7002_remove - Remove TVP7002 device support
+ * @c: ptr to i2c_client struct
+ *
+ * Reset the TVP7002 device
+ * Returns zero when successful or -EINVAL if register read fails.
+ */
+static int tvp7002_remove(struct i2c_client *c)
+{
+ struct v4l2_subdev *sd = i2c_get_clientdata(c);
+
+ v4l2_dbg(1, debug, sd, "removing tvp7002 adapter"
+ "on address 0x%x\n", c->addr << 1);
+
+ v4l2_device_unregister_subdev(sd);
+ kfree(to_tvp7002(sd));
+ return 0;
+}
+
+/* I2C Device ID table */
+static const struct i2c_device_id tvp7002_id[] = {
+ { "tvp7002", 0 },
+ { }
+};
+MODULE_DEVICE_TABLE(i2c, tvp7002_id);
+
+/* I2C driver data */
+static struct i2c_driver tvp7002_driver = {
+ .driver = {
+ .owner = THIS_MODULE,
+ .name = "tvp7002",
+ },
+ .probe = tvp7002_probe,
+ .remove = tvp7002_remove,
+ .id_table = tvp7002_id,
+};
+
+/*
+ * tvp7002_init - Initialize driver via I2C interface
+ *
+ * Register the TVP7002 driver.
+ * Returns 0 on success or < 0 on failure.
+ */
+static int __init tvp7002_init(void)
+{
+ return i2c_add_driver(&tvp7002_driver);
+}
+
+/*
+ * tvp7002_exit - Remove driver via I2C interface
+ *
+ * Unregister the TVP7002 driver.
+ * Returns 0 on success or < 0 on failure.
+ */
+static void __exit tvp7002_exit(void)
+{
+ i2c_del_driver(&tvp7002_driver);
+}
+
+module_init(tvp7002_init);
+module_exit(tvp7002_exit);
--
1.6.0.4
