target/linux/bcm27xx/patches-6.1/950-1206-media-rp1-csi2-Use-get_frame_desc-to-get-CSI-2-VC-an.patch

   1 From 425a6b752c38b50c97220db37a67b18b281f56e5 Mon Sep 17 00:00:00 2001
   2 From: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
   3 Date: Thu, 21 Sep 2023 15:28:20 +0300
   4 Subject: [PATCH] media: rp1: csi2: Use get_frame_desc to get CSI-2 VC and DT
   5
   6 Use get_frame_desc pad op for asking the CSI-2 VC and DT from the source
   7 device driver, instead of hardcoding to VC 0, and getting the DT from a
   8 formats table. To keep backward compatibility with sources that do not
   9 implement get_frame_desc, implement a fallback mechanism that always
  10 uses VC 0, and gets the DT from the formats table, based on the CSI2's
  11 sink pad's format.
  12
  13 Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
  14 ---
  15  .../media/platform/raspberrypi/rp1_cfe/cfe.c  |  4 +-
  16  .../media/platform/raspberrypi/rp1_cfe/csi2.c | 75 ++++++++++++++++++-
  17  .../media/platform/raspberrypi/rp1_cfe/csi2.h |  2 +-
  18  3 files changed, 77 insertions(+), 4 deletions(-)
  19
  20 --- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
  21 +++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
  22 @@ -838,7 +838,7 @@ static void cfe_start_channel(struct cfe
  23                  * this is handled by the CSI2 AUTO_ARM mode.
  24                  */
  25                 csi2_start_channel(&cfe->csi2, cfe->fe_csi2_channel,
  26 -                                  fmt->csi_dt, CSI2_MODE_FE_STREAMING,
  27 +                                  CSI2_MODE_FE_STREAMING,
  28                                    true, false, width, height);
  29                 csi2_set_buffer(&cfe->csi2, cfe->fe_csi2_channel, 0, 0, -1);
  30                 pisp_fe_start(&cfe->fe);
  31 @@ -872,7 +872,7 @@ static void cfe_start_channel(struct cfe
  32                         }
  33                 }
  34                 /* Unconditionally start this CSI2 channel. */
  35 -               csi2_start_channel(&cfe->csi2, node->id, fmt->csi_dt,
  36 +               csi2_start_channel(&cfe->csi2, node->id,
  37                                    mode,
  38                                    /* Auto arm */
  39                                    false,
  40 --- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
  41 +++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.c
  42 @@ -324,12 +324,84 @@ void csi2_set_compression(struct csi2_de
  43         csi2_reg_write(csi2, CSI2_CH_COMP_CTRL(channel), compression);
  44  }
  45
  46 +static int csi2_get_vc_dt_fallback(struct csi2_device *csi2,
  47 +                                  unsigned int channel, u8 *vc, u8 *dt)
  48 +{
  49 +       struct v4l2_subdev *sd = &csi2->sd;
  50 +       struct v4l2_subdev_state *state;
  51 +       struct v4l2_mbus_framefmt *fmt;
  52 +       const struct cfe_fmt *cfe_fmt;
  53 +
  54 +       state = v4l2_subdev_get_locked_active_state(sd);
  55 +
  56 +       /* Without Streams API, the channel number matches the sink pad */
  57 +       fmt = v4l2_subdev_get_pad_format(sd, state, channel);
  58 +       if (!fmt)
  59 +               return -EINVAL;
  60 +
  61 +       cfe_fmt = find_format_by_code(fmt->code);
  62 +       if (!cfe_fmt)
  63 +               return -EINVAL;
  64 +
  65 +       *vc = 0;
  66 +       *dt = cfe_fmt->csi_dt;
  67 +
  68 +       return 0;
  69 +}
  70 +
  71 +static int csi2_get_vc_dt(struct csi2_device *csi2, unsigned int channel,
  72 +                         u8 *vc, u8 *dt)
  73 +{
  74 +       struct v4l2_mbus_frame_desc remote_desc;
  75 +       const struct media_pad *remote_pad;
  76 +       struct v4l2_subdev *source_sd;
  77 +       int ret;
  78 +
  79 +       /* Without Streams API, the channel number matches the sink pad */
  80 +       remote_pad = media_pad_remote_pad_first(&csi2->pad[channel]);
  81 +       if (!remote_pad)
  82 +               return -EPIPE;
  83 +
  84 +       source_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
  85 +
  86 +       ret = v4l2_subdev_call(source_sd, pad, get_frame_desc,
  87 +                              remote_pad->index, &remote_desc);
  88 +       if (ret == -ENOIOCTLCMD) {
  89 +               csi2_dbg("source does not support get_frame_desc, use fallback\n");
  90 +               return csi2_get_vc_dt_fallback(csi2, channel, vc, dt);
  91 +       } else if (ret) {
  92 +               csi2_err("Failed to get frame descriptor\n");
  93 +               return ret;
  94 +       }
  95 +
  96 +       if (remote_desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
  97 +               csi2_err("Frame descriptor does not describe CSI-2 link");
  98 +               return -EINVAL;
  99 +       }
 100 +
 101 +       if (remote_desc.num_entries != 1) {
 102 +               csi2_err("Frame descriptor does not have a single entry");
 103 +               return -EINVAL;
 104 +       }
 105 +
 106 +       *vc = remote_desc.entry[0].bus.csi2.vc;
 107 +       *dt = remote_desc.entry[0].bus.csi2.dt;
 108 +
 109 +       return 0;
 110 +}
 111 +
 112  void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
 113 -                       u16 dt, enum csi2_mode mode, bool auto_arm,
 114 +                       enum csi2_mode mode, bool auto_arm,
 115                         bool pack_bytes, unsigned int width,
 116                         unsigned int height)
 117  {
 118         u32 ctrl;
 119 +       int ret;
 120 +       u8 vc, dt;
 121 +
 122 +       ret = csi2_get_vc_dt(csi2, channel, &vc, &dt);
 123 +       if (ret)
 124 +               return;
 125
 126         csi2_dbg("%s [%u]\n", __func__, channel);
 127
 128 @@ -369,6 +441,7 @@ void csi2_start_channel(struct csi2_devi
 129                 csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0);
 130         }
 131
 132 +       set_field(&ctrl, vc, VC_MASK);
 133         set_field(&ctrl, dt, DT_MASK);
 134         csi2_reg_write(csi2, CSI2_CH_CTRL(channel), ctrl);
 135         csi2->num_lines[channel] = height;
 136 --- a/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
 137 +++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi2.h
 138 @@ -79,7 +79,7 @@ void csi2_set_compression(struct csi2_de
 139                           enum csi2_compression_mode mode, unsigned int shift,
 140                           unsigned int offset);
 141  void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
 142 -                       u16 dt, enum csi2_mode mode, bool auto_arm,
 143 +                       enum csi2_mode mode, bool auto_arm,
 144                         bool pack_bytes, unsigned int width,
 145                         unsigned int height);
 146  void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel);