@@ -54,6 +54,11 @@ enum csi2rx_pads {
CSI2RX_PAD_MAX,
};
+struct csi2rx_fmt {
+ u32 code;
+ u8 bpp;
+};
+
struct csi2rx_priv {
struct device *dev;
unsigned int count;
@@ -79,12 +84,43 @@ struct csi2rx_priv {
struct v4l2_subdev subdev;
struct v4l2_async_notifier notifier;
struct media_pad pads[CSI2RX_PAD_MAX];
+ struct v4l2_mbus_framefmt fmt;
/* Remote source */
struct v4l2_subdev *source_subdev;
int source_pad;
};
+static const struct csi2rx_fmt formats[] = {
+ {
+ .code = MEDIA_BUS_FMT_YUYV8_1X16,
+ .bpp = 16,
+ },
+ {
+ .code = MEDIA_BUS_FMT_UYVY8_1X16,
+ .bpp = 16,
+ },
+ {
+ .code = MEDIA_BUS_FMT_YVYU8_1X16,
+ .bpp = 16,
+ },
+ {
+ .code = MEDIA_BUS_FMT_VYUY8_1X16,
+ .bpp = 16,
+ },
+};
+
+static const struct csi2rx_fmt *csi2rx_get_fmt_by_code(u32 code)
+{
+ unsigned int i;
+
+ for (i = 0; i < ARRAY_SIZE(formats); i++)
+ if (formats[i].code == code)
+ return &formats[i];
+
+ return NULL;
+}
+
static inline
struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
{
@@ -236,12 +272,103 @@ static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
return ret;
}
+static struct v4l2_mbus_framefmt *
+csi2rx_get_pad_format(struct csi2rx_priv *csi2rx,
+ struct v4l2_subdev_state *state,
+ unsigned int pad, u32 which)
+{
+ switch (which) {
+ case V4L2_SUBDEV_FORMAT_TRY:
+ return v4l2_subdev_get_try_format(&csi2rx->subdev, state, pad);
+ case V4L2_SUBDEV_FORMAT_ACTIVE:
+ return &csi2rx->fmt;
+ default:
+ return NULL;
+ }
+}
+
+static int csi2rx_get_fmt(struct v4l2_subdev *subdev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
+ struct v4l2_mbus_framefmt *framefmt;
+
+ framefmt = csi2rx_get_pad_format(csi2rx, state, format->pad,
+ format->which);
+ if (!framefmt)
+ return -EINVAL;
+
+ mutex_lock(&csi2rx->lock);
+ format->format = *framefmt;
+ mutex_unlock(&csi2rx->lock);
+
+ return 0;
+}
+
+static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
+ struct v4l2_mbus_framefmt *framefmt;
+
+ /* No transcoding, source and sink formats must match. */
+ if (format->pad != CSI2RX_PAD_SINK)
+ return csi2rx_get_fmt(subdev, state, format);
+
+ if (!csi2rx_get_fmt_by_code(format->format.code))
+ format->format.code = formats[0].code;
+
+ format->format.field = V4L2_FIELD_NONE;
+
+ framefmt = csi2rx_get_pad_format(csi2rx, state, format->pad,
+ format->which);
+ if (!framefmt)
+ return -EINVAL;
+
+ mutex_lock(&csi2rx->lock);
+ *framefmt = format->format;
+ mutex_unlock(&csi2rx->lock);
+
+ return 0;
+}
+
+static int csi2rx_init_cfg(struct v4l2_subdev *subdev,
+ struct v4l2_subdev_state *state)
+{
+ struct v4l2_subdev_format format = {
+ .which = state ? V4L2_SUBDEV_FORMAT_TRY
+ : V4L2_SUBDEV_FORMAT_ACTIVE,
+ .pad = CSI2RX_PAD_SINK,
+ .format = {
+ .width = 640,
+ .height = 480,
+ .code = MEDIA_BUS_FMT_UYVY8_1X16,
+ .field = V4L2_FIELD_NONE,
+ .colorspace = V4L2_COLORSPACE_SRGB,
+ .ycbcr_enc = V4L2_YCBCR_ENC_601,
+ .quantization = V4L2_QUANTIZATION_LIM_RANGE,
+ .xfer_func = V4L2_XFER_FUNC_SRGB,
+ },
+ };
+
+ return csi2rx_set_fmt(subdev, state, &format);
+}
+
+static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = {
+ .get_fmt = csi2rx_get_fmt,
+ .set_fmt = csi2rx_set_fmt,
+ .init_cfg = csi2rx_init_cfg,
+};
+
static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
.s_stream = csi2rx_s_stream,
};
static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
.video = &csi2rx_video_ops,
+ .pad = &csi2rx_pad_ops,
};
static int csi2rx_async_bound(struct v4l2_async_notifier *notifier,
@@ -457,6 +584,10 @@ static int csi2rx_probe(struct platform_device *pdev)
if (ret)
goto err_cleanup;
+ ret = csi2rx_init_cfg(&csi2rx->subdev, NULL);
+ if (ret)
+ goto err_cleanup;
+
ret = v4l2_async_register_subdev(&csi2rx->subdev);
if (ret < 0)
goto err_cleanup;