target/linux/bcm27xx/patches-5.15/950-0247-media-bcm2835-unicam-Add-support-for-get_mbus_config.patch

   1 From 8aea3cbdd53ea33b91022ea80ee5df0ae297194b Mon Sep 17 00:00:00 2001
   2 From: Dave Stevenson <dave.stevenson@raspberrypi.com>
   3 Date: Tue, 23 Jun 2020 14:32:51 +0100
   4 Subject: [PATCH] media: bcm2835-unicam: Add support for
   5  get_mbus_config to set num lanes
   6
   7 Use the get_mbus_config pad subdev call to allow a source to use
   8 fewer than the number of CSI2 lanes defined in device tree.
   9
  10 Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
  11 ---
  12  .../media/platform/bcm2835/bcm2835-unicam.c   | 31 ++++++++++++++++---
  13  1 file changed, 27 insertions(+), 4 deletions(-)
  14
  15 --- a/drivers/media/platform/bcm2835/bcm2835-unicam.c
  16 +++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
  17 @@ -1639,12 +1639,35 @@ static int unicam_start_streaming(struct
  18                 goto err_streaming;
  19         }
  20
  21 -       /*
  22 -        * TODO: Retrieve the number of active data lanes from the connected
  23 -        * subdevice.
  24 -        */
  25         dev->active_data_lanes = dev->max_data_lanes;
  26
  27 +       if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
  28 +               struct v4l2_mbus_config mbus_config = { 0 };
  29 +
  30 +               ret = v4l2_subdev_call(dev->sensor, pad, get_mbus_config,
  31 +                                      0, &mbus_config);
  32 +               if (ret < 0 && ret != -ENOIOCTLCMD) {
  33 +                       unicam_dbg(3, dev, "g_mbus_config failed\n");
  34 +                       goto err_pm_put;
  35 +               }
  36 +
  37 +               dev->active_data_lanes =
  38 +                       (mbus_config.flags & V4L2_MBUS_CSI2_LANE_MASK) >>
  39 +                                       __ffs(V4L2_MBUS_CSI2_LANE_MASK);
  40 +               if (!dev->active_data_lanes)
  41 +                       dev->active_data_lanes = dev->max_data_lanes;
  42 +               if (dev->active_data_lanes > dev->max_data_lanes) {
  43 +                       unicam_err(dev, "Device has requested %u data lanes, which is >%u configured in DT\n",
  44 +                                  dev->active_data_lanes,
  45 +                                  dev->max_data_lanes);
  46 +                       ret = -EINVAL;
  47 +                       goto err_pm_put;
  48 +               }
  49 +       }
  50 +
  51 +       unicam_dbg(1, dev, "Running with %u data lanes\n",
  52 +                  dev->active_data_lanes);
  53 +
  54         ret = clk_set_rate(dev->clock, 100 * 1000 * 1000);
  55         if (ret) {
  56                 unicam_err(dev, "failed to set up clock\n");