@@ -51,6 +51,11 @@
#define MAX_WIDTH_BYTES SZ_16K
#define MAX_HEIGHT_LINES SZ_16K
+#define TI_CSI2RX_PAD_SINK 0
+#define TI_CSI2RX_PAD_FIRST_SOURCE 1
+#define TI_CSI2RX_NUM_SOURCE_PADS 1
+#define TI_CSI2RX_NUM_PADS (1 + TI_CSI2RX_NUM_SOURCE_PADS)
+
#define DRAIN_TIMEOUT_MS 50
#define DRAIN_BUFFER_SIZE SZ_32K
@@ -97,6 +102,7 @@ struct ti_csi2rx_ctx {
struct mutex mutex; /* To serialize ioctls. */
struct v4l2_format v_fmt;
struct ti_csi2rx_dma dma;
+ struct media_pad pad;
u32 sequence;
u32 idx;
};
@@ -104,12 +110,15 @@ struct ti_csi2rx_ctx {
struct ti_csi2rx_dev {
struct device *dev;
void __iomem *shim;
+ struct mutex mutex; /* To serialize ioctls. */
+ unsigned int enable_count;
struct v4l2_device v4l2_dev;
struct media_device mdev;
struct media_pipeline pipe;
- struct media_pad pad;
+ struct media_pad pads[TI_CSI2RX_NUM_PADS];
struct v4l2_async_notifier notifier;
struct v4l2_subdev *source;
+ struct v4l2_subdev subdev;
struct ti_csi2rx_ctx ctx[TI_CSI2RX_NUM_CTX];
/* Buffer to drain stale data from PSI-L endpoint */
struct {
@@ -455,6 +464,15 @@ static int csi_async_notifier_complete(struct v4l2_async_notifier *notifier)
struct ti_csi2rx_dev *csi = dev_get_drvdata(notifier->v4l2_dev->dev);
int ret, i;
+ /* Create link from source to subdev */
+ ret = v4l2_create_fwnode_links_to_pad(csi->source,
+ &csi->pads[TI_CSI2RX_PAD_SINK],
+ MEDIA_LNK_FL_IMMUTABLE |
+ MEDIA_LNK_FL_ENABLED);
+ if (ret)
+ return ret;
+
+ /* Create and link video nodes for all DMA contexts */
for (i = 0; i < TI_CSI2RX_NUM_CTX; i++) {
struct ti_csi2rx_ctx *ctx = &csi->ctx[i];
struct video_device *vdev = &ctx->vdev;
@@ -462,13 +480,17 @@ static int csi_async_notifier_complete(struct v4l2_async_notifier *notifier)
ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
if (ret)
goto unregister_dev;
- }
- ret = v4l2_create_fwnode_links_to_pad(csi->source, &csi->pad,
- MEDIA_LNK_FL_IMMUTABLE |
- MEDIA_LNK_FL_ENABLED);
- if (ret)
- goto unregister_dev;
+ ret = media_create_pad_link(&csi->subdev.entity,
+ TI_CSI2RX_PAD_FIRST_SOURCE + ctx->idx,
+ &vdev->entity, 0,
+ MEDIA_LNK_FL_IMMUTABLE |
+ MEDIA_LNK_FL_ENABLED);
+ if (ret) {
+ video_unregister_device(vdev);
+ goto unregister_dev;
+ }
+ }
ret = v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
if (ret)
@@ -883,7 +905,7 @@ static int ti_csi2rx_start_streaming(struct vb2_queue *vq, unsigned int count)
dma->state = TI_CSI2RX_DMA_ACTIVE;
spin_unlock_irqrestore(&dma->lock, flags);
- ret = v4l2_subdev_call(csi->source, video, s_stream, 1);
+ ret = v4l2_subdev_call(&csi->subdev, video, s_stream, 1);
if (ret)
goto err_dma;
@@ -911,7 +933,7 @@ static void ti_csi2rx_stop_streaming(struct vb2_queue *vq)
writel(0, csi->shim + SHIM_CNTL);
writel(0, csi->shim + SHIM_DMACNTX(ctx->idx));
- ret = v4l2_subdev_call(csi->source, video, s_stream, 0);
+ ret = v4l2_subdev_call(&csi->subdev, video, s_stream, 0);
if (ret)
dev_err(csi->dev, "Failed to stop subdev stream\n");
@@ -929,6 +951,119 @@ static const struct vb2_ops csi_vb2_qops = {
.wait_finish = vb2_ops_wait_finish,
};
+static inline struct ti_csi2rx_dev *to_csi2rx_dev(struct v4l2_subdev *sd)
+{
+ return container_of(sd, struct ti_csi2rx_dev, subdev);
+}
+
+static int ti_csi2rx_sd_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *format)
+{
+ struct v4l2_mbus_framefmt *fmt;
+ int ret = 0;
+
+ /* No transcoding, don't allow setting source fmt */
+ if (format->pad >= TI_CSI2RX_PAD_FIRST_SOURCE)
+ return v4l2_subdev_get_fmt(sd, state, format);
+
+ if (!find_format_by_code(format->format.code))
+ format->format.code = ti_csi2rx_formats[0].code;
+
+ format->format.field = V4L2_FIELD_NONE;
+
+ fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
+ if (!fmt) {
+ ret = -EINVAL;
+ goto out;
+ }
+ *fmt = format->format;
+
+ fmt = v4l2_subdev_state_get_format(state, TI_CSI2RX_PAD_FIRST_SOURCE,
+ format->stream);
+ if (!fmt) {
+ ret = -EINVAL;
+ goto out;
+ }
+ *fmt = format->format;
+
+out:
+ return ret;
+}
+
+static int ti_csi2rx_sd_init_state(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state)
+{
+ struct v4l2_subdev_format format = {
+ .pad = TI_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 ti_csi2rx_sd_set_fmt(sd, state, &format);
+}
+
+static int ti_csi2rx_sd_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct ti_csi2rx_dev *csi = to_csi2rx_dev(sd);
+ int ret = 0;
+
+ mutex_lock(&csi->mutex);
+
+ if (enable) {
+ if (csi->enable_count > 0) {
+ csi->enable_count++;
+ goto out;
+ }
+
+ ret = v4l2_subdev_call(csi->source, video, s_stream, 1);
+ if (ret)
+ goto out;
+
+ csi->enable_count++;
+ } else {
+ if (csi->enable_count == 0) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ if (--csi->enable_count > 0)
+ goto out;
+
+ ret = v4l2_subdev_call(csi->source, video, s_stream, 0);
+ }
+
+out:
+ mutex_unlock(&csi->mutex);
+ return ret;
+}
+
+static const struct v4l2_subdev_pad_ops ti_csi2rx_subdev_pad_ops = {
+ .get_fmt = v4l2_subdev_get_fmt,
+ .set_fmt = ti_csi2rx_sd_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops ti_csi2rx_subdev_video_ops = {
+ .s_stream = ti_csi2rx_sd_s_stream,
+};
+
+static const struct v4l2_subdev_ops ti_csi2rx_subdev_ops = {
+ .video = &ti_csi2rx_subdev_video_ops,
+ .pad = &ti_csi2rx_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops ti_csi2rx_internal_ops = {
+ .init_state = ti_csi2rx_sd_init_state,
+};
+
static void ti_csi2rx_cleanup_dma(struct ti_csi2rx_ctx *ctx)
{
dma_release_channel(ctx->dma.chan);
@@ -936,6 +1071,7 @@ static void ti_csi2rx_cleanup_dma(struct ti_csi2rx_ctx *ctx)
static void ti_csi2rx_cleanup_v4l2(struct ti_csi2rx_dev *csi)
{
+ v4l2_subdev_cleanup(&csi->subdev);
media_device_unregister(&csi->mdev);
v4l2_device_unregister(&csi->v4l2_dev);
media_device_cleanup(&csi->mdev);
@@ -1001,7 +1137,7 @@ static int ti_csi2rx_link_validate(struct media_link *link)
const struct ti_csi2rx_fmt *ti_fmt;
int ret;
- ret = v4l2_subdev_call_state_active(csi->source, pad,
+ ret = v4l2_subdev_call_state_active(&csi->subdev, pad,
get_fmt, &source_fmt);
if (ret)
return ret;
@@ -1046,6 +1182,10 @@ static const struct media_entity_operations ti_csi2rx_video_entity_ops = {
.link_validate = ti_csi2rx_link_validate,
};
+static const struct media_entity_operations ti_csi2rx_subdev_entity_ops = {
+ .link_validate = v4l2_subdev_link_validate,
+};
+
static int ti_csi2rx_init_dma(struct ti_csi2rx_ctx *ctx)
{
struct dma_slave_config cfg = {
@@ -1077,7 +1217,8 @@ static int ti_csi2rx_init_dma(struct ti_csi2rx_ctx *ctx)
static int ti_csi2rx_v4l2_init(struct ti_csi2rx_dev *csi)
{
struct media_device *mdev = &csi->mdev;
- int ret;
+ struct v4l2_subdev *sd = &csi->subdev;
+ int ret, i;
mdev->dev = csi->dev;
mdev->hw_revision = 1;
@@ -1089,16 +1230,50 @@ static int ti_csi2rx_v4l2_init(struct ti_csi2rx_dev *csi)
ret = v4l2_device_register(csi->dev, &csi->v4l2_dev);
if (ret)
- return ret;
+ goto cleanup_media;
ret = media_device_register(mdev);
- if (ret) {
- v4l2_device_unregister(&csi->v4l2_dev);
- media_device_cleanup(mdev);
- return ret;
- }
+ if (ret)
+ goto unregister_v4l2;
+
+ v4l2_subdev_init(sd, &ti_csi2rx_subdev_ops);
+ sd->internal_ops = &ti_csi2rx_internal_ops;
+ sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+ sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+ strscpy(sd->name, dev_name(csi->dev), sizeof(sd->name));
+ sd->dev = csi->dev;
+ sd->entity.ops = &ti_csi2rx_subdev_entity_ops;
+
+ csi->pads[TI_CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+
+ for (i = TI_CSI2RX_PAD_FIRST_SOURCE; i < TI_CSI2RX_NUM_PADS; i++)
+ csi->pads[i].flags = MEDIA_PAD_FL_SOURCE;
+
+ ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(csi->pads),
+ csi->pads);
+ if (ret)
+ goto unregister_media;
+
+ ret = v4l2_subdev_init_finalize(sd);
+ if (ret)
+ goto unregister_media;
+
+ ret = v4l2_device_register_subdev(&csi->v4l2_dev, sd);
+ if (ret)
+ goto cleanup_subdev;
return 0;
+
+cleanup_subdev:
+ v4l2_subdev_cleanup(sd);
+unregister_media:
+ media_device_unregister(mdev);
+unregister_v4l2:
+ v4l2_device_unregister(&csi->v4l2_dev);
+cleanup_media:
+ media_device_cleanup(mdev);
+
+ return ret;
}
static int ti_csi2rx_init_ctx(struct ti_csi2rx_ctx *ctx)
@@ -1125,9 +1300,9 @@ static int ti_csi2rx_init_ctx(struct ti_csi2rx_ctx *ctx)
ti_csi2rx_fill_fmt(fmt, &ctx->v_fmt);
- csi->pad.flags = MEDIA_PAD_FL_SINK;
+ ctx->pad.flags = MEDIA_PAD_FL_SINK;
vdev->entity.ops = &ti_csi2rx_video_entity_ops;
- ret = media_entity_pads_init(&ctx->vdev.entity, 1, &csi->pad);
+ ret = media_entity_pads_init(&ctx->vdev.entity, 1, &ctx->pad);
if (ret)
return ret;
@@ -1183,6 +1358,8 @@ static int ti_csi2rx_probe(struct platform_device *pdev)
if (!csi->drain.vaddr)
return -ENOMEM;
+ mutex_init(&csi->mutex);
+
ret = ti_csi2rx_v4l2_init(csi);
if (ret)
goto err_v4l2;
@@ -1215,6 +1392,7 @@ static int ti_csi2rx_probe(struct platform_device *pdev)
ti_csi2rx_cleanup_ctx(&csi->ctx[i]);
err_v4l2:
+ mutex_destroy(&csi->mutex);
dma_free_coherent(csi->dev, csi->drain.len, csi->drain.vaddr,
csi->drain.paddr);
return ret;
@@ -1235,6 +1413,7 @@ static int ti_csi2rx_remove(struct platform_device *pdev)
ti_csi2rx_cleanup_notifier(csi);
ti_csi2rx_cleanup_v4l2(csi);
+ mutex_destroy(&csi->mutex);
dma_free_coherent(csi->dev, csi->drain.len, csi->drain.vaddr,
csi->drain.paddr);