@@ -61,6 +61,11 @@ enum csi2rx_pads {
CSI2RX_PAD_MAX,
};
+struct csi2rx_fmt {
+ u32 code;
+ u8 bpp;
+};
+
struct csi2rx_priv {
struct device *dev;
unsigned int count;
@@ -95,6 +100,32 @@ struct csi2rx_priv {
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, },
+ { .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, },
+ { .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, },
+ { .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, },
+ { .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, },
+ { .code = MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, },
+ { .code = MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, },
+ { .code = MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, },
+ { .code = MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, },
+};
+
+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)
{
@@ -303,12 +334,68 @@ static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
return ret;
}
+static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct v4l2_mbus_framefmt *fmt;
+ unsigned int i;
+
+ /* No transcoding, source and sink formats must match. */
+ if (format->pad != CSI2RX_PAD_SINK)
+ return v4l2_subdev_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;
+
+ /* Set sink format */
+ fmt = v4l2_subdev_get_pad_format(subdev, state, format->pad);
+ *fmt = format->format;
+
+ /* Propagate to source formats */
+ for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
+ fmt = v4l2_subdev_get_pad_format(subdev, state, i);
+ *fmt = format->format;
+ }
+
+ return 0;
+}
+
+static int csi2rx_init_cfg(struct v4l2_subdev *subdev,
+ struct v4l2_subdev_state *state)
+{
+ struct v4l2_subdev_format format = {
+ .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 = v4l2_subdev_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,
@@ -532,9 +619,13 @@ static int csi2rx_probe(struct platform_device *pdev)
if (ret)
goto err_cleanup;
+ ret = v4l2_subdev_init_finalize(&csi2rx->subdev);
+ if (ret)
+ goto err_cleanup;
+
ret = v4l2_async_register_subdev(&csi2rx->subdev);
if (ret < 0)
- goto err_cleanup;
+ goto err_free_state;
dev_info(&pdev->dev,
"Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
@@ -544,6 +635,8 @@ static int csi2rx_probe(struct platform_device *pdev)
return 0;
+err_free_state:
+ v4l2_subdev_cleanup(&csi2rx->subdev);
err_cleanup:
v4l2_async_nf_unregister(&csi2rx->notifier);
v4l2_async_nf_cleanup(&csi2rx->notifier);
@@ -560,6 +653,7 @@ static void csi2rx_remove(struct platform_device *pdev)
v4l2_async_nf_unregister(&csi2rx->notifier);
v4l2_async_nf_cleanup(&csi2rx->notifier);
v4l2_async_unregister_subdev(&csi2rx->subdev);
+ v4l2_subdev_cleanup(&csi2rx->subdev);
media_entity_cleanup(&csi2rx->subdev.entity);
kfree(csi2rx);
}