drivers/media/i2c/mt9m001.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Driver for MT9M001 CMOS Image Sensor from Micron
   4  *
   5  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   6  */
   7
   8 #include <linux/clk.h>
   9 #include <linux/delay.h>
  10 #include <linux/gpio/consumer.h>
  11 #include <linux/i2c.h>
  12 #include <linux/log2.h>
  13 #include <linux/module.h>
  14 #include <linux/pm_runtime.h>
  15 #include <linux/slab.h>
  16 #include <linux/videodev2.h>
  17
  18 #include <media/v4l2-ctrls.h>
  19 #include <media/v4l2-device.h>
  20 #include <media/v4l2-event.h>
  21 #include <media/v4l2-subdev.h>
  22
  23 /*
  24  * mt9m001 i2c address 0x5d
  25  */
  26
  27 /* mt9m001 selected register addresses */
  28 #define MT9M001_CHIP_VERSION            0x00
  29 #define MT9M001_ROW_START               0x01
  30 #define MT9M001_COLUMN_START            0x02
  31 #define MT9M001_WINDOW_HEIGHT           0x03
  32 #define MT9M001_WINDOW_WIDTH            0x04
  33 #define MT9M001_HORIZONTAL_BLANKING     0x05
  34 #define MT9M001_VERTICAL_BLANKING       0x06
  35 #define MT9M001_OUTPUT_CONTROL          0x07
  36 #define MT9M001_SHUTTER_WIDTH           0x09
  37 #define MT9M001_FRAME_RESTART           0x0b
  38 #define MT9M001_SHUTTER_DELAY           0x0c
  39 #define MT9M001_RESET                   0x0d
  40 #define MT9M001_READ_OPTIONS1           0x1e
  41 #define MT9M001_READ_OPTIONS2           0x20
  42 #define MT9M001_GLOBAL_GAIN             0x35
  43 #define MT9M001_CHIP_ENABLE             0xF1
  44
  45 #define MT9M001_MAX_WIDTH               1280
  46 #define MT9M001_MAX_HEIGHT              1024
  47 #define MT9M001_MIN_WIDTH               48
  48 #define MT9M001_MIN_HEIGHT              32
  49 #define MT9M001_COLUMN_SKIP             20
  50 #define MT9M001_ROW_SKIP                12
  51 #define MT9M001_DEFAULT_HBLANK          9
  52 #define MT9M001_DEFAULT_VBLANK          25
  53
  54 /* MT9M001 has only one fixed colorspace per pixelcode */
  55 struct mt9m001_datafmt {
  56         u32     code;
  57         enum v4l2_colorspace            colorspace;
  58 };
  59
  60 /* Find a data format by a pixel code in an array */
  61 static const struct mt9m001_datafmt *mt9m001_find_datafmt(
  62         u32 code, const struct mt9m001_datafmt *fmt,
  63         int n)
  64 {
  65         int i;
  66         for (i = 0; i < n; i++)
  67                 if (fmt[i].code == code)
  68                         return fmt + i;
  69
  70         return NULL;
  71 }
  72
  73 static const struct mt9m001_datafmt mt9m001_colour_fmts[] = {
  74         /*
  75          * Order important: first natively supported,
  76          * second supported with a GPIO extender
  77          */
  78         {MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_COLORSPACE_SRGB},
  79         {MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
  80 };
  81
  82 static const struct mt9m001_datafmt mt9m001_monochrome_fmts[] = {
  83         /* Order important - see above */
  84         {MEDIA_BUS_FMT_Y10_1X10, V4L2_COLORSPACE_JPEG},
  85         {MEDIA_BUS_FMT_Y8_1X8, V4L2_COLORSPACE_JPEG},
  86 };
  87
  88 struct mt9m001 {
  89         struct v4l2_subdev subdev;
  90         struct v4l2_ctrl_handler hdl;
  91         struct {
  92                 /* exposure/auto-exposure cluster */
  93                 struct v4l2_ctrl *autoexposure;
  94                 struct v4l2_ctrl *exposure;
  95         };
  96         bool streaming;
  97         struct mutex mutex;
  98         struct v4l2_rect rect;  /* Sensor window */
  99         struct clk *clk;
 100         struct gpio_desc *standby_gpio;
 101         struct gpio_desc *reset_gpio;
 102         const struct mt9m001_datafmt *fmt;
 103         const struct mt9m001_datafmt *fmts;
 104         int num_fmts;
 105         unsigned int total_h;
 106         unsigned short y_skip_top;      /* Lines to skip at the top */
 107         struct media_pad pad;
 108 };
 109
 110 static struct mt9m001 *to_mt9m001(const struct i2c_client *client)
 111 {
 112         return container_of(i2c_get_clientdata(client), struct mt9m001, subdev);
 113 }
 114
 115 static int reg_read(struct i2c_client *client, const u8 reg)
 116 {
 117         return i2c_smbus_read_word_swapped(client, reg);
 118 }
 119
 120 static int reg_write(struct i2c_client *client, const u8 reg,
 121                      const u16 data)
 122 {
 123         return i2c_smbus_write_word_swapped(client, reg, data);
 124 }
 125
 126 static int reg_set(struct i2c_client *client, const u8 reg,
 127                    const u16 data)
 128 {
 129         int ret;
 130
 131         ret = reg_read(client, reg);
 132         if (ret < 0)
 133                 return ret;
 134         return reg_write(client, reg, ret | data);
 135 }
 136
 137 static int reg_clear(struct i2c_client *client, const u8 reg,
 138                      const u16 data)
 139 {
 140         int ret;
 141
 142         ret = reg_read(client, reg);
 143         if (ret < 0)
 144                 return ret;
 145         return reg_write(client, reg, ret & ~data);
 146 }
 147
 148 struct mt9m001_reg {
 149         u8 reg;
 150         u16 data;
 151 };
 152
 153 static int multi_reg_write(struct i2c_client *client,
 154                            const struct mt9m001_reg *regs, int num)
 155 {
 156         int i;
 157
 158         for (i = 0; i < num; i++) {
 159                 int ret = reg_write(client, regs[i].reg, regs[i].data);
 160
 161                 if (ret)
 162                         return ret;
 163         }
 164
 165         return 0;
 166 }
 167
 168 static int mt9m001_init(struct i2c_client *client)
 169 {
 170         const struct mt9m001_reg init_regs[] = {
 171                 /*
 172                  * Issue a soft reset. This returns all registers to their
 173                  * default values.
 174                  */
 175                 { MT9M001_RESET, 1 },
 176                 { MT9M001_RESET, 0 },
 177                 /* Disable chip, synchronous option update */
 178                 { MT9M001_OUTPUT_CONTROL, 0 }
 179         };
 180
 181         dev_dbg(&client->dev, "%s\n", __func__);
 182
 183         return multi_reg_write(client, init_regs, ARRAY_SIZE(init_regs));
 184 }
 185
 186 static int mt9m001_apply_selection(struct v4l2_subdev *sd)
 187 {
 188         struct i2c_client *client = v4l2_get_subdevdata(sd);
 189         struct mt9m001 *mt9m001 = to_mt9m001(client);
 190         const struct mt9m001_reg regs[] = {
 191                 /* Blanking and start values - default... */
 192                 { MT9M001_HORIZONTAL_BLANKING, MT9M001_DEFAULT_HBLANK },
 193                 { MT9M001_VERTICAL_BLANKING, MT9M001_DEFAULT_VBLANK },
 194                 /*
 195                  * The caller provides a supported format, as verified per
 196                  * call to .set_fmt(FORMAT_TRY).
 197                  */
 198                 { MT9M001_COLUMN_START, mt9m001->rect.left },
 199                 { MT9M001_ROW_START, mt9m001->rect.top },
 200                 { MT9M001_WINDOW_WIDTH, mt9m001->rect.width - 1 },
 201                 { MT9M001_WINDOW_HEIGHT,
 202                         mt9m001->rect.height + mt9m001->y_skip_top - 1 },
 203         };
 204
 205         return multi_reg_write(client, regs, ARRAY_SIZE(regs));
 206 }
 207
 208 static int mt9m001_s_stream(struct v4l2_subdev *sd, int enable)
 209 {
 210         struct i2c_client *client = v4l2_get_subdevdata(sd);
 211         struct mt9m001 *mt9m001 = to_mt9m001(client);
 212         int ret = 0;
 213
 214         mutex_lock(&mt9m001->mutex);
 215
 216         if (mt9m001->streaming == enable)
 217                 goto done;
 218
 219         if (enable) {
 220                 ret = pm_runtime_get_sync(&client->dev);
 221                 if (ret < 0)
 222                         goto put_unlock;
 223
 224                 ret = mt9m001_apply_selection(sd);
 225                 if (ret)
 226                         goto put_unlock;
 227
 228                 ret = __v4l2_ctrl_handler_setup(&mt9m001->hdl);
 229                 if (ret)
 230                         goto put_unlock;
 231
 232                 /* Switch to master "normal" mode */
 233                 ret = reg_write(client, MT9M001_OUTPUT_CONTROL, 2);
 234                 if (ret < 0)
 235                         goto put_unlock;
 236         } else {
 237                 /* Switch to master stop sensor readout */
 238                 reg_write(client, MT9M001_OUTPUT_CONTROL, 0);
 239                 pm_runtime_put(&client->dev);
 240         }
 241
 242         mt9m001->streaming = enable;
 243 done:
 244         mutex_unlock(&mt9m001->mutex);
 245
 246         return 0;
 247
 248 put_unlock:
 249         pm_runtime_put(&client->dev);
 250         mutex_unlock(&mt9m001->mutex);
 251
 252         return ret;
 253 }
 254
 255 static int mt9m001_set_selection(struct v4l2_subdev *sd,
 256                 struct v4l2_subdev_pad_config *cfg,
 257                 struct v4l2_subdev_selection *sel)
 258 {
 259         struct i2c_client *client = v4l2_get_subdevdata(sd);
 260         struct mt9m001 *mt9m001 = to_mt9m001(client);
 261         struct v4l2_rect rect = sel->r;
 262
 263         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
 264             sel->target != V4L2_SEL_TGT_CROP)
 265                 return -EINVAL;
 266
 267         if (mt9m001->fmts == mt9m001_colour_fmts)
 268                 /*
 269                  * Bayer format - even number of rows for simplicity,
 270                  * but let the user play with the top row.
 271                  */
 272                 rect.height = ALIGN(rect.height, 2);
 273
 274         /* Datasheet requirement: see register description */
 275         rect.width = ALIGN(rect.width, 2);
 276         rect.left = ALIGN(rect.left, 2);
 277
 278         rect.width = clamp_t(u32, rect.width, MT9M001_MIN_WIDTH,
 279                         MT9M001_MAX_WIDTH);
 280         rect.left = clamp_t(u32, rect.left, MT9M001_COLUMN_SKIP,
 281                         MT9M001_COLUMN_SKIP + MT9M001_MAX_WIDTH - rect.width);
 282
 283         rect.height = clamp_t(u32, rect.height, MT9M001_MIN_HEIGHT,
 284                         MT9M001_MAX_HEIGHT);
 285         rect.top = clamp_t(u32, rect.top, MT9M001_ROW_SKIP,
 286                         MT9M001_ROW_SKIP + MT9M001_MAX_HEIGHT - rect.height);
 287
 288         mt9m001->total_h = rect.height + mt9m001->y_skip_top +
 289                            MT9M001_DEFAULT_VBLANK;
 290
 291         mt9m001->rect = rect;
 292
 293         return 0;
 294 }
 295
 296 static int mt9m001_get_selection(struct v4l2_subdev *sd,
 297                 struct v4l2_subdev_pad_config *cfg,
 298                 struct v4l2_subdev_selection *sel)
 299 {
 300         struct i2c_client *client = v4l2_get_subdevdata(sd);
 301         struct mt9m001 *mt9m001 = to_mt9m001(client);
 302
 303         if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 304                 return -EINVAL;
 305
 306         switch (sel->target) {
 307         case V4L2_SEL_TGT_CROP_BOUNDS:
 308                 sel->r.left = MT9M001_COLUMN_SKIP;
 309                 sel->r.top = MT9M001_ROW_SKIP;
 310                 sel->r.width = MT9M001_MAX_WIDTH;
 311                 sel->r.height = MT9M001_MAX_HEIGHT;
 312                 return 0;
 313         case V4L2_SEL_TGT_CROP:
 314                 sel->r = mt9m001->rect;
 315                 return 0;
 316         default:
 317                 return -EINVAL;
 318         }
 319 }
 320
 321 static int mt9m001_get_fmt(struct v4l2_subdev *sd,
 322                 struct v4l2_subdev_pad_config *cfg,
 323                 struct v4l2_subdev_format *format)
 324 {
 325         struct i2c_client *client = v4l2_get_subdevdata(sd);
 326         struct mt9m001 *mt9m001 = to_mt9m001(client);
 327         struct v4l2_mbus_framefmt *mf = &format->format;
 328
 329         if (format->pad)
 330                 return -EINVAL;
 331
 332         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
 333                 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
 334                 format->format = *mf;
 335                 return 0;
 336         }
 337
 338         mf->width       = mt9m001->rect.width;
 339         mf->height      = mt9m001->rect.height;
 340         mf->code        = mt9m001->fmt->code;
 341         mf->colorspace  = mt9m001->fmt->colorspace;
 342         mf->field       = V4L2_FIELD_NONE;
 343         mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
 344         mf->quantization = V4L2_QUANTIZATION_DEFAULT;
 345         mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
 346
 347         return 0;
 348 }
 349
 350 static int mt9m001_s_fmt(struct v4l2_subdev *sd,
 351                          const struct mt9m001_datafmt *fmt,
 352                          struct v4l2_mbus_framefmt *mf)
 353 {
 354         struct i2c_client *client = v4l2_get_subdevdata(sd);
 355         struct mt9m001 *mt9m001 = to_mt9m001(client);
 356         struct v4l2_subdev_selection sel = {
 357                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 358                 .target = V4L2_SEL_TGT_CROP,
 359                 .r.left = mt9m001->rect.left,
 360                 .r.top = mt9m001->rect.top,
 361                 .r.width = mf->width,
 362                 .r.height = mf->height,
 363         };
 364         int ret;
 365
 366         /* No support for scaling so far, just crop. TODO: use skipping */
 367         ret = mt9m001_set_selection(sd, NULL, &sel);
 368         if (!ret) {
 369                 mf->width       = mt9m001->rect.width;
 370                 mf->height      = mt9m001->rect.height;
 371                 mt9m001->fmt    = fmt;
 372                 mf->colorspace  = fmt->colorspace;
 373         }
 374
 375         return ret;
 376 }
 377
 378 static int mt9m001_set_fmt(struct v4l2_subdev *sd,
 379                 struct v4l2_subdev_pad_config *cfg,
 380                 struct v4l2_subdev_format *format)
 381 {
 382         struct v4l2_mbus_framefmt *mf = &format->format;
 383         struct i2c_client *client = v4l2_get_subdevdata(sd);
 384         struct mt9m001 *mt9m001 = to_mt9m001(client);
 385         const struct mt9m001_datafmt *fmt;
 386
 387         if (format->pad)
 388                 return -EINVAL;
 389
 390         v4l_bound_align_image(&mf->width, MT9M001_MIN_WIDTH,
 391                 MT9M001_MAX_WIDTH, 1,
 392                 &mf->height, MT9M001_MIN_HEIGHT + mt9m001->y_skip_top,
 393                 MT9M001_MAX_HEIGHT + mt9m001->y_skip_top, 0, 0);
 394
 395         if (mt9m001->fmts == mt9m001_colour_fmts)
 396                 mf->height = ALIGN(mf->height - 1, 2);
 397
 398         fmt = mt9m001_find_datafmt(mf->code, mt9m001->fmts,
 399                                    mt9m001->num_fmts);
 400         if (!fmt) {
 401                 fmt = mt9m001->fmt;
 402                 mf->code = fmt->code;
 403         }
 404
 405         mf->colorspace  = fmt->colorspace;
 406         mf->field       = V4L2_FIELD_NONE;
 407         mf->ycbcr_enc   = V4L2_YCBCR_ENC_DEFAULT;
 408         mf->quantization = V4L2_QUANTIZATION_DEFAULT;
 409         mf->xfer_func   = V4L2_XFER_FUNC_DEFAULT;
 410
 411         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 412                 return mt9m001_s_fmt(sd, fmt, mf);
 413         cfg->try_fmt = *mf;
 414         return 0;
 415 }
 416
 417 #ifdef CONFIG_VIDEO_ADV_DEBUG
 418 static int mt9m001_g_register(struct v4l2_subdev *sd,
 419                               struct v4l2_dbg_register *reg)
 420 {
 421         struct i2c_client *client = v4l2_get_subdevdata(sd);
 422
 423         if (reg->reg > 0xff)
 424                 return -EINVAL;
 425
 426         reg->size = 2;
 427         reg->val = reg_read(client, reg->reg);
 428
 429         if (reg->val > 0xffff)
 430                 return -EIO;
 431
 432         return 0;
 433 }
 434
 435 static int mt9m001_s_register(struct v4l2_subdev *sd,
 436                               const struct v4l2_dbg_register *reg)
 437 {
 438         struct i2c_client *client = v4l2_get_subdevdata(sd);
 439
 440         if (reg->reg > 0xff)
 441                 return -EINVAL;
 442
 443         if (reg_write(client, reg->reg, reg->val) < 0)
 444                 return -EIO;
 445
 446         return 0;
 447 }
 448 #endif
 449
 450 static int mt9m001_power_on(struct device *dev)
 451 {
 452         struct i2c_client *client = to_i2c_client(dev);
 453         struct mt9m001 *mt9m001 = to_mt9m001(client);
 454         int ret;
 455
 456         ret = clk_prepare_enable(mt9m001->clk);
 457         if (ret)
 458                 return ret;
 459
 460         if (mt9m001->standby_gpio) {
 461                 gpiod_set_value_cansleep(mt9m001->standby_gpio, 0);
 462                 usleep_range(1000, 2000);
 463         }
 464
 465         if (mt9m001->reset_gpio) {
 466                 gpiod_set_value_cansleep(mt9m001->reset_gpio, 1);
 467                 usleep_range(1000, 2000);
 468                 gpiod_set_value_cansleep(mt9m001->reset_gpio, 0);
 469                 usleep_range(1000, 2000);
 470         }
 471
 472         return 0;
 473 }
 474
 475 static int mt9m001_power_off(struct device *dev)
 476 {
 477         struct i2c_client *client = to_i2c_client(dev);
 478         struct mt9m001 *mt9m001 = to_mt9m001(client);
 479
 480         gpiod_set_value_cansleep(mt9m001->standby_gpio, 1);
 481         clk_disable_unprepare(mt9m001->clk);
 482
 483         return 0;
 484 }
 485
 486 static int mt9m001_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 487 {
 488         struct mt9m001 *mt9m001 = container_of(ctrl->handler,
 489                                                struct mt9m001, hdl);
 490         s32 min, max;
 491
 492         switch (ctrl->id) {
 493         case V4L2_CID_EXPOSURE_AUTO:
 494                 min = mt9m001->exposure->minimum;
 495                 max = mt9m001->exposure->maximum;
 496                 mt9m001->exposure->val =
 497                         (524 + (mt9m001->total_h - 1) * (max - min)) / 1048 + min;
 498                 break;
 499         }
 500         return 0;
 501 }
 502
 503 static int mt9m001_s_ctrl(struct v4l2_ctrl *ctrl)
 504 {
 505         struct mt9m001 *mt9m001 = container_of(ctrl->handler,
 506                                                struct mt9m001, hdl);
 507         struct v4l2_subdev *sd = &mt9m001->subdev;
 508         struct i2c_client *client = v4l2_get_subdevdata(sd);
 509         struct v4l2_ctrl *exp = mt9m001->exposure;
 510         int data;
 511         int ret;
 512
 513         if (!pm_runtime_get_if_in_use(&client->dev))
 514                 return 0;
 515
 516         switch (ctrl->id) {
 517         case V4L2_CID_VFLIP:
 518                 if (ctrl->val)
 519                         ret = reg_set(client, MT9M001_READ_OPTIONS2, 0x8000);
 520                 else
 521                         ret = reg_clear(client, MT9M001_READ_OPTIONS2, 0x8000);
 522                 break;
 523
 524         case V4L2_CID_GAIN:
 525                 /* See Datasheet Table 7, Gain settings. */
 526                 if (ctrl->val <= ctrl->default_value) {
 527                         /* Pack it into 0..1 step 0.125, register values 0..8 */
 528                         unsigned long range = ctrl->default_value - ctrl->minimum;
 529                         data = ((ctrl->val - (s32)ctrl->minimum) * 8 + range / 2) / range;
 530
 531                         dev_dbg(&client->dev, "Setting gain %d\n", data);
 532                         ret = reg_write(client, MT9M001_GLOBAL_GAIN, data);
 533                 } else {
 534                         /* Pack it into 1.125..15 variable step, register values 9..67 */
 535                         /* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
 536                         unsigned long range = ctrl->maximum - ctrl->default_value - 1;
 537                         unsigned long gain = ((ctrl->val - (s32)ctrl->default_value - 1) *
 538                                                111 + range / 2) / range + 9;
 539
 540                         if (gain <= 32)
 541                                 data = gain;
 542                         else if (gain <= 64)
 543                                 data = ((gain - 32) * 16 + 16) / 32 + 80;
 544                         else
 545                                 data = ((gain - 64) * 7 + 28) / 56 + 96;
 546
 547                         dev_dbg(&client->dev, "Setting gain from %d to %d\n",
 548                                  reg_read(client, MT9M001_GLOBAL_GAIN), data);
 549                         ret = reg_write(client, MT9M001_GLOBAL_GAIN, data);
 550                 }
 551                 break;
 552
 553         case V4L2_CID_EXPOSURE_AUTO:
 554                 if (ctrl->val == V4L2_EXPOSURE_MANUAL) {
 555                         unsigned long range = exp->maximum - exp->minimum;
 556                         unsigned long shutter = ((exp->val - (s32)exp->minimum) * 1048 +
 557                                                  range / 2) / range + 1;
 558
 559                         dev_dbg(&client->dev,
 560                                 "Setting shutter width from %d to %lu\n",
 561                                 reg_read(client, MT9M001_SHUTTER_WIDTH), shutter);
 562                         ret = reg_write(client, MT9M001_SHUTTER_WIDTH, shutter);
 563                 } else {
 564                         mt9m001->total_h = mt9m001->rect.height +
 565                                 mt9m001->y_skip_top + MT9M001_DEFAULT_VBLANK;
 566                         ret = reg_write(client, MT9M001_SHUTTER_WIDTH,
 567                                         mt9m001->total_h);
 568                 }
 569                 break;
 570         default:
 571                 ret = -EINVAL;
 572                 break;
 573         }
 574
 575         pm_runtime_put(&client->dev);
 576
 577         return ret;
 578 }
 579
 580 /*
 581  * Interface active, can use i2c. If it fails, it can indeed mean, that
 582  * this wasn't our capture interface, so, we wait for the right one
 583  */
 584 static int mt9m001_video_probe(struct i2c_client *client)
 585 {
 586         struct mt9m001 *mt9m001 = to_mt9m001(client);
 587         s32 data;
 588         int ret;
 589
 590         /* Enable the chip */
 591         data = reg_write(client, MT9M001_CHIP_ENABLE, 1);
 592         dev_dbg(&client->dev, "write: %d\n", data);
 593
 594         /* Read out the chip version register */
 595         data = reg_read(client, MT9M001_CHIP_VERSION);
 596
 597         /* must be 0x8411 or 0x8421 for colour sensor and 8431 for bw */
 598         switch (data) {
 599         case 0x8411:
 600         case 0x8421:
 601                 mt9m001->fmts = mt9m001_colour_fmts;
 602                 mt9m001->num_fmts = ARRAY_SIZE(mt9m001_colour_fmts);
 603                 break;
 604         case 0x8431:
 605                 mt9m001->fmts = mt9m001_monochrome_fmts;
 606                 mt9m001->num_fmts = ARRAY_SIZE(mt9m001_monochrome_fmts);
 607                 break;
 608         default:
 609                 dev_err(&client->dev,
 610                         "No MT9M001 chip detected, register read %x\n", data);
 611                 ret = -ENODEV;
 612                 goto done;
 613         }
 614
 615         mt9m001->fmt = &mt9m001->fmts[0];
 616
 617         dev_info(&client->dev, "Detected a MT9M001 chip ID %x (%s)\n", data,
 618                  data == 0x8431 ? "C12STM" : "C12ST");
 619
 620         ret = mt9m001_init(client);
 621         if (ret < 0) {
 622                 dev_err(&client->dev, "Failed to initialise the camera\n");
 623                 goto done;
 624         }
 625
 626         /* mt9m001_init() has reset the chip, returning registers to defaults */
 627         ret = v4l2_ctrl_handler_setup(&mt9m001->hdl);
 628
 629 done:
 630         return ret;
 631 }
 632
 633 static int mt9m001_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
 634 {
 635         struct i2c_client *client = v4l2_get_subdevdata(sd);
 636         struct mt9m001 *mt9m001 = to_mt9m001(client);
 637
 638         *lines = mt9m001->y_skip_top;
 639
 640         return 0;
 641 }
 642
 643 static const struct v4l2_ctrl_ops mt9m001_ctrl_ops = {
 644         .g_volatile_ctrl = mt9m001_g_volatile_ctrl,
 645         .s_ctrl = mt9m001_s_ctrl,
 646 };
 647
 648 static const struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
 649         .log_status = v4l2_ctrl_subdev_log_status,
 650         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 651         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 652 #ifdef CONFIG_VIDEO_ADV_DEBUG
 653         .g_register     = mt9m001_g_register,
 654         .s_register     = mt9m001_s_register,
 655 #endif
 656 };
 657
 658 static int mt9m001_init_cfg(struct v4l2_subdev *sd,
 659                             struct v4l2_subdev_pad_config *cfg)
 660 {
 661         struct i2c_client *client = v4l2_get_subdevdata(sd);
 662         struct mt9m001 *mt9m001 = to_mt9m001(client);
 663         struct v4l2_mbus_framefmt *try_fmt =
 664                 v4l2_subdev_get_try_format(sd, cfg, 0);
 665
 666         try_fmt->width          = MT9M001_MAX_WIDTH;
 667         try_fmt->height         = MT9M001_MAX_HEIGHT;
 668         try_fmt->code           = mt9m001->fmts[0].code;
 669         try_fmt->colorspace     = mt9m001->fmts[0].colorspace;
 670         try_fmt->field          = V4L2_FIELD_NONE;
 671         try_fmt->ycbcr_enc      = V4L2_YCBCR_ENC_DEFAULT;
 672         try_fmt->quantization   = V4L2_QUANTIZATION_DEFAULT;
 673         try_fmt->xfer_func      = V4L2_XFER_FUNC_DEFAULT;
 674
 675         return 0;
 676 }
 677
 678 static int mt9m001_enum_mbus_code(struct v4l2_subdev *sd,
 679                 struct v4l2_subdev_pad_config *cfg,
 680                 struct v4l2_subdev_mbus_code_enum *code)
 681 {
 682         struct i2c_client *client = v4l2_get_subdevdata(sd);
 683         struct mt9m001 *mt9m001 = to_mt9m001(client);
 684
 685         if (code->pad || code->index >= mt9m001->num_fmts)
 686                 return -EINVAL;
 687
 688         code->code = mt9m001->fmts[code->index].code;
 689         return 0;
 690 }
 691
 692 static int mt9m001_g_mbus_config(struct v4l2_subdev *sd,
 693                                 struct v4l2_mbus_config *cfg)
 694 {
 695         /* MT9M001 has all capture_format parameters fixed */
 696         cfg->flags = V4L2_MBUS_PCLK_SAMPLE_FALLING |
 697                 V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 698                 V4L2_MBUS_DATA_ACTIVE_HIGH | V4L2_MBUS_MASTER;
 699         cfg->type = V4L2_MBUS_PARALLEL;
 700
 701         return 0;
 702 }
 703
 704 static const struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
 705         .s_stream       = mt9m001_s_stream,
 706         .g_mbus_config  = mt9m001_g_mbus_config,
 707 };
 708
 709 static const struct v4l2_subdev_sensor_ops mt9m001_subdev_sensor_ops = {
 710         .g_skip_top_lines       = mt9m001_g_skip_top_lines,
 711 };
 712
 713 static const struct v4l2_subdev_pad_ops mt9m001_subdev_pad_ops = {
 714         .init_cfg       = mt9m001_init_cfg,
 715         .enum_mbus_code = mt9m001_enum_mbus_code,
 716         .get_selection  = mt9m001_get_selection,
 717         .set_selection  = mt9m001_set_selection,
 718         .get_fmt        = mt9m001_get_fmt,
 719         .set_fmt        = mt9m001_set_fmt,
 720 };
 721
 722 static const struct v4l2_subdev_ops mt9m001_subdev_ops = {
 723         .core   = &mt9m001_subdev_core_ops,
 724         .video  = &mt9m001_subdev_video_ops,
 725         .sensor = &mt9m001_subdev_sensor_ops,
 726         .pad    = &mt9m001_subdev_pad_ops,
 727 };
 728
 729 static int mt9m001_probe(struct i2c_client *client,
 730                          const struct i2c_device_id *did)
 731 {
 732         struct mt9m001 *mt9m001;
 733         struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
 734         int ret;
 735
 736         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 737                 dev_warn(&adapter->dev,
 738                          "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
 739                 return -EIO;
 740         }
 741
 742         mt9m001 = devm_kzalloc(&client->dev, sizeof(*mt9m001), GFP_KERNEL);
 743         if (!mt9m001)
 744                 return -ENOMEM;
 745
 746         mt9m001->clk = devm_clk_get(&client->dev, NULL);
 747         if (IS_ERR(mt9m001->clk))
 748                 return PTR_ERR(mt9m001->clk);
 749
 750         mt9m001->standby_gpio = devm_gpiod_get_optional(&client->dev, "standby",
 751                                                         GPIOD_OUT_LOW);
 752         if (IS_ERR(mt9m001->standby_gpio))
 753                 return PTR_ERR(mt9m001->standby_gpio);
 754
 755         mt9m001->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
 756                                                       GPIOD_OUT_LOW);
 757         if (IS_ERR(mt9m001->reset_gpio))
 758                 return PTR_ERR(mt9m001->reset_gpio);
 759
 760         v4l2_i2c_subdev_init(&mt9m001->subdev, client, &mt9m001_subdev_ops);
 761         mt9m001->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
 762                                  V4L2_SUBDEV_FL_HAS_EVENTS;
 763         v4l2_ctrl_handler_init(&mt9m001->hdl, 4);
 764         v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
 765                         V4L2_CID_VFLIP, 0, 1, 1, 0);
 766         v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
 767                         V4L2_CID_GAIN, 0, 127, 1, 64);
 768         mt9m001->exposure = v4l2_ctrl_new_std(&mt9m001->hdl, &mt9m001_ctrl_ops,
 769                         V4L2_CID_EXPOSURE, 1, 255, 1, 255);
 770         /*
 771          * Simulated autoexposure. If enabled, we calculate shutter width
 772          * ourselves in the driver based on vertical blanking and frame width
 773          */
 774         mt9m001->autoexposure = v4l2_ctrl_new_std_menu(&mt9m001->hdl,
 775                         &mt9m001_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
 776                         V4L2_EXPOSURE_AUTO);
 777         mt9m001->subdev.ctrl_handler = &mt9m001->hdl;
 778         if (mt9m001->hdl.error)
 779                 return mt9m001->hdl.error;
 780
 781         v4l2_ctrl_auto_cluster(2, &mt9m001->autoexposure,
 782                                         V4L2_EXPOSURE_MANUAL, true);
 783
 784         mutex_init(&mt9m001->mutex);
 785         mt9m001->hdl.lock = &mt9m001->mutex;
 786
 787         /* Second stage probe - when a capture adapter is there */
 788         mt9m001->y_skip_top     = 0;
 789         mt9m001->rect.left      = MT9M001_COLUMN_SKIP;
 790         mt9m001->rect.top       = MT9M001_ROW_SKIP;
 791         mt9m001->rect.width     = MT9M001_MAX_WIDTH;
 792         mt9m001->rect.height    = MT9M001_MAX_HEIGHT;
 793
 794         ret = mt9m001_power_on(&client->dev);
 795         if (ret)
 796                 goto error_hdl_free;
 797
 798         pm_runtime_set_active(&client->dev);
 799         pm_runtime_enable(&client->dev);
 800
 801         ret = mt9m001_video_probe(client);
 802         if (ret)
 803                 goto error_power_off;
 804
 805         mt9m001->pad.flags = MEDIA_PAD_FL_SOURCE;
 806         mt9m001->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 807         ret = media_entity_pads_init(&mt9m001->subdev.entity, 1, &mt9m001->pad);
 808         if (ret)
 809                 goto error_power_off;
 810
 811         ret = v4l2_async_register_subdev(&mt9m001->subdev);
 812         if (ret)
 813                 goto error_entity_cleanup;
 814
 815         pm_runtime_idle(&client->dev);
 816
 817         return 0;
 818
 819 error_entity_cleanup:
 820         media_entity_cleanup(&mt9m001->subdev.entity);
 821 error_power_off:
 822         pm_runtime_disable(&client->dev);
 823         pm_runtime_set_suspended(&client->dev);
 824         mt9m001_power_off(&client->dev);
 825
 826 error_hdl_free:
 827         v4l2_ctrl_handler_free(&mt9m001->hdl);
 828         mutex_destroy(&mt9m001->mutex);
 829
 830         return ret;
 831 }
 832
 833 static int mt9m001_remove(struct i2c_client *client)
 834 {
 835         struct mt9m001 *mt9m001 = to_mt9m001(client);
 836
 837         pm_runtime_get_sync(&client->dev);
 838
 839         v4l2_async_unregister_subdev(&mt9m001->subdev);
 840         media_entity_cleanup(&mt9m001->subdev.entity);
 841
 842         pm_runtime_disable(&client->dev);
 843         pm_runtime_set_suspended(&client->dev);
 844         pm_runtime_put_noidle(&client->dev);
 845         mt9m001_power_off(&client->dev);
 846
 847         v4l2_ctrl_handler_free(&mt9m001->hdl);
 848         mutex_destroy(&mt9m001->mutex);
 849
 850         return 0;
 851 }
 852
 853 static const struct i2c_device_id mt9m001_id[] = {
 854         { "mt9m001", 0 },
 855         { }
 856 };
 857 MODULE_DEVICE_TABLE(i2c, mt9m001_id);
 858
 859 static const struct dev_pm_ops mt9m001_pm_ops = {
 860         SET_RUNTIME_PM_OPS(mt9m001_power_off, mt9m001_power_on, NULL)
 861 };
 862
 863 static const struct of_device_id mt9m001_of_match[] = {
 864         { .compatible = "onnn,mt9m001", },
 865         { /* sentinel */ },
 866 };
 867 MODULE_DEVICE_TABLE(of, mt9m001_of_match);
 868
 869 static struct i2c_driver mt9m001_i2c_driver = {
 870         .driver = {
 871                 .name = "mt9m001",
 872                 .pm = &mt9m001_pm_ops,
 873                 .of_match_table = mt9m001_of_match,
 874         },
 875         .probe          = mt9m001_probe,
 876         .remove         = mt9m001_remove,
 877         .id_table       = mt9m001_id,
 878 };
 879
 880 module_i2c_driver(mt9m001_i2c_driver);
 881
 882 MODULE_DESCRIPTION("Micron MT9M001 Camera driver");
 883 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
 884 MODULE_LICENSE("GPL v2");