target/linux/bcm27xx/patches-5.4/950-0803-media-rcar-csi2-Negotiate-data-lanes-number.patch

   1 From 15221304a23fd99c84a6da4b68dc0e887150d1ee Mon Sep 17 00:00:00 2001
   2 From: Jacopo Mondi <jacopo+renesas@jmondi.org>
   3 Date: Tue, 16 Jun 2020 16:12:44 +0200
   4 Subject: [PATCH] media: rcar-csi2: Negotiate data lanes number
   5
   6 Upstream https://patchwork.linuxtv.org/patch/64675/
   7
   8 Use the newly introduced get_mbus_config() subdevice pad operation to
   9 retrieve the remote subdevice MIPI CSI-2 bus configuration and configure
  10 the number of active data lanes accordingly.
  11
  12 In order to be able to call the remote subdevice operation cache the
  13 index of the remote pad connected to the single CSI-2 input port.
  14
  15 Signed-off-by: Jacopo Mondi <jacopo+renesas@jmondi.org>
  16 ---
  17  drivers/media/platform/rcar-vin/rcar-csi2.c | 74 +++++++++++++++++++--
  18  1 file changed, 67 insertions(+), 7 deletions(-)
  19
  20 --- a/drivers/media/platform/rcar-vin/rcar-csi2.c
  21 +++ b/drivers/media/platform/rcar-vin/rcar-csi2.c
  22 @@ -362,6 +362,7 @@ struct rcar_csi2 {
  23
  24         struct v4l2_async_notifier notifier;
  25         struct v4l2_subdev *remote;
  26 +       unsigned int remote_pad;
  27
  28         struct v4l2_mbus_framefmt mf;
  29
  30 @@ -407,13 +408,14 @@ static void rcsi2_exit_standby(struct rc
  31         reset_control_deassert(priv->rstc);
  32  }
  33
  34 -static int rcsi2_wait_phy_start(struct rcar_csi2 *priv)
  35 +static int rcsi2_wait_phy_start(struct rcar_csi2 *priv,
  36 +                               unsigned int active_lanes)
  37  {
  38         unsigned int timeout;
  39
  40         /* Wait for the clock and data lanes to enter LP-11 state. */
  41         for (timeout = 0; timeout <= 20; timeout++) {
  42 -               const u32 lane_mask = (1 << priv->lanes) - 1;
  43 +               const u32 lane_mask = (1 << active_lanes) - 1;
  44
  45                 if ((rcsi2_read(priv, PHCLM_REG) & PHCLM_STOPSTATECKL)  &&
  46                     (rcsi2_read(priv, PHDLM_REG) & lane_mask) == lane_mask)
  47 @@ -445,7 +447,8 @@ static int rcsi2_set_phypll(struct rcar_
  48         return 0;
  49  }
  50
  51 -static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp)
  52 +static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp,
  53 +                          unsigned int active_lanes)
  54  {
  55         struct v4l2_subdev *source;
  56         struct v4l2_ctrl *ctrl;
  57 @@ -470,15 +473,63 @@ static int rcsi2_calc_mbps(struct rcar_c
  58          * bps = link_freq * 2
  59          */
  60         mbps = v4l2_ctrl_g_ctrl_int64(ctrl) * bpp;
  61 -       do_div(mbps, priv->lanes * 1000000);
  62 +       do_div(mbps, active_lanes * 1000000);
  63
  64         return mbps;
  65  }
  66
  67 +static int rcsi2_config_active_lanes(struct rcar_csi2 *priv,
  68 +                                    unsigned int *active_lanes)
  69 +{
  70 +       struct v4l2_mbus_config mbus_config = { 0 };
  71 +       unsigned int num_lanes = (-1U);
  72 +       int ret;
  73 +
  74 +       *active_lanes = priv->lanes;
  75 +       ret = v4l2_subdev_call(priv->remote, pad, get_mbus_config,
  76 +                              priv->remote_pad, &mbus_config);
  77 +       if (ret == -ENOIOCTLCMD) {
  78 +               dev_dbg(priv->dev, "No remote mbus configuration available\n");
  79 +               return 0;
  80 +       }
  81 +
  82 +       if (ret) {
  83 +               dev_err(priv->dev, "Failed to get remote mbus configuration\n");
  84 +               return ret;
  85 +       }
  86 +
  87 +       if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) {
  88 +               dev_err(priv->dev, "Unsupported media bus type %u\n",
  89 +                       mbus_config.type);
  90 +               return -EINVAL;
  91 +       }
  92 +
  93 +       if (mbus_config.flags & V4L2_MBUS_CSI2_1_LANE)
  94 +               num_lanes = 1;
  95 +       else if (mbus_config.flags & V4L2_MBUS_CSI2_2_LANE)
  96 +               num_lanes = 2;
  97 +       else if (mbus_config.flags & V4L2_MBUS_CSI2_3_LANE)
  98 +               num_lanes = 3;
  99 +       else if (mbus_config.flags & V4L2_MBUS_CSI2_4_LANE)
 100 +               num_lanes = 4;
 101 +
 102 +       if (num_lanes > priv->lanes) {
 103 +               dev_err(priv->dev,
 104 +                       "Unsupported mbus config: too many data lanes %u\n",
 105 +                       num_lanes);
 106 +               return -EINVAL;
 107 +       }
 108 +
 109 +       *active_lanes = num_lanes;
 110 +
 111 +       return 0;
 112 +}
 113 +
 114  static int rcsi2_start_receiver(struct rcar_csi2 *priv)
 115  {
 116         const struct rcar_csi2_format *format;
 117         u32 phycnt, vcdt = 0, vcdt2 = 0, fld = 0;
 118 +       unsigned int active_lanes;
 119         unsigned int i;
 120         int mbps, ret;
 121
 122 @@ -522,10 +573,18 @@ static int rcsi2_start_receiver(struct r
 123                         fld |= FLD_FLD_NUM(1);
 124         }
 125
 126 +       /*
 127 +        * Get the number of active data lanes inspecting the remote mbus
 128 +        * configuration.
 129 +        */
 130 +       ret = rcsi2_config_active_lanes(priv, &active_lanes);
 131 +       if (ret)
 132 +               return ret;
 133 +
 134         phycnt = PHYCNT_ENABLECLK;
 135 -       phycnt |= (1 << priv->lanes) - 1;
 136 +       phycnt |= (1 << active_lanes) - 1;
 137
 138 -       mbps = rcsi2_calc_mbps(priv, format->bpp);
 139 +       mbps = rcsi2_calc_mbps(priv, format->bpp, active_lanes);
 140         if (mbps < 0)
 141                 return mbps;
 142
 143 @@ -572,7 +631,7 @@ static int rcsi2_start_receiver(struct r
 144         rcsi2_write(priv, PHYCNT_REG, phycnt | PHYCNT_SHUTDOWNZ);
 145         rcsi2_write(priv, PHYCNT_REG, phycnt | PHYCNT_SHUTDOWNZ | PHYCNT_RSTZ);
 146
 147 -       ret = rcsi2_wait_phy_start(priv);
 148 +       ret = rcsi2_wait_phy_start(priv, active_lanes);
 149         if (ret)
 150                 return ret;
 151
 152 @@ -749,6 +808,7 @@ static int rcsi2_notify_bound(struct v4l
 153         }
 154
 155         priv->remote = subdev;
 156 +       priv->remote_pad = pad;
 157
 158         dev_dbg(priv->dev, "Bound %s pad: %d\n", subdev->name, pad);
 159