target/linux/bcm27xx/patches-6.1/950-1061-drivers-media-cfe-Set-the-CSI-2-link-frequency-corre.patch

   1 From a11312709f46b71bf320a9dcc8cf4e09056552cd Mon Sep 17 00:00:00 2001
   2 From: Naushir Patuck <naush@raspberrypi.com>
   3 Date: Wed, 1 Nov 2023 13:25:54 +0000
   4 Subject: [PATCH] drivers: media: cfe: Set the CSI-2 link frequency correctly
   5
   6 Use the sensor provided link frequency to set the DPHY timing parameters
   7 on stream_on. This replaces the hard-coded 999 MHz value currently being
   8 used. As a fallback, revert to the original 999 Mhz link frequency.
   9
  10 As a drive-by, fix a 80-character line formatting error.
  11
  12 Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
  13 ---
  14  .../media/platform/raspberrypi/rp1_cfe/cfe.c  | 63 +++++++++++++++++--
  15  1 file changed, 58 insertions(+), 5 deletions(-)
  16
  17 --- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
  18 +++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
  19 @@ -780,7 +780,8 @@ static void cfe_start_channel(struct cfe
  20                         __func__, node_desc[FE_OUT0].name,
  21                         cfe->fe_csi2_channel);
  22
  23 -               source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state, cfe->fe_csi2_channel);
  24 +               source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state,
  25 +                                                       cfe->fe_csi2_channel);
  26                 fmt = find_format_by_code(source_fmt->code);
  27
  28                 width = source_fmt->width;
  29 @@ -982,6 +983,59 @@ static void cfe_buffer_queue(struct vb2_
  30         spin_unlock_irqrestore(&cfe->state_lock, flags);
  31  }
  32
  33 +static u64 sensor_link_frequency(struct cfe_device *cfe)
  34 +{
  35 +       struct v4l2_mbus_framefmt *source_fmt;
  36 +       struct v4l2_subdev_state *state;
  37 +       struct media_entity *entity;
  38 +       struct v4l2_subdev *subdev;
  39 +       const struct cfe_fmt *fmt;
  40 +       struct media_pad *pad;
  41 +       s64 link_freq;
  42 +
  43 +       state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd);
  44 +       source_fmt = v4l2_subdev_get_pad_format(&cfe->csi2.sd, state, 0);
  45 +       fmt = find_format_by_code(source_fmt->code);
  46 +       v4l2_subdev_unlock_state(state);
  47 +
  48 +       /*
  49 +        * Walk up the media graph to find either the sensor entity, or another
  50 +        * entity that advertises the V4L2_CID_LINK_FREQ or V4L2_CID_PIXEL_RATE
  51 +        * control through the subdev.
  52 +        */
  53 +       entity = &cfe->csi2.sd.entity;
  54 +       while (1) {
  55 +               pad = &entity->pads[0];
  56 +               if (!(pad->flags & MEDIA_PAD_FL_SINK))
  57 +                       goto err;
  58 +
  59 +               pad = media_pad_remote_pad_first(pad);
  60 +               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
  61 +                       goto err;
  62 +
  63 +               entity = pad->entity;
  64 +               subdev = media_entity_to_v4l2_subdev(entity);
  65 +               if (entity->function == MEDIA_ENT_F_CAM_SENSOR ||
  66 +                   v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_LINK_FREQ) ||
  67 +                   v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE))
  68 +                       break;
  69 +       }
  70 +
  71 +       link_freq = v4l2_get_link_freq(subdev->ctrl_handler, fmt->depth,
  72 +                                      cfe->csi2.active_data_lanes * 2);
  73 +       if (link_freq < 0)
  74 +               goto err;
  75 +
  76 +       /* x2 for DDR. */
  77 +       link_freq *= 2;
  78 +       cfe_info("Using a link frequency of %lld Hz\n", link_freq);
  79 +       return link_freq;
  80 +
  81 +err:
  82 +       cfe_err("Unable to determine sensor link frequency, using 999 MHz\n");
  83 +       return 999 * 1000000UL;
  84 +}
  85 +
  86  static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count)
  87  {
  88         struct v4l2_mbus_config mbus_config = { 0 };
  89 @@ -1049,10 +1103,11 @@ static int cfe_start_streaming(struct vb
  90                 goto err_disable_cfe;
  91         }
  92
  93 -       cfe_dbg("Starting sensor streaming\n");
  94 -
  95 +       cfe_dbg("Configuring CSI-2 block\n");
  96 +       cfe->csi2.dphy.dphy_freq = sensor_link_frequency(cfe) / 1000000UL;
  97         csi2_open_rx(&cfe->csi2);
  98
  99 +       cfe_dbg("Starting sensor streaming\n");
 100         cfe->sequence = 0;
 101         ret = v4l2_subdev_call(cfe->sensor, video, s_stream, 1);
 102         if (ret < 0) {
 103 @@ -1945,8 +2000,6 @@ static int of_cfe_connect_subdevs(struct
 104                 }
 105         }
 106
 107 -       /* TODO: Get the frequency from devicetree */
 108 -       cfe->csi2.dphy.dphy_freq = 999;
 109         cfe->csi2.dphy.num_lanes = ep.bus.mipi_csi2.num_data_lanes;
 110         cfe->csi2.bus_flags = ep.bus.mipi_csi2.flags;
 111