Back to home page

OSCL-LXR

 
 

    


0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Driver for MT9M032 CMOS Image Sensor from Micron
0004  *
0005  * Copyright (C) 2010-2011 Lund Engineering
0006  * Contact: Gil Lund <gwlund@lundeng.com>
0007  * Author: Martin Hostettler <martin@neutronstar.dyndns.org>
0008  */
0009 
0010 #include <linux/delay.h>
0011 #include <linux/i2c.h>
0012 #include <linux/init.h>
0013 #include <linux/kernel.h>
0014 #include <linux/math64.h>
0015 #include <linux/module.h>
0016 #include <linux/mutex.h>
0017 #include <linux/slab.h>
0018 #include <linux/v4l2-mediabus.h>
0019 
0020 #include <media/media-entity.h>
0021 #include <media/i2c/mt9m032.h>
0022 #include <media/v4l2-ctrls.h>
0023 #include <media/v4l2-device.h>
0024 #include <media/v4l2-subdev.h>
0025 
0026 #include "aptina-pll.h"
0027 
0028 /*
0029  * width and height include active boundary and black parts
0030  *
0031  * column    0-  15 active boundary
0032  * column   16-1455 image
0033  * column 1456-1471 active boundary
0034  * column 1472-1599 black
0035  *
0036  * row       0-  51 black
0037  * row      53-  59 active boundary
0038  * row      60-1139 image
0039  * row    1140-1147 active boundary
0040  * row    1148-1151 black
0041  */
0042 
0043 #define MT9M032_PIXEL_ARRAY_WIDTH           1600
0044 #define MT9M032_PIXEL_ARRAY_HEIGHT          1152
0045 
0046 #define MT9M032_CHIP_VERSION                0x00
0047 #define     MT9M032_CHIP_VERSION_VALUE      0x1402
0048 #define MT9M032_ROW_START               0x01
0049 #define     MT9M032_ROW_START_MIN           0
0050 #define     MT9M032_ROW_START_MAX           1152
0051 #define     MT9M032_ROW_START_DEF           60
0052 #define MT9M032_COLUMN_START                0x02
0053 #define     MT9M032_COLUMN_START_MIN        0
0054 #define     MT9M032_COLUMN_START_MAX        1600
0055 #define     MT9M032_COLUMN_START_DEF        16
0056 #define MT9M032_ROW_SIZE                0x03
0057 #define     MT9M032_ROW_SIZE_MIN            32
0058 #define     MT9M032_ROW_SIZE_MAX            1152
0059 #define     MT9M032_ROW_SIZE_DEF            1080
0060 #define MT9M032_COLUMN_SIZE             0x04
0061 #define     MT9M032_COLUMN_SIZE_MIN         32
0062 #define     MT9M032_COLUMN_SIZE_MAX         1600
0063 #define     MT9M032_COLUMN_SIZE_DEF         1440
0064 #define MT9M032_HBLANK                  0x05
0065 #define MT9M032_VBLANK                  0x06
0066 #define     MT9M032_VBLANK_MAX          0x7ff
0067 #define MT9M032_SHUTTER_WIDTH_HIGH          0x08
0068 #define MT9M032_SHUTTER_WIDTH_LOW           0x09
0069 #define     MT9M032_SHUTTER_WIDTH_MIN       1
0070 #define     MT9M032_SHUTTER_WIDTH_MAX       1048575
0071 #define     MT9M032_SHUTTER_WIDTH_DEF       1943
0072 #define MT9M032_PIX_CLK_CTRL                0x0a
0073 #define     MT9M032_PIX_CLK_CTRL_INV_PIXCLK     0x8000
0074 #define MT9M032_RESTART                 0x0b
0075 #define MT9M032_RESET                   0x0d
0076 #define MT9M032_PLL_CONFIG1             0x11
0077 #define     MT9M032_PLL_CONFIG1_PREDIV_MASK     0x3f
0078 #define     MT9M032_PLL_CONFIG1_MUL_SHIFT       8
0079 #define MT9M032_READ_MODE1              0x1e
0080 #define     MT9M032_READ_MODE1_OUTPUT_BAD_FRAMES    (1 << 13)
0081 #define     MT9M032_READ_MODE1_MAINTAIN_FRAME_RATE  (1 << 12)
0082 #define     MT9M032_READ_MODE1_XOR_LINE_VALID   (1 << 11)
0083 #define     MT9M032_READ_MODE1_CONT_LINE_VALID  (1 << 10)
0084 #define     MT9M032_READ_MODE1_INVERT_TRIGGER   (1 << 9)
0085 #define     MT9M032_READ_MODE1_SNAPSHOT     (1 << 8)
0086 #define     MT9M032_READ_MODE1_GLOBAL_RESET     (1 << 7)
0087 #define     MT9M032_READ_MODE1_BULB_EXPOSURE    (1 << 6)
0088 #define     MT9M032_READ_MODE1_INVERT_STROBE    (1 << 5)
0089 #define     MT9M032_READ_MODE1_STROBE_ENABLE    (1 << 4)
0090 #define     MT9M032_READ_MODE1_STROBE_START_TRIG1   (0 << 2)
0091 #define     MT9M032_READ_MODE1_STROBE_START_EXP (1 << 2)
0092 #define     MT9M032_READ_MODE1_STROBE_START_SHUTTER (2 << 2)
0093 #define     MT9M032_READ_MODE1_STROBE_START_TRIG2   (3 << 2)
0094 #define     MT9M032_READ_MODE1_STROBE_END_TRIG1 (0 << 0)
0095 #define     MT9M032_READ_MODE1_STROBE_END_EXP   (1 << 0)
0096 #define     MT9M032_READ_MODE1_STROBE_END_SHUTTER   (2 << 0)
0097 #define     MT9M032_READ_MODE1_STROBE_END_TRIG2 (3 << 0)
0098 #define MT9M032_READ_MODE2              0x20
0099 #define     MT9M032_READ_MODE2_VFLIP_SHIFT      15
0100 #define     MT9M032_READ_MODE2_HFLIP_SHIFT      14
0101 #define     MT9M032_READ_MODE2_ROW_BLC      0x40
0102 #define MT9M032_GAIN_GREEN1             0x2b
0103 #define MT9M032_GAIN_BLUE               0x2c
0104 #define MT9M032_GAIN_RED                0x2d
0105 #define MT9M032_GAIN_GREEN2             0x2e
0106 
0107 /* write only */
0108 #define MT9M032_GAIN_ALL                0x35
0109 #define     MT9M032_GAIN_DIGITAL_MASK       0x7f
0110 #define     MT9M032_GAIN_DIGITAL_SHIFT      8
0111 #define     MT9M032_GAIN_AMUL_SHIFT         6
0112 #define     MT9M032_GAIN_ANALOG_MASK        0x3f
0113 #define MT9M032_FORMATTER1              0x9e
0114 #define     MT9M032_FORMATTER1_PLL_P1_6     (1 << 8)
0115 #define     MT9M032_FORMATTER1_PARALLEL     (1 << 12)
0116 #define MT9M032_FORMATTER2              0x9f
0117 #define     MT9M032_FORMATTER2_DOUT_EN      0x1000
0118 #define     MT9M032_FORMATTER2_PIXCLK_EN        0x2000
0119 
0120 /*
0121  * The available MT9M032 datasheet is missing documentation for register 0x10
0122  * MT9P031 seems to be close enough, so use constants from that datasheet for
0123  * now.
0124  * But keep the name MT9P031 to remind us, that this isn't really confirmed
0125  * for this sensor.
0126  */
0127 #define MT9P031_PLL_CONTROL             0x10
0128 #define     MT9P031_PLL_CONTROL_PWROFF      0x0050
0129 #define     MT9P031_PLL_CONTROL_PWRON       0x0051
0130 #define     MT9P031_PLL_CONTROL_USEPLL      0x0052
0131 
0132 struct mt9m032 {
0133     struct v4l2_subdev subdev;
0134     struct media_pad pad;
0135     struct mt9m032_platform_data *pdata;
0136 
0137     unsigned int pix_clock;
0138 
0139     struct v4l2_ctrl_handler ctrls;
0140     struct {
0141         struct v4l2_ctrl *hflip;
0142         struct v4l2_ctrl *vflip;
0143     };
0144 
0145     struct mutex lock; /* Protects streaming, format, interval and crop */
0146 
0147     bool streaming;
0148 
0149     struct v4l2_mbus_framefmt format;
0150     struct v4l2_rect crop;
0151     struct v4l2_fract frame_interval;
0152 };
0153 
0154 #define to_mt9m032(sd)  container_of(sd, struct mt9m032, subdev)
0155 #define to_dev(sensor) \
0156     (&((struct i2c_client *)v4l2_get_subdevdata(&(sensor)->subdev))->dev)
0157 
0158 static int mt9m032_read(struct i2c_client *client, u8 reg)
0159 {
0160     return i2c_smbus_read_word_swapped(client, reg);
0161 }
0162 
0163 static int mt9m032_write(struct i2c_client *client, u8 reg, const u16 data)
0164 {
0165     return i2c_smbus_write_word_swapped(client, reg, data);
0166 }
0167 
0168 static u32 mt9m032_row_time(struct mt9m032 *sensor, unsigned int width)
0169 {
0170     unsigned int effective_width;
0171     u32 ns;
0172 
0173     effective_width = width + 716; /* empirical value */
0174     ns = div_u64(1000000000ULL * effective_width, sensor->pix_clock);
0175     dev_dbg(to_dev(sensor), "MT9M032 line time: %u ns\n", ns);
0176     return ns;
0177 }
0178 
0179 static int mt9m032_update_timing(struct mt9m032 *sensor,
0180                  struct v4l2_fract *interval)
0181 {
0182     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0183     struct v4l2_rect *crop = &sensor->crop;
0184     unsigned int min_vblank;
0185     unsigned int vblank;
0186     u32 row_time;
0187 
0188     if (!interval)
0189         interval = &sensor->frame_interval;
0190 
0191     row_time = mt9m032_row_time(sensor, crop->width);
0192 
0193     vblank = div_u64(1000000000ULL * interval->numerator,
0194              (u64)row_time * interval->denominator)
0195            - crop->height;
0196 
0197     if (vblank > MT9M032_VBLANK_MAX) {
0198         /* hardware limits to 11 bit values */
0199         interval->denominator = 1000;
0200         interval->numerator =
0201             div_u64((crop->height + MT9M032_VBLANK_MAX) *
0202                 (u64)row_time * interval->denominator,
0203                 1000000000ULL);
0204         vblank = div_u64(1000000000ULL * interval->numerator,
0205                  (u64)row_time * interval->denominator)
0206                - crop->height;
0207     }
0208     /* enforce minimal 1.6ms blanking time. */
0209     min_vblank = 1600000 / row_time;
0210     vblank = clamp_t(unsigned int, vblank, min_vblank, MT9M032_VBLANK_MAX);
0211 
0212     return mt9m032_write(client, MT9M032_VBLANK, vblank);
0213 }
0214 
0215 static int mt9m032_update_geom_timing(struct mt9m032 *sensor)
0216 {
0217     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0218     int ret;
0219 
0220     ret = mt9m032_write(client, MT9M032_COLUMN_SIZE,
0221                 sensor->crop.width - 1);
0222     if (!ret)
0223         ret = mt9m032_write(client, MT9M032_ROW_SIZE,
0224                     sensor->crop.height - 1);
0225     if (!ret)
0226         ret = mt9m032_write(client, MT9M032_COLUMN_START,
0227                     sensor->crop.left);
0228     if (!ret)
0229         ret = mt9m032_write(client, MT9M032_ROW_START,
0230                     sensor->crop.top);
0231     if (!ret)
0232         ret = mt9m032_update_timing(sensor, NULL);
0233     return ret;
0234 }
0235 
0236 static int update_formatter2(struct mt9m032 *sensor, bool streaming)
0237 {
0238     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0239     u16 reg_val =   MT9M032_FORMATTER2_DOUT_EN
0240               | 0x0070;  /* parts reserved! */
0241                  /* possibly for changing to 14-bit mode */
0242 
0243     if (streaming)
0244         reg_val |= MT9M032_FORMATTER2_PIXCLK_EN;   /* pixclock enable */
0245 
0246     return mt9m032_write(client, MT9M032_FORMATTER2, reg_val);
0247 }
0248 
0249 static int mt9m032_setup_pll(struct mt9m032 *sensor)
0250 {
0251     static const struct aptina_pll_limits limits = {
0252         .ext_clock_min = 8000000,
0253         .ext_clock_max = 16500000,
0254         .int_clock_min = 2000000,
0255         .int_clock_max = 24000000,
0256         .out_clock_min = 322000000,
0257         .out_clock_max = 693000000,
0258         .pix_clock_max = 99000000,
0259         .n_min = 1,
0260         .n_max = 64,
0261         .m_min = 16,
0262         .m_max = 255,
0263         .p1_min = 6,
0264         .p1_max = 7,
0265     };
0266 
0267     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0268     struct mt9m032_platform_data *pdata = sensor->pdata;
0269     struct aptina_pll pll;
0270     u16 reg_val;
0271     int ret;
0272 
0273     pll.ext_clock = pdata->ext_clock;
0274     pll.pix_clock = pdata->pix_clock;
0275 
0276     ret = aptina_pll_calculate(&client->dev, &limits, &pll);
0277     if (ret < 0)
0278         return ret;
0279 
0280     sensor->pix_clock = pdata->pix_clock;
0281 
0282     ret = mt9m032_write(client, MT9M032_PLL_CONFIG1,
0283                 (pll.m << MT9M032_PLL_CONFIG1_MUL_SHIFT) |
0284                 ((pll.n - 1) & MT9M032_PLL_CONFIG1_PREDIV_MASK));
0285     if (!ret)
0286         ret = mt9m032_write(client, MT9P031_PLL_CONTROL,
0287                     MT9P031_PLL_CONTROL_PWRON |
0288                     MT9P031_PLL_CONTROL_USEPLL);
0289     if (!ret)       /* more reserved, Continuous, Master Mode */
0290         ret = mt9m032_write(client, MT9M032_READ_MODE1, 0x8000 |
0291                     MT9M032_READ_MODE1_STROBE_START_EXP |
0292                     MT9M032_READ_MODE1_STROBE_END_SHUTTER);
0293     if (!ret) {
0294         reg_val = (pll.p1 == 6 ? MT9M032_FORMATTER1_PLL_P1_6 : 0)
0295             | MT9M032_FORMATTER1_PARALLEL | 0x001e; /* 14-bit */
0296         ret = mt9m032_write(client, MT9M032_FORMATTER1, reg_val);
0297     }
0298 
0299     return ret;
0300 }
0301 
0302 /* -----------------------------------------------------------------------------
0303  * Subdev pad operations
0304  */
0305 
0306 static int mt9m032_enum_mbus_code(struct v4l2_subdev *subdev,
0307                   struct v4l2_subdev_state *sd_state,
0308                   struct v4l2_subdev_mbus_code_enum *code)
0309 {
0310     if (code->index != 0)
0311         return -EINVAL;
0312 
0313     code->code = MEDIA_BUS_FMT_Y8_1X8;
0314     return 0;
0315 }
0316 
0317 static int mt9m032_enum_frame_size(struct v4l2_subdev *subdev,
0318                    struct v4l2_subdev_state *sd_state,
0319                    struct v4l2_subdev_frame_size_enum *fse)
0320 {
0321     if (fse->index != 0 || fse->code != MEDIA_BUS_FMT_Y8_1X8)
0322         return -EINVAL;
0323 
0324     fse->min_width = MT9M032_COLUMN_SIZE_DEF;
0325     fse->max_width = MT9M032_COLUMN_SIZE_DEF;
0326     fse->min_height = MT9M032_ROW_SIZE_DEF;
0327     fse->max_height = MT9M032_ROW_SIZE_DEF;
0328 
0329     return 0;
0330 }
0331 
0332 /**
0333  * __mt9m032_get_pad_crop() - get crop rect
0334  * @sensor: pointer to the sensor struct
0335  * @sd_state: v4l2_subdev_state for getting the try crop rect from
0336  * @which: select try or active crop rect
0337  *
0338  * Returns a pointer the current active or fh relative try crop rect
0339  */
0340 static struct v4l2_rect *
0341 __mt9m032_get_pad_crop(struct mt9m032 *sensor,
0342                struct v4l2_subdev_state *sd_state,
0343                enum v4l2_subdev_format_whence which)
0344 {
0345     switch (which) {
0346     case V4L2_SUBDEV_FORMAT_TRY:
0347         return v4l2_subdev_get_try_crop(&sensor->subdev, sd_state, 0);
0348     case V4L2_SUBDEV_FORMAT_ACTIVE:
0349         return &sensor->crop;
0350     default:
0351         return NULL;
0352     }
0353 }
0354 
0355 /**
0356  * __mt9m032_get_pad_format() - get format
0357  * @sensor: pointer to the sensor struct
0358  * @sd_state: v4l2_subdev_state for getting the try format from
0359  * @which: select try or active format
0360  *
0361  * Returns a pointer the current active or fh relative try format
0362  */
0363 static struct v4l2_mbus_framefmt *
0364 __mt9m032_get_pad_format(struct mt9m032 *sensor,
0365              struct v4l2_subdev_state *sd_state,
0366              enum v4l2_subdev_format_whence which)
0367 {
0368     switch (which) {
0369     case V4L2_SUBDEV_FORMAT_TRY:
0370         return v4l2_subdev_get_try_format(&sensor->subdev, sd_state,
0371                           0);
0372     case V4L2_SUBDEV_FORMAT_ACTIVE:
0373         return &sensor->format;
0374     default:
0375         return NULL;
0376     }
0377 }
0378 
0379 static int mt9m032_get_pad_format(struct v4l2_subdev *subdev,
0380                   struct v4l2_subdev_state *sd_state,
0381                   struct v4l2_subdev_format *fmt)
0382 {
0383     struct mt9m032 *sensor = to_mt9m032(subdev);
0384 
0385     mutex_lock(&sensor->lock);
0386     fmt->format = *__mt9m032_get_pad_format(sensor, sd_state, fmt->which);
0387     mutex_unlock(&sensor->lock);
0388 
0389     return 0;
0390 }
0391 
0392 static int mt9m032_set_pad_format(struct v4l2_subdev *subdev,
0393                   struct v4l2_subdev_state *sd_state,
0394                   struct v4l2_subdev_format *fmt)
0395 {
0396     struct mt9m032 *sensor = to_mt9m032(subdev);
0397     int ret;
0398 
0399     mutex_lock(&sensor->lock);
0400 
0401     if (sensor->streaming && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
0402         ret = -EBUSY;
0403         goto done;
0404     }
0405 
0406     /* Scaling is not supported, the format is thus fixed. */
0407     fmt->format = *__mt9m032_get_pad_format(sensor, sd_state, fmt->which);
0408     ret = 0;
0409 
0410 done:
0411     mutex_unlock(&sensor->lock);
0412     return ret;
0413 }
0414 
0415 static int mt9m032_get_pad_selection(struct v4l2_subdev *subdev,
0416                      struct v4l2_subdev_state *sd_state,
0417                      struct v4l2_subdev_selection *sel)
0418 {
0419     struct mt9m032 *sensor = to_mt9m032(subdev);
0420 
0421     if (sel->target != V4L2_SEL_TGT_CROP)
0422         return -EINVAL;
0423 
0424     mutex_lock(&sensor->lock);
0425     sel->r = *__mt9m032_get_pad_crop(sensor, sd_state, sel->which);
0426     mutex_unlock(&sensor->lock);
0427 
0428     return 0;
0429 }
0430 
0431 static int mt9m032_set_pad_selection(struct v4l2_subdev *subdev,
0432                      struct v4l2_subdev_state *sd_state,
0433                      struct v4l2_subdev_selection *sel)
0434 {
0435     struct mt9m032 *sensor = to_mt9m032(subdev);
0436     struct v4l2_mbus_framefmt *format;
0437     struct v4l2_rect *__crop;
0438     struct v4l2_rect rect;
0439     int ret = 0;
0440 
0441     if (sel->target != V4L2_SEL_TGT_CROP)
0442         return -EINVAL;
0443 
0444     mutex_lock(&sensor->lock);
0445 
0446     if (sensor->streaming && sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
0447         ret = -EBUSY;
0448         goto done;
0449     }
0450 
0451     /* Clamp the crop rectangle boundaries and align them to a multiple of 2
0452      * pixels to ensure a GRBG Bayer pattern.
0453      */
0454     rect.left = clamp(ALIGN(sel->r.left, 2), MT9M032_COLUMN_START_MIN,
0455               MT9M032_COLUMN_START_MAX);
0456     rect.top = clamp(ALIGN(sel->r.top, 2), MT9M032_ROW_START_MIN,
0457              MT9M032_ROW_START_MAX);
0458     rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
0459                  MT9M032_COLUMN_SIZE_MIN, MT9M032_COLUMN_SIZE_MAX);
0460     rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
0461                   MT9M032_ROW_SIZE_MIN, MT9M032_ROW_SIZE_MAX);
0462 
0463     rect.width = min_t(unsigned int, rect.width,
0464                MT9M032_PIXEL_ARRAY_WIDTH - rect.left);
0465     rect.height = min_t(unsigned int, rect.height,
0466                 MT9M032_PIXEL_ARRAY_HEIGHT - rect.top);
0467 
0468     __crop = __mt9m032_get_pad_crop(sensor, sd_state, sel->which);
0469 
0470     if (rect.width != __crop->width || rect.height != __crop->height) {
0471         /* Reset the output image size if the crop rectangle size has
0472          * been modified.
0473          */
0474         format = __mt9m032_get_pad_format(sensor, sd_state,
0475                           sel->which);
0476         format->width = rect.width;
0477         format->height = rect.height;
0478     }
0479 
0480     *__crop = rect;
0481     sel->r = rect;
0482 
0483     if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
0484         ret = mt9m032_update_geom_timing(sensor);
0485 
0486 done:
0487     mutex_unlock(&sensor->lock);
0488     return ret;
0489 }
0490 
0491 static int mt9m032_get_frame_interval(struct v4l2_subdev *subdev,
0492                       struct v4l2_subdev_frame_interval *fi)
0493 {
0494     struct mt9m032 *sensor = to_mt9m032(subdev);
0495 
0496     mutex_lock(&sensor->lock);
0497     memset(fi, 0, sizeof(*fi));
0498     fi->interval = sensor->frame_interval;
0499     mutex_unlock(&sensor->lock);
0500 
0501     return 0;
0502 }
0503 
0504 static int mt9m032_set_frame_interval(struct v4l2_subdev *subdev,
0505                       struct v4l2_subdev_frame_interval *fi)
0506 {
0507     struct mt9m032 *sensor = to_mt9m032(subdev);
0508     int ret;
0509 
0510     mutex_lock(&sensor->lock);
0511 
0512     if (sensor->streaming) {
0513         ret = -EBUSY;
0514         goto done;
0515     }
0516 
0517     /* Avoid divisions by 0. */
0518     if (fi->interval.denominator == 0)
0519         fi->interval.denominator = 1;
0520 
0521     ret = mt9m032_update_timing(sensor, &fi->interval);
0522     if (!ret)
0523         sensor->frame_interval = fi->interval;
0524 
0525 done:
0526     mutex_unlock(&sensor->lock);
0527     return ret;
0528 }
0529 
0530 static int mt9m032_s_stream(struct v4l2_subdev *subdev, int streaming)
0531 {
0532     struct mt9m032 *sensor = to_mt9m032(subdev);
0533     int ret;
0534 
0535     mutex_lock(&sensor->lock);
0536     ret = update_formatter2(sensor, streaming);
0537     if (!ret)
0538         sensor->streaming = streaming;
0539     mutex_unlock(&sensor->lock);
0540 
0541     return ret;
0542 }
0543 
0544 /* -----------------------------------------------------------------------------
0545  * V4L2 subdev core operations
0546  */
0547 
0548 #ifdef CONFIG_VIDEO_ADV_DEBUG
0549 static int mt9m032_g_register(struct v4l2_subdev *sd,
0550                   struct v4l2_dbg_register *reg)
0551 {
0552     struct mt9m032 *sensor = to_mt9m032(sd);
0553     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0554     int val;
0555 
0556     if (reg->reg > 0xff)
0557         return -EINVAL;
0558 
0559     val = mt9m032_read(client, reg->reg);
0560     if (val < 0)
0561         return -EIO;
0562 
0563     reg->size = 2;
0564     reg->val = val;
0565 
0566     return 0;
0567 }
0568 
0569 static int mt9m032_s_register(struct v4l2_subdev *sd,
0570                   const struct v4l2_dbg_register *reg)
0571 {
0572     struct mt9m032 *sensor = to_mt9m032(sd);
0573     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0574 
0575     if (reg->reg > 0xff)
0576         return -EINVAL;
0577 
0578     return mt9m032_write(client, reg->reg, reg->val);
0579 }
0580 #endif
0581 
0582 /* -----------------------------------------------------------------------------
0583  * V4L2 subdev control operations
0584  */
0585 
0586 static int update_read_mode2(struct mt9m032 *sensor, bool vflip, bool hflip)
0587 {
0588     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0589     int reg_val = (vflip << MT9M032_READ_MODE2_VFLIP_SHIFT)
0590             | (hflip << MT9M032_READ_MODE2_HFLIP_SHIFT)
0591             | MT9M032_READ_MODE2_ROW_BLC
0592             | 0x0007;
0593 
0594     return mt9m032_write(client, MT9M032_READ_MODE2, reg_val);
0595 }
0596 
0597 static int mt9m032_set_gain(struct mt9m032 *sensor, s32 val)
0598 {
0599     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0600     int digital_gain_val;   /* in 1/8th (0..127) */
0601     int analog_mul;     /* 0 or 1 */
0602     int analog_gain_val;    /* in 1/16th. (0..63) */
0603     u16 reg_val;
0604 
0605     digital_gain_val = 51; /* from setup example */
0606 
0607     if (val < 63) {
0608         analog_mul = 0;
0609         analog_gain_val = val;
0610     } else {
0611         analog_mul = 1;
0612         analog_gain_val = val / 2;
0613     }
0614 
0615     /* a_gain = (1 + analog_mul) + (analog_gain_val + 1) / 16 */
0616     /* overall_gain = a_gain * (1 + digital_gain_val / 8) */
0617 
0618     reg_val = ((digital_gain_val & MT9M032_GAIN_DIGITAL_MASK)
0619            << MT9M032_GAIN_DIGITAL_SHIFT)
0620         | ((analog_mul & 1) << MT9M032_GAIN_AMUL_SHIFT)
0621         | (analog_gain_val & MT9M032_GAIN_ANALOG_MASK);
0622 
0623     return mt9m032_write(client, MT9M032_GAIN_ALL, reg_val);
0624 }
0625 
0626 static int mt9m032_try_ctrl(struct v4l2_ctrl *ctrl)
0627 {
0628     if (ctrl->id == V4L2_CID_GAIN && ctrl->val >= 63) {
0629         /* round because of multiplier used for values >= 63 */
0630         ctrl->val &= ~1;
0631     }
0632 
0633     return 0;
0634 }
0635 
0636 static int mt9m032_set_ctrl(struct v4l2_ctrl *ctrl)
0637 {
0638     struct mt9m032 *sensor =
0639         container_of(ctrl->handler, struct mt9m032, ctrls);
0640     struct i2c_client *client = v4l2_get_subdevdata(&sensor->subdev);
0641     int ret;
0642 
0643     switch (ctrl->id) {
0644     case V4L2_CID_GAIN:
0645         return mt9m032_set_gain(sensor, ctrl->val);
0646 
0647     case V4L2_CID_HFLIP:
0648     /* case V4L2_CID_VFLIP: -- In the same cluster */
0649         return update_read_mode2(sensor, sensor->vflip->val,
0650                      sensor->hflip->val);
0651 
0652     case V4L2_CID_EXPOSURE:
0653         ret = mt9m032_write(client, MT9M032_SHUTTER_WIDTH_HIGH,
0654                     (ctrl->val >> 16) & 0xffff);
0655         if (ret < 0)
0656             return ret;
0657 
0658         return mt9m032_write(client, MT9M032_SHUTTER_WIDTH_LOW,
0659                      ctrl->val & 0xffff);
0660     }
0661 
0662     return 0;
0663 }
0664 
0665 static const struct v4l2_ctrl_ops mt9m032_ctrl_ops = {
0666     .s_ctrl = mt9m032_set_ctrl,
0667     .try_ctrl = mt9m032_try_ctrl,
0668 };
0669 
0670 /* -------------------------------------------------------------------------- */
0671 
0672 static const struct v4l2_subdev_core_ops mt9m032_core_ops = {
0673 #ifdef CONFIG_VIDEO_ADV_DEBUG
0674     .g_register = mt9m032_g_register,
0675     .s_register = mt9m032_s_register,
0676 #endif
0677 };
0678 
0679 static const struct v4l2_subdev_video_ops mt9m032_video_ops = {
0680     .s_stream = mt9m032_s_stream,
0681     .g_frame_interval = mt9m032_get_frame_interval,
0682     .s_frame_interval = mt9m032_set_frame_interval,
0683 };
0684 
0685 static const struct v4l2_subdev_pad_ops mt9m032_pad_ops = {
0686     .enum_mbus_code = mt9m032_enum_mbus_code,
0687     .enum_frame_size = mt9m032_enum_frame_size,
0688     .get_fmt = mt9m032_get_pad_format,
0689     .set_fmt = mt9m032_set_pad_format,
0690     .set_selection = mt9m032_set_pad_selection,
0691     .get_selection = mt9m032_get_pad_selection,
0692 };
0693 
0694 static const struct v4l2_subdev_ops mt9m032_ops = {
0695     .core = &mt9m032_core_ops,
0696     .video = &mt9m032_video_ops,
0697     .pad = &mt9m032_pad_ops,
0698 };
0699 
0700 /* -----------------------------------------------------------------------------
0701  * Driver initialization and probing
0702  */
0703 
0704 static int mt9m032_probe(struct i2c_client *client,
0705              const struct i2c_device_id *devid)
0706 {
0707     struct mt9m032_platform_data *pdata = client->dev.platform_data;
0708     struct i2c_adapter *adapter = client->adapter;
0709     struct mt9m032 *sensor;
0710     int chip_version;
0711     int ret;
0712 
0713     if (pdata == NULL) {
0714         dev_err(&client->dev, "No platform data\n");
0715         return -EINVAL;
0716     }
0717 
0718     if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
0719         dev_warn(&client->dev,
0720              "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
0721         return -EIO;
0722     }
0723 
0724     if (!client->dev.platform_data)
0725         return -ENODEV;
0726 
0727     sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
0728     if (sensor == NULL)
0729         return -ENOMEM;
0730 
0731     mutex_init(&sensor->lock);
0732 
0733     sensor->pdata = pdata;
0734 
0735     v4l2_i2c_subdev_init(&sensor->subdev, client, &mt9m032_ops);
0736     sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
0737 
0738     chip_version = mt9m032_read(client, MT9M032_CHIP_VERSION);
0739     if (chip_version != MT9M032_CHIP_VERSION_VALUE) {
0740         dev_err(&client->dev, "MT9M032 not detected, wrong version "
0741             "0x%04x\n", chip_version);
0742         ret = -ENODEV;
0743         goto error_sensor;
0744     }
0745 
0746     dev_info(&client->dev, "MT9M032 detected at address 0x%02x\n",
0747          client->addr);
0748 
0749     sensor->frame_interval.numerator = 1;
0750     sensor->frame_interval.denominator = 30;
0751 
0752     sensor->crop.left = MT9M032_COLUMN_START_DEF;
0753     sensor->crop.top = MT9M032_ROW_START_DEF;
0754     sensor->crop.width = MT9M032_COLUMN_SIZE_DEF;
0755     sensor->crop.height = MT9M032_ROW_SIZE_DEF;
0756 
0757     sensor->format.width = sensor->crop.width;
0758     sensor->format.height = sensor->crop.height;
0759     sensor->format.code = MEDIA_BUS_FMT_Y8_1X8;
0760     sensor->format.field = V4L2_FIELD_NONE;
0761     sensor->format.colorspace = V4L2_COLORSPACE_SRGB;
0762 
0763     v4l2_ctrl_handler_init(&sensor->ctrls, 5);
0764 
0765     v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
0766               V4L2_CID_GAIN, 0, 127, 1, 64);
0767 
0768     sensor->hflip = v4l2_ctrl_new_std(&sensor->ctrls,
0769                       &mt9m032_ctrl_ops,
0770                       V4L2_CID_HFLIP, 0, 1, 1, 0);
0771     sensor->vflip = v4l2_ctrl_new_std(&sensor->ctrls,
0772                       &mt9m032_ctrl_ops,
0773                       V4L2_CID_VFLIP, 0, 1, 1, 0);
0774 
0775     v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
0776               V4L2_CID_EXPOSURE, MT9M032_SHUTTER_WIDTH_MIN,
0777               MT9M032_SHUTTER_WIDTH_MAX, 1,
0778               MT9M032_SHUTTER_WIDTH_DEF);
0779     v4l2_ctrl_new_std(&sensor->ctrls, &mt9m032_ctrl_ops,
0780               V4L2_CID_PIXEL_RATE, pdata->pix_clock,
0781               pdata->pix_clock, 1, pdata->pix_clock);
0782 
0783     if (sensor->ctrls.error) {
0784         ret = sensor->ctrls.error;
0785         dev_err(&client->dev, "control initialization error %d\n", ret);
0786         goto error_ctrl;
0787     }
0788 
0789     v4l2_ctrl_cluster(2, &sensor->hflip);
0790 
0791     sensor->subdev.ctrl_handler = &sensor->ctrls;
0792     sensor->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
0793     sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
0794     ret = media_entity_pads_init(&sensor->subdev.entity, 1, &sensor->pad);
0795     if (ret < 0)
0796         goto error_ctrl;
0797 
0798     ret = mt9m032_write(client, MT9M032_RESET, 1);  /* reset on */
0799     if (ret < 0)
0800         goto error_entity;
0801     ret = mt9m032_write(client, MT9M032_RESET, 0);  /* reset off */
0802     if (ret < 0)
0803         goto error_entity;
0804 
0805     ret = mt9m032_setup_pll(sensor);
0806     if (ret < 0)
0807         goto error_entity;
0808     usleep_range(10000, 11000);
0809 
0810     ret = v4l2_ctrl_handler_setup(&sensor->ctrls);
0811     if (ret < 0)
0812         goto error_entity;
0813 
0814     /* SIZE */
0815     ret = mt9m032_update_geom_timing(sensor);
0816     if (ret < 0)
0817         goto error_entity;
0818 
0819     ret = mt9m032_write(client, 0x41, 0x0000);  /* reserved !!! */
0820     if (ret < 0)
0821         goto error_entity;
0822     ret = mt9m032_write(client, 0x42, 0x0003);  /* reserved !!! */
0823     if (ret < 0)
0824         goto error_entity;
0825     ret = mt9m032_write(client, 0x43, 0x0003);  /* reserved !!! */
0826     if (ret < 0)
0827         goto error_entity;
0828     ret = mt9m032_write(client, 0x7f, 0x0000);  /* reserved !!! */
0829     if (ret < 0)
0830         goto error_entity;
0831     if (sensor->pdata->invert_pixclock) {
0832         ret = mt9m032_write(client, MT9M032_PIX_CLK_CTRL,
0833                     MT9M032_PIX_CLK_CTRL_INV_PIXCLK);
0834         if (ret < 0)
0835             goto error_entity;
0836     }
0837 
0838     ret = mt9m032_write(client, MT9M032_RESTART, 1); /* Restart on */
0839     if (ret < 0)
0840         goto error_entity;
0841     msleep(100);
0842     ret = mt9m032_write(client, MT9M032_RESTART, 0); /* Restart off */
0843     if (ret < 0)
0844         goto error_entity;
0845     msleep(100);
0846     ret = update_formatter2(sensor, false);
0847     if (ret < 0)
0848         goto error_entity;
0849 
0850     return ret;
0851 
0852 error_entity:
0853     media_entity_cleanup(&sensor->subdev.entity);
0854 error_ctrl:
0855     v4l2_ctrl_handler_free(&sensor->ctrls);
0856 error_sensor:
0857     mutex_destroy(&sensor->lock);
0858     return ret;
0859 }
0860 
0861 static int mt9m032_remove(struct i2c_client *client)
0862 {
0863     struct v4l2_subdev *subdev = i2c_get_clientdata(client);
0864     struct mt9m032 *sensor = to_mt9m032(subdev);
0865 
0866     v4l2_device_unregister_subdev(subdev);
0867     v4l2_ctrl_handler_free(&sensor->ctrls);
0868     media_entity_cleanup(&subdev->entity);
0869     mutex_destroy(&sensor->lock);
0870     return 0;
0871 }
0872 
0873 static const struct i2c_device_id mt9m032_id_table[] = {
0874     { MT9M032_NAME, 0 },
0875     { }
0876 };
0877 
0878 MODULE_DEVICE_TABLE(i2c, mt9m032_id_table);
0879 
0880 static struct i2c_driver mt9m032_i2c_driver = {
0881     .driver = {
0882         .name = MT9M032_NAME,
0883     },
0884     .probe = mt9m032_probe,
0885     .remove = mt9m032_remove,
0886     .id_table = mt9m032_id_table,
0887 };
0888 
0889 module_i2c_driver(mt9m032_i2c_driver);
0890 
0891 MODULE_AUTHOR("Martin Hostettler <martin@neutronstar.dyndns.org>");
0892 MODULE_DESCRIPTION("MT9M032 camera sensor driver");
0893 MODULE_LICENSE("GPL v2");