From a11312709f46b71bf320a9dcc8cf4e09056552cd Mon Sep 17 00:00:00 2001 From: Naushir Patuck Date: Wed, 1 Nov 2023 13:25:54 +0000 Subject: [PATCH] drivers: media: cfe: Set the CSI-2 link frequency correctly Use the sensor provided link frequency to set the DPHY timing parameters on stream_on. This replaces the hard-coded 999 MHz value currently being used. As a fallback, revert to the original 999 Mhz link frequency. As a drive-by, fix a 80-character line formatting error. Signed-off-by: Naushir Patuck --- .../media/platform/raspberrypi/rp1_cfe/cfe.c | 63 +++++++++++++++++-- 1 file changed, 58 insertions(+), 5 deletions(-) --- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c +++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c @@ -780,7 +780,8 @@ static void cfe_start_channel(struct cfe __func__, node_desc[FE_OUT0].name, cfe->fe_csi2_channel); - source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state, cfe->fe_csi2_channel); + source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state, + cfe->fe_csi2_channel); fmt = find_format_by_code(source_fmt->code); width = source_fmt->width; @@ -982,6 +983,59 @@ static void cfe_buffer_queue(struct vb2_ spin_unlock_irqrestore(&cfe->state_lock, flags); } +static u64 sensor_link_frequency(struct cfe_device *cfe) +{ + struct v4l2_mbus_framefmt *source_fmt; + struct v4l2_subdev_state *state; + struct media_entity *entity; + struct v4l2_subdev *subdev; + const struct cfe_fmt *fmt; + struct media_pad *pad; + s64 link_freq; + + state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd); + source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state, 0); + fmt = find_format_by_code(source_fmt->code); + v4l2_subdev_unlock_state(state); + + /* + * Walk up the media graph to find either the sensor entity, or another + * entity that advertises the V4L2_CID_LINK_FREQ or V4L2_CID_PIXEL_RATE + * control through the subdev. + */ + entity = &cfe->csi2.sd.entity; + while (1) { + pad = &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + goto err; + + pad = media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + goto err; + + entity = pad->entity; + subdev = media_entity_to_v4l2_subdev(entity); + if (entity->function == MEDIA_ENT_F_CAM_SENSOR || + v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_LINK_FREQ) || + v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE)) + break; + } + + link_freq = v4l2_get_link_freq(subdev->ctrl_handler, fmt->depth, + cfe->csi2.active_data_lanes * 2); + if (link_freq < 0) + goto err; + + /* x2 for DDR. */ + link_freq *= 2; + cfe_info("Using a link frequency of %lld Hz\n", link_freq); + return link_freq; + +err: + cfe_err("Unable to determine sensor link frequency, using 999 MHz\n"); + return 999 * 1000000UL; +} + static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count) { struct v4l2_mbus_config mbus_config = { 0 }; @@ -1049,10 +1103,11 @@ static int cfe_start_streaming(struct vb goto err_disable_cfe; } - cfe_dbg("Starting sensor streaming\n"); - + cfe_dbg("Configuring CSI-2 block\n"); + cfe->csi2.dphy.dphy_freq = sensor_link_frequency(cfe) / 1000000UL; csi2_open_rx(&cfe->csi2); + cfe_dbg("Starting sensor streaming\n"); cfe->sequence = 0; ret = v4l2_subdev_call(cfe->sensor, video, s_stream, 1); if (ret < 0) { @@ -1945,8 +2000,6 @@ static int of_cfe_connect_subdevs(struct } } - /* TODO: Get the frequency from devicetree */ - cfe->csi2.dphy.dphy_freq = 999; cfe->csi2.dphy.num_lanes = ep.bus.mipi_csi2.num_data_lanes; cfe->csi2.bus_flags = ep.bus.mipi_csi2.flags;