git.librecmc.org Git - oweals/openwrt.git/blob

   1 From 2c51b8e533a8b43bde18072c9dbbd0fc5084bbe7 Mon Sep 17 00:00:00 2001
   2 From: Dave Stevenson <dave.stevenson@raspberrypi.org>
   3 Date: Wed, 2 Oct 2019 17:40:38 +0100
   4 Subject: [PATCH] media: bcm2835-unicam: Rework to not cache the list
   5  of active fmts
   6
   7 Some sensors will change Bayer order based on H & V flips,
   8 therefore collecting the list of formats at async_bound has
   9 problems.
  10
  11 Enumerate the formats from the sensor every time.
  12
  13 Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.org>
  14 ---
  15  .../media/platform/bcm2835/bcm2835-unicam.c   | 246 ++++++++++--------
  16  1 file changed, 136 insertions(+), 110 deletions(-)
  17
  18 --- a/drivers/media/platform/bcm2835/bcm2835-unicam.c
  19 +++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
  20 @@ -363,8 +363,6 @@ struct unicam_device {
  21         /* Used to store current mbus frame format */
  22         struct v4l2_mbus_framefmt m_fmt;
  23
  24 -       struct unicam_fmt active_fmts[MAX_POSSIBLE_PIX_FMTS];
  25 -       int num_active_fmt;
  26         unsigned int virtual_channel;
  27         enum v4l2_mbus_type bus_type;
  28         /*
  29 @@ -455,48 +453,30 @@ static int find_mbus_depth_by_code(u32 c
  30         return 0;
  31  }
  32
  33 -static const struct unicam_fmt *find_format_by_code(struct unicam_device *dev,
  34 -                                                   u32 code)
  35 +static const struct unicam_fmt *find_format_by_code(u32 code)
  36  {
  37 -       const struct unicam_fmt *fmt;
  38         unsigned int k;
  39
  40 -       for (k = 0; k < dev->num_active_fmt; k++) {
  41 -               fmt = &dev->active_fmts[k];
  42 -               if (fmt->code == code)
  43 -                       return fmt;
  44 +       for (k = 0; k < ARRAY_SIZE(formats); k++) {
  45 +               if (formats[k].code == code)
  46 +                       return &formats[k];
  47         }
  48
  49         return NULL;
  50  }
  51
  52 -static const struct unicam_fmt *find_format_by_pix(struct unicam_device *dev,
  53 -                                                  u32 pixelformat)
  54 +static const struct unicam_fmt *find_format_by_pix(u32 pixelformat)
  55  {
  56 -       const struct unicam_fmt *fmt;
  57         unsigned int k;
  58
  59 -       for (k = 0; k < dev->num_active_fmt; k++) {
  60 -               fmt = &dev->active_fmts[k];
  61 -               if (fmt->fourcc == pixelformat)
  62 -                       return fmt;
  63 +       for (k = 0; k < ARRAY_SIZE(formats); k++) {
  64 +               if (formats[k].fourcc == pixelformat)
  65 +                       return &formats[k];
  66         }
  67
  68         return NULL;
  69  }
  70
  71 -static void dump_active_formats(struct unicam_device *dev)
  72 -{
  73 -       int i;
  74 -
  75 -       for (i = 0; i < dev->num_active_fmt; i++) {
  76 -               unicam_dbg(3, dev, "active_fmt[%d] (%p) is code %04x, fourcc " V4L2_FOURCC_CONV ", depth %d\n",
  77 -                          i, &dev->active_fmts[i], dev->active_fmts[i].code,
  78 -                          V4L2_FOURCC_CONV_ARGS(dev->active_fmts[i].fourcc),
  79 -                          dev->active_fmts[i].depth);
  80 -       }
  81 -}
  82 -
  83  static inline unsigned int bytes_per_line(u32 width,
  84                                           const struct unicam_fmt *fmt)
  85  {
  86 @@ -726,14 +706,40 @@ static int unicam_enum_fmt_vid_cap(struc
  87                                    struct v4l2_fmtdesc *f)
  88  {
  89         struct unicam_device *dev = video_drvdata(file);
  90 +       struct v4l2_subdev_mbus_code_enum mbus_code;
  91         const struct unicam_fmt *fmt = NULL;
  92 +       int index = 0;
  93 +       int ret = 0;
  94 +       int i;
  95
  96 -       if (f->index >= dev->num_active_fmt)
  97 -               return -EINVAL;
  98 +       /* Loop whilst the sensor driver says it has more formats, but add a
  99 +        * failsafe against a dodgy driver at 128 (more than any sensor will
 100 +        * ever sensibly advertise)
 101 +        */
 102 +       for (i = 0; !ret && i < 128 ; i++) {
 103 +               memset(&mbus_code, 0, sizeof(mbus_code));
 104 +               mbus_code.index = i;
 105
 106 -       fmt = &dev->active_fmts[f->index];
 107 +               ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code,
 108 +                                      NULL, &mbus_code);
 109 +               if (ret < 0) {
 110 +                       unicam_dbg(2, dev,
 111 +                                  "subdev->enum_mbus_code idx %d returned %d - index invalid\n",
 112 +                                  i, ret);
 113 +                       return -EINVAL;
 114 +               }
 115
 116 -       f->pixelformat = fmt->fourcc;
 117 +               fmt = find_format_by_code(mbus_code.code);
 118 +               if (fmt) {
 119 +                       if (fmt->fourcc) {
 120 +                               if (index == f->index) {
 121 +                                       f->pixelformat = fmt->fourcc;
 122 +                                       break;
 123 +                               }
 124 +                               index++;
 125 +                       }
 126 +               }
 127 +       }
 128
 129         return 0;
 130  }
 131 @@ -748,6 +754,39 @@ static int unicam_g_fmt_vid_cap(struct f
 132         return 0;
 133  }
 134
 135 +static
 136 +const struct unicam_fmt *get_first_supported_format(struct unicam_device *dev)
 137 +{
 138 +       struct v4l2_subdev_mbus_code_enum mbus_code;
 139 +       const struct unicam_fmt *fmt = NULL;
 140 +       int ret;
 141 +       int j;
 142 +
 143 +       for (j = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++j) {
 144 +               memset(&mbus_code, 0, sizeof(mbus_code));
 145 +               mbus_code.index = j;
 146 +               ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL,
 147 +                                      &mbus_code);
 148 +               if (ret < 0) {
 149 +                       unicam_dbg(2, dev,
 150 +                                  "subdev->enum_mbus_code idx %d returned %d - continue\n",
 151 +                                  j, ret);
 152 +                       continue;
 153 +               }
 154 +
 155 +               unicam_dbg(2, dev, "subdev %s: code: %04x idx: %d\n",
 156 +                          dev->sensor->name, mbus_code.code, j);
 157 +
 158 +               fmt = find_format_by_code(mbus_code.code);
 159 +               unicam_dbg(2, dev, "fmt %04x returned as %p, V4L2 FOURCC %04x, csi_dt %02X\n",
 160 +                          mbus_code.code, fmt, fmt ? fmt->fourcc : 0,
 161 +                          fmt ? fmt->csi_dt : 0);
 162 +               if (fmt)
 163 +                       return fmt;
 164 +       }
 165 +
 166 +       return NULL;
 167 +}
 168  static int unicam_try_fmt_vid_cap(struct file *file, void *priv,
 169                                   struct v4l2_format *f)
 170  {
 171 @@ -759,13 +798,15 @@ static int unicam_try_fmt_vid_cap(struct
 172         struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
 173         int ret;
 174
 175 -       fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
 176 +       fmt = find_format_by_pix(f->fmt.pix.pixelformat);
 177         if (!fmt) {
 178 -               unicam_dbg(3, dev, "Fourcc format (0x%08x) not found. Use default of %08X\n",
 179 -                          f->fmt.pix.pixelformat, dev->active_fmts[0].fourcc);
 180 +               /* Pixel format not supported by unicam. Choose the first
 181 +                * supported format, and let the sensor choose something else.
 182 +                */
 183 +               unicam_dbg(3, dev, "Fourcc format (0x%08x) not found. Use first format.\n",
 184 +                          f->fmt.pix.pixelformat);
 185
 186 -               /* Just get the first one enumerated */
 187 -               fmt = &dev->active_fmts[0];
 188 +               fmt = &formats[0];
 189                 f->fmt.pix.pixelformat = fmt->fourcc;
 190         }
 191
 192 @@ -785,6 +826,40 @@ static int unicam_try_fmt_vid_cap(struct
 193                 unicam_info(dev, "Sensor trying to send interlaced video - results may be unpredictable\n");
 194
 195         v4l2_fill_pix_format(&f->fmt.pix, &sd_fmt.format);
 196 +       if (mbus_fmt->code != fmt->code) {
 197 +               /* Sensor has returned an alternate format */
 198 +               fmt = find_format_by_code(mbus_fmt->code);
 199 +               if (!fmt) {
 200 +                       /* The alternate format is one unicam can't support.
 201 +                        * Find the first format that is supported by both, and
 202 +                        * then set that.
 203 +                        */
 204 +                       fmt = get_first_supported_format(dev);
 205 +                       mbus_fmt->code = fmt->code;
 206 +
 207 +                       ret = v4l2_subdev_call(dev->sensor, pad, set_fmt,
 208 +                                              dev->sensor_config, &sd_fmt);
 209 +                       if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
 210 +                               return ret;
 211 +
 212 +                       if (mbus_fmt->field != V4L2_FIELD_NONE)
 213 +                               unicam_info(dev, "Sensor trying to send interlaced video - results may be unpredictable\n");
 214 +
 215 +                       v4l2_fill_pix_format(&f->fmt.pix, &sd_fmt.format);
 216 +
 217 +                       if (mbus_fmt->code != fmt->code) {
 218 +                               /* We've set a format that the sensor reports
 219 +                                * as being supported, but it refuses to set it.
 220 +                                * Not much else we can do.
 221 +                                * Assume that the sensor driver may accept the
 222 +                                * format when it is set (rather than tried).
 223 +                                */
 224 +                               unicam_err(dev, "Sensor won't accept default format, and Unicam can't support sensor default\n");
 225 +                       }
 226 +               }
 227 +
 228 +               f->fmt.pix.pixelformat = fmt->fourcc;
 229 +       }
 230
 231         return unicam_calc_format_size_bpl(dev, fmt, f);
 232  }
 233 @@ -805,10 +880,18 @@ static int unicam_s_fmt_vid_cap(struct f
 234         if (ret < 0)
 235                 return ret;
 236
 237 -       fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
 238 +       fmt = find_format_by_pix(f->fmt.pix.pixelformat);
 239         if (!fmt) {
 240 -               /* Unknown pixel format - adopt a default */
 241 -               fmt = &dev->active_fmts[0];
 242 +               /* Unknown pixel format - adopt a default.
 243 +                * This shouldn't happen as try_fmt should have resolved any
 244 +                * issues first.
 245 +                */
 246 +               fmt = get_first_supported_format(dev);
 247 +               if (!fmt)
 248 +                       /* It shouldn't be possible to get here with no
 249 +                        * supported formats
 250 +                        */
 251 +                       return -EINVAL;
 252                 f->fmt.pix.pixelformat = fmt->fourcc;
 253                 return -EINVAL;
 254         }
 255 @@ -944,6 +1027,7 @@ static void unicam_set_packing_config(st
 256                         unpack = UNICAM_PUM_NONE;
 257                         break;
 258                 }
 259 +
 260                 switch (v4l2_depth) {
 261                 case 8:
 262                         pack = UNICAM_PPM_PACK8;
 263 @@ -1439,7 +1523,7 @@ static int unicam_enum_framesizes(struct
 264         int ret;
 265
 266         /* check for valid format */
 267 -       fmt = find_format_by_pix(dev, fsize->pixel_format);
 268 +       fmt = find_format_by_pix(fsize->pixel_format);
 269         if (!fmt) {
 270                 unicam_dbg(3, dev, "Invalid pixel code: %x\n",
 271                            fsize->pixel_format);
 272 @@ -1478,7 +1562,7 @@ static int unicam_enum_frameintervals(st
 273         };
 274         int ret;
 275
 276 -       fmt = find_format_by_pix(dev, fival->pixel_format);
 277 +       fmt = find_format_by_pix(fival->pixel_format);
 278         if (!fmt)
 279                 return -EINVAL;
 280
 281 @@ -1742,27 +1826,6 @@ static const struct v4l2_ioctl_ops unica
 282         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 283  };
 284
 285 -/*
 286 - * Adds an entry to the active_fmts array
 287 - * Returns non-zero if attempting to write off the end of the array.
 288 - */
 289 -static int unicam_add_active_format(struct unicam_device *unicam,
 290 -                                   const struct unicam_fmt *fmt)
 291 -{
 292 -       //Ensure we don't run off the end of the array.
 293 -       if (unicam->num_active_fmt >= MAX_POSSIBLE_PIX_FMTS)
 294 -               return 1;
 295 -
 296 -       unicam->active_fmts[unicam->num_active_fmt] = *fmt;
 297 -       unicam_dbg(2, unicam,
 298 -                  "matched fourcc: " V4L2_FOURCC_CONV ": code: %04x idx: %d\n",
 299 -                  V4L2_FOURCC_CONV_ARGS(fmt->fourcc),
 300 -                  fmt->code, unicam->num_active_fmt);
 301 -       unicam->num_active_fmt++;
 302 -
 303 -       return 0;
 304 -}
 305 -
 306  static int
 307  unicam_async_bound(struct v4l2_async_notifier *notifier,
 308                    struct v4l2_subdev *subdev,
 309 @@ -1770,9 +1833,6 @@ unicam_async_bound(struct v4l2_async_not
 310  {
 311         struct unicam_device *unicam = container_of(notifier->v4l2_dev,
 312                                                struct unicam_device, v4l2_dev);
 313 -       struct v4l2_subdev_mbus_code_enum mbus_code;
 314 -       int ret = 0;
 315 -       int j;
 316
 317         if (unicam->sensor) {
 318                 unicam_info(unicam, "Rejecting subdev %s (Already set!!)",
 319 @@ -1783,47 +1843,6 @@ unicam_async_bound(struct v4l2_async_not
 320         unicam->sensor = subdev;
 321         unicam_dbg(1, unicam, "Using sensor %s for capture\n", subdev->name);
 322
 323 -       /* Enumerate sub device formats and enable all matching local formats */
 324 -       unicam->num_active_fmt = 0;
 325 -       unicam_dbg(2, unicam, "Get supported formats...\n");
 326 -       for (j = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++j) {
 327 -               const struct unicam_fmt *fmt = NULL;
 328 -               int k;
 329 -
 330 -               memset(&mbus_code, 0, sizeof(mbus_code));
 331 -               mbus_code.index = j;
 332 -               ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
 333 -                                      NULL, &mbus_code);
 334 -               if (ret < 0) {
 335 -                       unicam_dbg(2, unicam,
 336 -                                  "subdev->enum_mbus_code idx %d returned %d - continue\n",
 337 -                                  j, ret);
 338 -                       continue;
 339 -               }
 340 -
 341 -               unicam_dbg(2, unicam, "subdev %s: code: %04x idx: %d\n",
 342 -                          subdev->name, mbus_code.code, j);
 343 -
 344 -               for (k = 0; k < ARRAY_SIZE(formats); k++) {
 345 -                       if (mbus_code.code == formats[k].code) {
 346 -                               fmt = &formats[k];
 347 -                               break;
 348 -                       }
 349 -               }
 350 -               unicam_dbg(2, unicam, "fmt %04x returned as %p, V4L2 FOURCC %04x, csi_dt %02X\n",
 351 -                          mbus_code.code, fmt, fmt ? fmt->fourcc : 0,
 352 -                          fmt ? fmt->csi_dt : 0);
 353 -               if (fmt) {
 354 -                       if (unicam_add_active_format(unicam, fmt)) {
 355 -                               unicam_dbg(1, unicam, "Active fmt list truncated\n");
 356 -                               break;
 357 -                       }
 358 -               }
 359 -       }
 360 -       unicam_dbg(2, unicam,
 361 -                  "Done all formats\n");
 362 -       dump_active_formats(unicam);
 363 -
 364         return 0;
 365  }
 366
 367 @@ -1849,10 +1868,17 @@ static int unicam_probe_complete(struct
 368                 return ret;
 369         }
 370
 371 -       fmt = find_format_by_code(unicam, mbus_fmt.code);
 372 +       fmt = find_format_by_code(mbus_fmt.code);
 373         if (!fmt) {
 374 -               /* Default image format not valid. Choose first active fmt. */
 375 -               fmt = &unicam->active_fmts[0];
 376 +               /* Find the first format that the sensor and unicam both
 377 +                * support
 378 +                */
 379 +               fmt = get_first_supported_format(unicam);
 380 +
 381 +               if (!fmt)
 382 +                       /* No compatible formats */
 383 +                       return -EINVAL;
 384 +
 385                 mbus_fmt.code = fmt->code;
 386                 ret = __subdev_set_format(unicam, &mbus_fmt);
 387                 if (ret)