Call get_frame_desc to find out VC & DT, instead of hardcoding the VC
routing and deducing the DT based on the mbus format.
Signed-off-by: Tomi Valkeinen <tomi.valkeinen+renesas@xxxxxxxxxxxxxxxx>
---
drivers/media/platform/renesas/rcar-isp.c | 108 +++++++++++++++++++++---------
1 file changed, 77 insertions(+), 31 deletions(-)
diff --git a/drivers/media/platform/renesas/rcar-isp.c b/drivers/media/platform/renesas/rcar-isp.c
index 5f3fcdb7e37e..53927954ba73 100644
--- a/drivers/media/platform/renesas/rcar-isp.c
+++ b/drivers/media/platform/renesas/rcar-isp.c
@@ -225,24 +225,86 @@ static void risp_power_off(struct rcar_isp *isp)
pm_runtime_put(isp->dev);
}
-static int risp_start(struct rcar_isp *isp, struct v4l2_subdev_state *state)
+static int risp_configure_routing(struct rcar_isp *isp,
+ struct v4l2_subdev_state *state)
{
- const struct v4l2_mbus_framefmt *fmt;
- const struct rcar_isp_format *format;
- unsigned int vc;
- u32 sel_csi = 0;
+ struct v4l2_mbus_frame_desc source_fd;
+ struct v4l2_subdev_route *route;
int ret;
- fmt = v4l2_subdev_state_get_format(state, RCAR_ISP_SINK, 0);
- if (!fmt)
- return -EINVAL;
+ ret = v4l2_subdev_call(isp->remote, pad, get_frame_desc,
+ isp->remote_pad, &source_fd);
+ if (ret)
+ return ret;
- format = risp_code_to_fmt(fmt->code);
- if (!format) {
- dev_err(isp->dev, "Unsupported bus format\n");
- return -EINVAL;
+ /* Clear the channel registers */
+ for (unsigned int ch = 0; ch < 12; ++ch) {
+ risp_write(isp, ISPCS_FILTER_ID_CH_REG(ch), 0);
+ risp_write(isp, ISPCS_DT_CODE03_CH_REG(ch), 0);
}
+ /* Clear the proc mode registers */
+ for (unsigned int dt = 0; dt < 64; ++dt)
+ risp_write(isp, ISPPROCMODE_DT_REG(dt), 0);
+
+ for_each_active_route(&state->routing, route) {
+ struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
+ const struct rcar_isp_format *format;
+ const struct v4l2_mbus_framefmt *fmt;
+ unsigned int i;
+ u8 vc, dt, ch;
+ u32 v;
+
+ for (i = 0; i < source_fd.num_entries; i++) {
+ if (source_fd.entry[i].stream == route->sink_stream) {
+ source_entry = &source_fd.entry[i];
+ break;
+ }
+ }
+
+ if (!source_entry) {
+ dev_err(isp->dev,
+ "Failed to find stream from source frame desc\n");
+ return -EPIPE;
+ }
+
+ vc = source_entry->bus.csi2.vc;
+ dt = source_entry->bus.csi2.dt;
+ /* Channels 4 - 11 go to VIN */
+ ch = route->source_pad - 1 + 4;
+
+ fmt = v4l2_subdev_state_get_format(state, route->sink_pad,
+ route->sink_stream);
+ if (!fmt)
+ return -EINVAL;
+
+ format = risp_code_to_fmt(fmt->code);
+ if (!format) {
+ dev_err(isp->dev, "Unsupported bus format\n");
+ return -EINVAL;
+ }
+
+ /* VC Filtering */
+ risp_write(isp, ISPCS_FILTER_ID_CH_REG(ch), BIT(vc));
+
+ /* DT Filtering */
+ risp_write(isp, ISPCS_DT_CODE03_CH_REG(ch),
+ ISPCS_DT_CODE03_EN0 | ISPCS_DT_CODE03_DT0(dt));
+
+ /* Proc mode */
+ v = risp_read(isp, ISPPROCMODE_DT_REG(dt));
+ v |= ISPPROCMODE_DT_PROC_MODE_VCn(vc, format->procmode);
+ risp_write(isp, ISPPROCMODE_DT_REG(dt), v);
+ }
+
+ return 0;
+}
+
+static int risp_start(struct rcar_isp *isp, struct v4l2_subdev_state *state)
+{
+ u32 sel_csi = 0;
+ int ret;
+
ret = risp_power_on(isp);
if (ret) {
dev_err(isp->dev, "Failed to power on ISP\n");
@@ -256,25 +318,9 @@ static int risp_start(struct rcar_isp *isp, struct v4l2_subdev_state *state)
risp_write(isp, ISPINPUTSEL0_REG,
risp_read(isp, ISPINPUTSEL0_REG) | sel_csi);
- /* Configure Channel Selector. */
- for (vc = 0; vc < 4; vc++) {
- u8 ch = vc + 4;
- u8 dt = format->datatype;
-
- risp_write(isp, ISPCS_FILTER_ID_CH_REG(ch), BIT(vc));
- risp_write(isp, ISPCS_DT_CODE03_CH_REG(ch),
- ISPCS_DT_CODE03_EN3 | ISPCS_DT_CODE03_DT3(dt) |
- ISPCS_DT_CODE03_EN2 | ISPCS_DT_CODE03_DT2(dt) |
- ISPCS_DT_CODE03_EN1 | ISPCS_DT_CODE03_DT1(dt) |
- ISPCS_DT_CODE03_EN0 | ISPCS_DT_CODE03_DT0(dt));
- }
-
- /* Setup processing method. */
- risp_write(isp, ISPPROCMODE_DT_REG(format->datatype),
- ISPPROCMODE_DT_PROC_MODE_VCn(3, format->procmode) |
- ISPPROCMODE_DT_PROC_MODE_VCn(2, format->procmode) |
- ISPPROCMODE_DT_PROC_MODE_VCn(1, format->procmode) |
- ISPPROCMODE_DT_PROC_MODE_VCn(0, format->procmode));
+ ret = risp_configure_routing(isp, state);
+ if (ret)
+ return ret;
/* Start ISP. */
risp_write(isp, ISPSTART_REG, ISPSTART_START);
--
2.43.0
