diff --git a/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c b/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c index d0a681ba78eb..1ec63715baf2 100644 --- a/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c +++ b/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c @@ -143,17 +143,6 @@ static inline struct ti_csi2rx_dev *to_csi2rx_dev(struct v4l2_subdev *sd) return container_of(sd, struct ti_csi2rx_dev, subdev); } -static const struct v4l2_mbus_framefmt ti_csi2rx_default_fmt = { - .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, -}; - static const struct ti_csi2rx_fmt ti_csi2rx_formats[] = { { .fourcc = V4L2_PIX_FMT_YUYV, @@ -565,10 +554,6 @@ static void ti_csi2rx_setup_shim(struct ti_csi2rx_ctx *ctx) fmt = find_format_by_fourcc(ctx->v_fmt.fmt.pix.pixelformat); - /* De-assert the pixel interface reset. */ - reg = SHIM_CNTL_PIX_RST; - writel(reg, csi->shim + SHIM_CNTL); - /* Negotiate pixel count from the source */ ti_csi2rx_request_max_ppc(csi); @@ -887,34 +872,82 @@ static void ti_csi2rx_buffer_queue(struct vb2_buffer *vb) } } +static int ti_csi2rx_get_stream(struct ti_csi2rx_ctx *ctx) +{ + struct ti_csi2rx_dev *csi = ctx->csi; + struct media_pad *pad; + struct v4l2_subdev_state *state; + struct v4l2_subdev_route *r; + + /* Get the source pad connected to this ctx */ + pad = media_entity_remote_source_pad_unique(ctx->pad.entity); + if (!pad) { + dev_err(csi->dev, "No pad connected to ctx %d\n", ctx->idx); + return -ENODEV; + } + + state = v4l2_subdev_get_locked_active_state(&csi->subdev); + + for_each_active_route(&state->routing, r) { + if (r->source_pad == pad->index) { + ctx->stream = r->sink_stream; + return 0; + } + } + + /* No route found for this ctx */ + return -ENODEV; +} + static int ti_csi2rx_get_vc_and_dt(struct ti_csi2rx_ctx *ctx) { struct ti_csi2rx_dev *csi = ctx->csi; + struct ti_csi2rx_ctx *curr_ctx; struct v4l2_mbus_frame_desc fd; - struct media_pad *pad; - int ret, i; + struct media_pad *source_pad; + const struct ti_csi2rx_fmt *fmt; + int ret; + unsigned int i, j; - pad = media_entity_remote_pad_unique(&csi->subdev.entity, MEDIA_PAD_FL_SOURCE); - if (IS_ERR(pad)) - return PTR_ERR(pad); + /* Get the frame desc from source */ + source_pad = media_entity_remote_pad_unique(&csi->subdev.entity, MEDIA_PAD_FL_SOURCE); + if (IS_ERR(source_pad)) + return PTR_ERR(source_pad); - ret = v4l2_subdev_call(csi->source, pad, get_frame_desc, pad->index, &fd); - if (ret) + ret = v4l2_subdev_call(csi->source, pad, get_frame_desc, source_pad->index, &fd); + if (ret) { + if (ret == -ENOIOCTLCMD) { + ctx->vc = 0; + fmt = find_format_by_fourcc(ctx->v_fmt.fmt.pix.pixelformat); + ctx->dt = fmt->csi_dt; + } return ret; + } if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) return -EINVAL; - for (i = 0; i < fd.num_entries; i++) { - if (ctx->stream == fd.entry[i].stream) { - ctx->vc = fd.entry[i].bus.csi2.vc; - ctx->dt = fd.entry[i].bus.csi2.dt; - break; - } + for (i = 0; i < csi->num_ctx; i++) { + curr_ctx = &csi->ctx[i]; - /* Return error if no matching stream found */ - if (i == fd.num_entries) - return -EINVAL; + /* Capture VC 0 by default */ + curr_ctx->vc = 0; + + ret = ti_csi2rx_get_stream(curr_ctx); + if (ret) + continue; + + for (j = 0; j < fd.num_entries; j++) { + if (curr_ctx->stream == fd.entry[j].stream) { + curr_ctx->vc = fd.entry[j].bus.csi2.vc; + curr_ctx->dt = fd.entry[j].bus.csi2.dt; + break; + } + + /* Return error if no matching stream found */ + if (j == fd.num_entries) + return -EINVAL; + } } return 0; @@ -925,8 +958,6 @@ static int ti_csi2rx_start_streaming(struct vb2_queue *vq, unsigned int count) struct ti_csi2rx_ctx *ctx = vb2_get_drv_priv(vq); struct ti_csi2rx_dev *csi = ctx->csi; struct ti_csi2rx_dma *dma = &ctx->dma; - struct ti_csi2rx_buffer *buf; - const struct ti_csi2rx_fmt *fmt; unsigned long flags; int ret = 0; @@ -941,35 +972,9 @@ static int ti_csi2rx_start_streaming(struct vb2_queue *vq, unsigned int count) if (ret) goto err; - ret = ti_csi2rx_get_vc_and_dt(ctx); - if (ret == -ENOIOCTLCMD) { - ctx->vc = 0; - fmt = find_format_by_fourcc(ctx->v_fmt.fmt.pix.pixelformat); - ctx->dt = fmt->csi_dt; - } else if (ret < 0) { - goto err; - } - - ti_csi2rx_setup_shim(ctx); - - ctx->sequence = 0; - - spin_lock_irqsave(&dma->lock, flags); - buf = list_entry(dma->queue.next, struct ti_csi2rx_buffer, list); - - ret = ti_csi2rx_start_dma(ctx, buf); - if (ret) { - dev_err(csi->dev, "Failed to start DMA: %d\n", ret); - spin_unlock_irqrestore(&dma->lock, flags); - goto err_pipeline; - } - - list_move_tail(&buf->list, &dma->submitted); - dma->state = TI_CSI2RX_DMA_ACTIVE; - spin_unlock_irqrestore(&dma->lock, flags); - + /* Start stream 0, we don't allow multiple streams on the source pad */ ret = v4l2_subdev_enable_streams(&csi->subdev, - TI_CSI2RX_PAD_FIRST_SOURCE, + TI_CSI2RX_PAD_FIRST_SOURCE + ctx->idx, BIT_U64(0)); if (ret) goto err_dma; @@ -978,7 +983,6 @@ static int ti_csi2rx_start_streaming(struct vb2_queue *vq, unsigned int count) err_dma: ti_csi2rx_stop_dma(ctx); -err_pipeline: video_device_pipeline_stop(&ctx->vdev); writel(0, csi->shim + SHIM_CNTL); writel(0, csi->shim + SHIM_DMACNTX(ctx->idx)); @@ -995,11 +999,8 @@ static void ti_csi2rx_stop_streaming(struct vb2_queue *vq) video_device_pipeline_stop(&ctx->vdev); - writel(0, csi->shim + SHIM_CNTL); - writel(0, csi->shim + SHIM_DMACNTX(ctx->idx)); - ret = v4l2_subdev_disable_streams(&csi->subdev, - TI_CSI2RX_PAD_FIRST_SOURCE, + TI_CSI2RX_PAD_FIRST_SOURCE + ctx->idx, BIT_U64(0)); if (ret) dev_err(csi->dev, "Failed to stop subdev stream\n"); @@ -1046,25 +1047,84 @@ static int ti_csi2rx_sd_set_fmt(struct v4l2_subdev *sd, fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream); *fmt = format->format; - fmt = v4l2_subdev_state_get_format(state, TI_CSI2RX_PAD_FIRST_SOURCE, - format->stream); + fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad, + format->stream); + if (!fmt) + return -EINVAL; + *fmt = format->format; return 0; } +static int _ti_csi2rx_sd_set_routing(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + struct v4l2_subdev_krouting *routing) +{ + int ret; + + static const struct v4l2_mbus_framefmt 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, + }; + + ret = v4l2_subdev_routing_validate(sd, routing, + V4L2_SUBDEV_ROUTING_ONLY_1_TO_1 | + V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING); + + if (ret) + return ret; + + /* Only stream ID 0 allowed on source pads */ + for (unsigned int i = 0; i < routing->num_routes; ++i) { + const struct v4l2_subdev_route *route = &routing->routes[i]; + + if (route->source_stream != 0) + return -EINVAL; + } + + ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format); + + return ret; +} + +static int ti_csi2rx_sd_set_routing(struct v4l2_subdev *sd, + struct v4l2_subdev_state *state, + enum v4l2_subdev_format_whence which, + struct v4l2_subdev_krouting *routing) +{ + struct ti_csi2rx_dev *csi = to_csi2rx_dev(sd); + + if (csi->enable_count > 0) + return -EBUSY; + + return _ti_csi2rx_sd_set_routing(sd, state, routing); +} + static int ti_csi2rx_sd_init_state(struct v4l2_subdev *sd, struct v4l2_subdev_state *state) { - struct v4l2_mbus_framefmt *fmt; + struct v4l2_subdev_route routes[] = { { + .sink_pad = 0, + .sink_stream = 0, + .source_pad = TI_CSI2RX_PAD_FIRST_SOURCE, + .source_stream = 0, + .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE, + } }; - fmt = v4l2_subdev_state_get_format(state, TI_CSI2RX_PAD_SINK); - *fmt = ti_csi2rx_default_fmt; + struct v4l2_subdev_krouting routing = { + .num_routes = 1, + .routes = routes, + }; - fmt = v4l2_subdev_state_get_format(state, TI_CSI2RX_PAD_FIRST_SOURCE); - *fmt = ti_csi2rx_default_fmt; - - return 0; + /* Initialize routing to single route to the fist source pad */ + return _ti_csi2rx_sd_set_routing(sd, state, &routing); } static int ti_csi2rx_sd_enable_streams(struct v4l2_subdev *sd, @@ -1072,15 +1132,56 @@ static int ti_csi2rx_sd_enable_streams(struct v4l2_subdev *sd, u32 pad, u64 streams_mask) { struct ti_csi2rx_dev *csi = to_csi2rx_dev(sd); + struct ti_csi2rx_ctx *ctx = &csi->ctx[pad - TI_CSI2RX_PAD_FIRST_SOURCE]; + struct ti_csi2rx_dma *dma = &ctx->dma; struct media_pad *remote_pad; + struct ti_csi2rx_buffer *buf; + unsigned long flags; + u64 sink_streams; int ret = 0; + unsigned int reg; + + ret = ti_csi2rx_get_stream(ctx); + if (ret) + return ret; + + /* Get the VC and DT for all enabled ctx on first stream start */ + if (!csi->enable_count) { + ret = ti_csi2rx_get_vc_and_dt(ctx); + if (ret < 0 && ret != -ENOIOCTLCMD) + return ret; + + /* De-assert the pixel interface reset. */ + reg = SHIM_CNTL_PIX_RST; + writel(reg, csi->shim + SHIM_CNTL); + } + + ti_csi2rx_setup_shim(ctx); + ctx->sequence = 0; + + spin_lock_irqsave(&dma->lock, flags); + buf = list_entry(dma->queue.next, struct ti_csi2rx_buffer, list); + + ret = ti_csi2rx_start_dma(ctx, buf); + if (ret) { + dev_err(csi->dev, "Failed to start DMA: %d\n", ret); + spin_unlock_irqrestore(&dma->lock, flags); + return ret; + } + + list_move_tail(&buf->list, &dma->submitted); + dma->state = TI_CSI2RX_DMA_ACTIVE; + spin_unlock_irqrestore(&dma->lock, flags); remote_pad = media_entity_remote_source_pad_unique(&csi->subdev.entity); if (!remote_pad) return -ENODEV; + sink_streams = v4l2_subdev_state_xlate_streams(state, pad, + TI_CSI2RX_PAD_SINK, + &streams_mask); ret = v4l2_subdev_enable_streams(csi->source, remote_pad->index, - BIT_U64(0)); + sink_streams); if (ret) return ret; @@ -1094,18 +1195,28 @@ static int ti_csi2rx_sd_disable_streams(struct v4l2_subdev *sd, u32 pad, u64 streams_mask) { struct ti_csi2rx_dev *csi = to_csi2rx_dev(sd); + struct ti_csi2rx_ctx *ctx = &csi->ctx[pad - TI_CSI2RX_PAD_FIRST_SOURCE]; struct media_pad *remote_pad; + u64 sink_streams; int ret = 0; + WARN_ON(csi->enable_count == 0); + + writel(0, csi->shim + SHIM_DMACNTX(ctx->idx)); + + /* assert pixel reset to prevent stale data */ + if (csi->enable_count == 1) + writel(0, csi->shim + SHIM_CNTL); + remote_pad = media_entity_remote_source_pad_unique(&csi->subdev.entity); if (!remote_pad) return -ENODEV; - - if (csi->enable_count == 0) - return -EINVAL; + sink_streams = v4l2_subdev_state_xlate_streams(state, pad, + TI_CSI2RX_PAD_SINK, + &streams_mask); ret = v4l2_subdev_disable_streams(csi->source, remote_pad->index, - BIT_U64(0)); + sink_streams); if (!ret) --csi->enable_count; @@ -1114,6 +1225,7 @@ static int ti_csi2rx_sd_disable_streams(struct v4l2_subdev *sd, static const struct v4l2_subdev_pad_ops ti_csi2rx_subdev_pad_ops = { .enum_mbus_code = ti_csi2rx_enum_mbus_code, + .set_routing = ti_csi2rx_sd_set_routing, .get_fmt = v4l2_subdev_get_fmt, .set_fmt = ti_csi2rx_sd_set_fmt, .enable_streams = ti_csi2rx_sd_enable_streams, @@ -1293,7 +1405,7 @@ static int ti_csi2rx_v4l2_init(struct ti_csi2rx_dev *csi) 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; + sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS; strscpy(sd->name, dev_name(csi->dev), sizeof(sd->name)); sd->dev = csi->dev; sd->entity.ops = &ti_csi2rx_subdev_entity_ops;