target/linux/bcm27xx/patches-5.4/950-0462-media-ov5647-Add-basic-support-for-multiple-sensor-m.patch

   1 From 9e584d9de3387588bf455d3c45ec6a092bfa4266 Mon Sep 17 00:00:00 2001
   2 From: David Plowman <david.plowman@raspberrypi.com>
   3 Date: Wed, 29 Jan 2020 15:30:53 +0000
   4 Subject: [PATCH] media: ov5647: Add basic support for multiple sensor
   5  modes.
   6
   7 Specifically:
   8
   9 Added a structure ov5647_mode and a list of supported_modes (though no
  10 actual new modes as yet). The state object points to the "current mode".
  11
  12 ov5647_enum_mbus_code, ov5647_enum_frame_size, ov5647_set_fmt and
  13 ov5647_get_fmt all needed upgrading to cope with multiple modes.
  14
  15 __sensor_init (which writes all the registers) is now called by
  16 ov5647_stream_on (once the mode is known) rather than by
  17 ov5647_sensor_power.
  18
  19 Signed-off-by: David Plowman <david.plowman@raspberrypi.com>
  20 Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
  21 ---
  22  drivers/media/i2c/ov5647.c | 268 ++++++++++++++++++++++++++++---------
  23  1 file changed, 202 insertions(+), 66 deletions(-)
  24
  25 --- a/drivers/media/i2c/ov5647.c
  26 +++ b/drivers/media/i2c/ov5647.c
  27 @@ -86,13 +86,17 @@ struct regval_list {
  28         u8 data;
  29  };
  30
  31 +struct ov5647_mode {
  32 +       struct v4l2_mbus_framefmt       format;
  33 +       struct regval_list              *reg_list;
  34 +       unsigned int                    num_regs;
  35 +};
  36 +
  37  struct ov5647 {
  38         struct v4l2_subdev              sd;
  39         struct media_pad                pad;
  40         struct mutex                    lock;
  41 -       struct v4l2_mbus_framefmt       format;
  42 -       unsigned int                    width;
  43 -       unsigned int                    height;
  44 +       const struct ov5647_mode        *mode;
  45         int                             power_count;
  46         struct clk                      *xclk;
  47         struct gpio_desc                *pwdn;
  48 @@ -207,6 +211,32 @@ static struct regval_list ov5647_640x480
  49         {0x0100, 0x01},
  50  };
  51
  52 +static struct ov5647_mode supported_modes_8bit[] = {
  53 +       /*
  54 +        * Original 8-bit VGA mode
  55 +        * Uncentred crop (top left quarter) from 2x2 binned 1296x972 image.
  56 +        */
  57 +       {
  58 +               {
  59 +                       .code = MEDIA_BUS_FMT_SBGGR8_1X8,
  60 +                       .colorspace = V4L2_COLORSPACE_SRGB,
  61 +                       .field = V4L2_FIELD_NONE,
  62 +                       .width = 640,
  63 +                       .height = 480
  64 +               },
  65 +               ov5647_640x480,
  66 +               ARRAY_SIZE(ov5647_640x480)
  67 +       },
  68 +       /* more modes below here... */
  69 +};
  70 +
  71 +static struct ov5647_mode supported_modes_10bit[] = {
  72 +       /* no 10-bit modes yet */
  73 +};
  74 +
  75 +/* Use original 8-bit VGA mode as default. */
  76 +#define OV5647_DEFAULT_MODE (&supported_modes_8bit[0])
  77 +
  78  static int ov5647_write(struct v4l2_subdev *sd, u16 reg, u8 val)
  79  {
  80         int ret;
  81 @@ -293,12 +323,55 @@ static int ov5647_set_virtual_channel(st
  82         return ov5647_write(sd, OV5647_REG_MIPI_CTRL14, channel_id | (channel << 6));
  83  }
  84
  85 +static int __sensor_init(struct v4l2_subdev *sd)
  86 +{
  87 +       int ret;
  88 +       u8 resetval, rdval;
  89 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
  90 +       struct ov5647 *state = to_state(sd);
  91 +
  92 +       ret = ov5647_read(sd, OV5647_SW_STANDBY, &rdval);
  93 +       if (ret < 0)
  94 +               return ret;
  95 +
  96 +       ret = ov5647_write_array(sd, state->mode->reg_list,
  97 +                                state->mode->num_regs);
  98 +       if (ret < 0) {
  99 +               dev_err(&client->dev, "write sensor default regs error\n");
 100 +               return ret;
 101 +       }
 102 +
 103 +       ret = ov5647_set_virtual_channel(sd, 0);
 104 +       if (ret < 0)
 105 +               return ret;
 106 +
 107 +       ret = ov5647_read(sd, OV5647_SW_STANDBY, &resetval);
 108 +       if (ret < 0)
 109 +               return ret;
 110 +
 111 +       if (!(resetval & 0x01)) {
 112 +               dev_err(&client->dev, "Device was in SW standby");
 113 +               ret = ov5647_write(sd, OV5647_SW_STANDBY, 0x01);
 114 +               if (ret < 0)
 115 +                       return ret;
 116 +       }
 117 +
 118 +       return 0;
 119 +}
 120 +
 121  static int ov5647_stream_on(struct v4l2_subdev *sd)
 122  {
 123 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 124         struct ov5647 *ov5647 = to_state(sd);
 125         u8 val = MIPI_CTRL00_BUS_IDLE;
 126         int ret;
 127
 128 +       ret = __sensor_init(sd);
 129 +       if (ret < 0) {
 130 +               dev_err(&client->dev, "sensor_init failed\n");
 131 +               return ret;
 132 +       }
 133 +
 134         if (ov5647->flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)
 135                 val |= MIPI_CTRL00_CLOCK_LANE_GATE |
 136                        MIPI_CTRL00_LINE_SYNC_ENABLE;
 137 @@ -347,44 +420,6 @@ static int set_sw_standby(struct v4l2_su
 138         return ov5647_write(sd, OV5647_SW_STANDBY, rdval);
 139  }
 140
 141 -static int __sensor_init(struct v4l2_subdev *sd)
 142 -{
 143 -       int ret;
 144 -       u8 resetval, rdval;
 145 -       struct i2c_client *client = v4l2_get_subdevdata(sd);
 146 -
 147 -       ret = ov5647_read(sd, OV5647_SW_STANDBY, &rdval);
 148 -       if (ret < 0)
 149 -               return ret;
 150 -
 151 -       ret = ov5647_write_array(sd, ov5647_640x480,
 152 -                                       ARRAY_SIZE(ov5647_640x480));
 153 -       if (ret < 0) {
 154 -               dev_err(&client->dev, "write sensor default regs error\n");
 155 -               return ret;
 156 -       }
 157 -
 158 -       ret = ov5647_set_virtual_channel(sd, 0);
 159 -       if (ret < 0)
 160 -               return ret;
 161 -
 162 -       ret = ov5647_read(sd, OV5647_SW_STANDBY, &resetval);
 163 -       if (ret < 0)
 164 -               return ret;
 165 -
 166 -       if (!(resetval & 0x01)) {
 167 -               dev_err(&client->dev, "Device was in SW standby");
 168 -               ret = ov5647_write(sd, OV5647_SW_STANDBY, 0x01);
 169 -               if (ret < 0)
 170 -                       return ret;
 171 -       }
 172 -
 173 -       /*
 174 -        * stream off to make the clock lane into LP-11 state.
 175 -        */
 176 -       return ov5647_stream_off(sd);
 177 -}
 178 -
 179  static int ov5647_sensor_power(struct v4l2_subdev *sd, int on)
 180  {
 181         int ret = 0;
 182 @@ -408,7 +443,7 @@ static int ov5647_sensor_power(struct v4
 183                 }
 184
 185                 ret = ov5647_write_array(sd, sensor_oe_enable_regs,
 186 -                               ARRAY_SIZE(sensor_oe_enable_regs));
 187 +                                        ARRAY_SIZE(sensor_oe_enable_regs));
 188                 if (ret < 0) {
 189                         clk_disable_unprepare(ov5647->xclk);
 190                         dev_err(&client->dev,
 191 @@ -416,7 +451,10 @@ static int ov5647_sensor_power(struct v4
 192                         goto out;
 193                 }
 194
 195 -               ret = __sensor_init(sd);
 196 +               /*
 197 +                * Ensure streaming off to make clock lane go into LP-11 state.
 198 +                */
 199 +               ret = ov5647_stream_off(sd);
 200                 if (ret < 0) {
 201                         clk_disable_unprepare(ov5647->xclk);
 202                         dev_err(&client->dev,
 203 @@ -427,7 +465,7 @@ static int ov5647_sensor_power(struct v4
 204                 dev_dbg(&client->dev, "OV5647 power off\n");
 205
 206                 ret = ov5647_write_array(sd, sensor_oe_disable_regs,
 207 -                               ARRAY_SIZE(sensor_oe_disable_regs));
 208 +                                        ARRAY_SIZE(sensor_oe_disable_regs));
 209
 210                 if (ret < 0)
 211                         dev_dbg(&client->dev, "disable oe failed\n");
 212 @@ -489,10 +527,19 @@ static const struct v4l2_subdev_core_ops
 213
 214  static int ov5647_s_stream(struct v4l2_subdev *sd, int enable)
 215  {
 216 +       struct ov5647 *state = to_state(sd);
 217 +       int ret = 0;
 218 +
 219 +       mutex_lock(&state->lock);
 220 +
 221         if (enable)
 222 -               return ov5647_stream_on(sd);
 223 +               ret = ov5647_stream_on(sd);
 224         else
 225 -               return ov5647_stream_off(sd);
 226 +               ret = ov5647_stream_off(sd);
 227 +
 228 +       mutex_unlock(&state->lock);
 229 +
 230 +       return ret;
 231  }
 232
 233  static const struct v4l2_subdev_video_ops ov5647_subdev_video_ops = {
 234 @@ -503,38 +550,127 @@ static int ov5647_enum_mbus_code(struct
 235                                 struct v4l2_subdev_pad_config *cfg,
 236                                 struct v4l2_subdev_mbus_code_enum *code)
 237  {
 238 -       if (code->index > 0)
 239 +       if (code->index == 0 && ARRAY_SIZE(supported_modes_8bit))
 240 +               code->code = MEDIA_BUS_FMT_SBGGR8_1X8;
 241 +       else if (code->index == 0 && ARRAY_SIZE(supported_modes_8bit) == 0 &&
 242 +                ARRAY_SIZE(supported_modes_10bit))
 243 +               code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 244 +       else if (code->index == 1 && ARRAY_SIZE(supported_modes_8bit) &&
 245 +                ARRAY_SIZE(supported_modes_10bit))
 246 +               code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 247 +       else
 248                 return -EINVAL;
 249
 250 -       code->code = MEDIA_BUS_FMT_SBGGR8_1X8;
 251 +       return 0;
 252 +}
 253 +
 254 +static int ov5647_enum_frame_size(struct v4l2_subdev *sd,
 255 +                                 struct v4l2_subdev_pad_config *cfg,
 256 +                                 struct v4l2_subdev_frame_size_enum *fse)
 257 +{
 258 +       struct ov5647_mode *mode = NULL;
 259 +
 260 +       if (fse->code == MEDIA_BUS_FMT_SBGGR8_1X8) {
 261 +               if (fse->index >= ARRAY_SIZE(supported_modes_8bit))
 262 +                       return -EINVAL;
 263 +               mode = &supported_modes_8bit[fse->index];
 264 +       } else if (fse->code == MEDIA_BUS_FMT_SBGGR10_1X10) {
 265 +               if (fse->index >= ARRAY_SIZE(supported_modes_10bit))
 266 +                       return -EINVAL;
 267 +               mode = &supported_modes_10bit[fse->index];
 268 +       } else {
 269 +               return -EINVAL;
 270 +       }
 271 +
 272 +       fse->min_width = mode->format.width;
 273 +       fse->max_width = fse->min_width;
 274 +       fse->min_height = mode->format.height;
 275 +       fse->max_height = fse->min_height;
 276 +
 277 +       return 0;
 278 +}
 279 +
 280 +static int ov5647_set_fmt(struct v4l2_subdev *sd,
 281 +                         struct v4l2_subdev_pad_config *cfg,
 282 +                         struct v4l2_subdev_format *format)
 283 +{
 284 +       struct v4l2_mbus_framefmt *fmt = &format->format;
 285 +       struct ov5647 *state = to_state(sd);
 286 +       struct v4l2_mbus_framefmt *framefmt;
 287 +       const struct ov5647_mode *mode_8bit, *mode_10bit, *mode = NULL;
 288 +
 289 +       if (format->pad != 0)
 290 +               return -EINVAL;
 291 +
 292 +       mutex_lock(&state->lock);
 293 +
 294 +       /*
 295 +        * Try to respect any given pixel format, otherwise try for a 10-bit
 296 +        * mode.
 297 +        */
 298 +       mode_8bit = v4l2_find_nearest_size(supported_modes_8bit,
 299 +                                          ARRAY_SIZE(supported_modes_8bit),
 300 +                                          format.width, format.height,
 301 +                                          format->format.width,
 302 +                                          format->format.height);
 303 +       mode_10bit = v4l2_find_nearest_size(supported_modes_10bit,
 304 +                                           ARRAY_SIZE(supported_modes_10bit),
 305 +                                           format.width, format.height,
 306 +                                           format->format.width,
 307 +                                           format->format.height);
 308 +       if (format->format.code == MEDIA_BUS_FMT_SBGGR8_1X8 && mode_8bit)
 309 +               mode = mode_8bit;
 310 +       else if (format->format.code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
 311 +                mode_10bit)
 312 +               mode = mode_10bit;
 313 +       else if (mode_10bit)
 314 +               mode = mode_10bit;
 315 +       else
 316 +               mode = mode_8bit;
 317 +
 318 +       if (!mode)
 319 +               return -EINVAL;
 320 +
 321 +       *fmt = mode->format;
 322 +       if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 323 +               framefmt = v4l2_subdev_get_try_format(sd, cfg, format->pad);
 324 +               *framefmt = format->format;
 325 +       } else {
 326 +               state->mode = mode;
 327 +       }
 328 +
 329 +       mutex_unlock(&state->lock);
 330
 331         return 0;
 332  }
 333
 334 -static int ov5647_set_get_fmt(struct v4l2_subdev *sd,
 335 -                             struct v4l2_subdev_pad_config *cfg,
 336 -                             struct v4l2_subdev_format *format)
 337 +static int ov5647_get_fmt(struct v4l2_subdev *sd,
 338 +                         struct v4l2_subdev_pad_config *cfg,
 339 +                         struct v4l2_subdev_format *format)
 340  {
 341         struct v4l2_mbus_framefmt *fmt = &format->format;
 342 +       struct ov5647 *state = to_state(sd);
 343
 344         if (format->pad != 0)
 345                 return -EINVAL;
 346
 347 -       /* Only one format is supported, so return that */
 348 -       memset(fmt, 0, sizeof(*fmt));
 349 -       fmt->code = MEDIA_BUS_FMT_SBGGR8_1X8;
 350 -       fmt->colorspace = V4L2_COLORSPACE_SRGB;
 351 -       fmt->field = V4L2_FIELD_NONE;
 352 -       fmt->width = 640;
 353 -       fmt->height = 480;
 354 +       mutex_lock(&state->lock);
 355 +
 356 +       if (format->which == V4L2_SUBDEV_FORMAT_TRY)
 357 +               *fmt = *v4l2_subdev_get_try_format(sd, cfg, format->pad);
 358 +       else
 359 +               *fmt = state->mode->format;
 360 +
 361 +       mutex_unlock(&state->lock);
 362
 363         return 0;
 364  }
 365
 366  static const struct v4l2_subdev_pad_ops ov5647_subdev_pad_ops = {
 367         .enum_mbus_code = ov5647_enum_mbus_code,
 368 -       .set_fmt =        ov5647_set_get_fmt,
 369 -       .get_fmt =        ov5647_set_get_fmt,
 370 +       .set_fmt =        ov5647_set_fmt,
 371 +       .get_fmt =        ov5647_get_fmt,
 372 +       .enum_frame_size = ov5647_enum_frame_size,
 373  };
 374
 375  static const struct v4l2_subdev_ops ov5647_subdev_ops = {
 376 @@ -580,18 +716,15 @@ static int ov5647_open(struct v4l2_subde
 377                                 v4l2_subdev_get_try_format(sd, fh->pad, 0);
 378         struct v4l2_rect *crop =
 379                                 v4l2_subdev_get_try_crop(sd, fh->pad, 0);
 380 +       struct ov5647 *state = to_state(sd);
 381
 382         crop->left = OV5647_COLUMN_START_DEF;
 383         crop->top = OV5647_ROW_START_DEF;
 384         crop->width = OV5647_WINDOW_WIDTH_DEF;
 385         crop->height = OV5647_WINDOW_HEIGHT_DEF;
 386
 387 -       format->code = MEDIA_BUS_FMT_SBGGR8_1X8;
 388 -
 389 -       format->width = OV5647_WINDOW_WIDTH_DEF;
 390 -       format->height = OV5647_WINDOW_HEIGHT_DEF;
 391 -       format->field = V4L2_FIELD_NONE;
 392 -       format->colorspace = V4L2_COLORSPACE_SRGB;
 393 +       /* Set the default format to the same as the sensor. */
 394 +       *format = state->mode->format;
 395
 396         return 0;
 397  }
 398 @@ -660,6 +793,9 @@ static int ov5647_probe(struct i2c_clien
 399
 400         mutex_init(&sensor->lock);
 401
 402 +       /* Set the default mode before we init the subdev */
 403 +       sensor->mode = OV5647_DEFAULT_MODE;
 404 +
 405         sd = &sensor->sd;
 406         v4l2_i2c_subdev_init(sd, client, &ov5647_subdev_ops);
 407         sensor->sd.internal_ops = &ov5647_subdev_internal_ops;