From 425a6b752c38b50c97220db37a67b18b281f56e5 Mon Sep 17 00:00:00 2001 From: Tomi Valkeinen Date: Thu, 21 Sep 2023 15:28:20 +0300 Subject: [PATCH] media: rp1: csi2: Use get_frame_desc to get CSI-2 VC and DT Use get_frame_desc pad op for asking the CSI-2 VC and DT from the source device driver, instead of hardcoding to VC 0, and getting the DT from a formats table. To keep backward compatibility with sources that do not implement get_frame_desc, implement a fallback mechanism that always uses VC 0, and gets the DT from the formats table, based on the CSI2's sink pad's format. Signed-off-by: Tomi Valkeinen --- .../media/platform/raspberrypi/rp1_cfe/cfe.c | 4 +- .../media/platform/raspberrypi/rp1_cfe/csi2.c | 75 ++++++++++++++++++- .../media/platform/raspberrypi/rp1_cfe/csi2.h | 2 +- 3 files changed, 77 insertions(+), 4 deletions(-) --- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c +++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c @@ -838,7 +838,7 @@ static void cfe_start_channel(struct cfe * this is handled by the CSI2 AUTO_ARM mode. */ csi2_start_channel(&cfe->csi2, cfe->fe_csi2_channel, - fmt->csi_dt, CSI2_MODE_FE_STREAMING, + CSI2_MODE_FE_STREAMING, true, false, width, height); csi2_set_buffer(&cfe->csi2, cfe->fe_csi2_channel, 0, 0, -1); pisp_fe_start(&cfe->fe); @@ -872,7 +872,7 @@ static void cfe_start_channel(struct cfe } } /* Unconditionally start this CSI2 channel. */ - csi2_start_channel(&cfe->csi2, node->id, fmt->csi_dt, + csi2_start_channel(&cfe->csi2, node->id, mode, /* Auto arm */ false, --- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c +++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c @@ -324,12 +324,84 @@ void csi2_set_compression(struct csi2_de csi2_reg_write(csi2, CSI2_CH_COMP_CTRL(channel), compression); } +static int csi2_get_vc_dt_fallback(struct csi2_device *csi2, + unsigned int channel, u8 *vc, u8 *dt) +{ + struct v4l2_subdev *sd = &csi2->sd; + struct v4l2_subdev_state *state; + struct v4l2_mbus_framefmt *fmt; + const struct cfe_fmt *cfe_fmt; + + state = v4l2_subdev_get_locked_active_state(sd); + + /* Without Streams API, the channel number matches the sink pad */ + fmt = v4l2_subdev_get_pad_format(sd, state, channel); + if (!fmt) + return -EINVAL; + + cfe_fmt = find_format_by_code(fmt->code); + if (!cfe_fmt) + return -EINVAL; + + *vc = 0; + *dt = cfe_fmt->csi_dt; + + return 0; +} + +static int csi2_get_vc_dt(struct csi2_device *csi2, unsigned int channel, + u8 *vc, u8 *dt) +{ + struct v4l2_mbus_frame_desc remote_desc; + const struct media_pad *remote_pad; + struct v4l2_subdev *source_sd; + int ret; + + /* Without Streams API, the channel number matches the sink pad */ + remote_pad = media_pad_remote_pad_first(&csi2->pad[channel]); + if (!remote_pad) + return -EPIPE; + + source_sd = media_entity_to_v4l2_subdev(remote_pad->entity); + + ret = v4l2_subdev_call(source_sd, pad, get_frame_desc, + remote_pad->index, &remote_desc); + if (ret == -ENOIOCTLCMD) { + csi2_dbg("source does not support get_frame_desc, use fallback\n"); + return csi2_get_vc_dt_fallback(csi2, channel, vc, dt); + } else if (ret) { + csi2_err("Failed to get frame descriptor\n"); + return ret; + } + + if (remote_desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) { + csi2_err("Frame descriptor does not describe CSI-2 link"); + return -EINVAL; + } + + if (remote_desc.num_entries != 1) { + csi2_err("Frame descriptor does not have a single entry"); + return -EINVAL; + } + + *vc = remote_desc.entry[0].bus.csi2.vc; + *dt = remote_desc.entry[0].bus.csi2.dt; + + return 0; +} + void csi2_start_channel(struct csi2_device *csi2, unsigned int channel, - u16 dt, enum csi2_mode mode, bool auto_arm, + enum csi2_mode mode, bool auto_arm, bool pack_bytes, unsigned int width, unsigned int height) { u32 ctrl; + int ret; + u8 vc, dt; + + ret = csi2_get_vc_dt(csi2, channel, &vc, &dt); + if (ret) + return; csi2_dbg("%s [%u]\n", __func__, channel); @@ -369,6 +441,7 @@ void csi2_start_channel(struct csi2_devi csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0); } + set_field(&ctrl, vc, VC_MASK); set_field(&ctrl, dt, DT_MASK); csi2_reg_write(csi2, CSI2_CH_CTRL(channel), ctrl); csi2->num_lines[channel] = height; --- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h +++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h @@ -79,7 +79,7 @@ void csi2_set_compression(struct csi2_de enum csi2_compression_mode mode, unsigned int shift, unsigned int offset); void csi2_start_channel(struct csi2_device *csi2, unsigned int channel, - u16 dt, enum csi2_mode mode, bool auto_arm, + enum csi2_mode mode, bool auto_arm, bool pack_bytes, unsigned int width, unsigned int height); void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel);