drivers/media/video/ov9640.c

   1 /*
   2  * OmniVision OV96xx Camera Driver
   3  *
   4  * Copyright (C) 2009 Marek Vasut <marek.vasut@gmail.com>
   5  *
   6  * Based on ov772x camera driver:
   7  *
   8  * Copyright (C) 2008 Renesas Solutions Corp.
   9  * Kuninori Morimoto <morimoto.kuninori@renesas.com>
  10  *
  11  * Based on ov7670 and soc_camera_platform driver,
  12  *
  13  * Copyright 2006-7 Jonathan Corbet <corbet@lwn.net>
  14  * Copyright (C) 2008 Magnus Damm
  15  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
  16  *
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License version 2 as
  19  * published by the Free Software Foundation.
  20  */
  21
  22 #include <linux/init.h>
  23 #include <linux/module.h>
  24 #include <linux/i2c.h>
  25 #include <linux/slab.h>
  26 #include <linux/delay.h>
  27 #include <linux/videodev2.h>
  28 #include <media/v4l2-chip-ident.h>
  29 #include <media/v4l2-common.h>
  30 #include <media/soc_camera.h>
  31
  32 #include "ov9640.h"
  33
  34 /* default register setup */
  35 static const struct ov9640_reg ov9640_regs_dflt[] = {
  36         { OV9640_COM5,  OV9640_COM5_SYSCLK | OV9640_COM5_LONGEXP },
  37         { OV9640_COM6,  OV9640_COM6_OPT_BLC | OV9640_COM6_ADBLC_BIAS |
  38                         OV9640_COM6_FMT_RST | OV9640_COM6_ADBLC_OPTEN },
  39         { OV9640_PSHFT, OV9640_PSHFT_VAL(0x01) },
  40         { OV9640_ACOM,  OV9640_ACOM_2X_ANALOG | OV9640_ACOM_RSVD },
  41         { OV9640_TSLB,  OV9640_TSLB_YUYV_UYVY },
  42         { OV9640_COM16, OV9640_COM16_RB_AVG },
  43
  44         /* Gamma curve P */
  45         { 0x6c, 0x40 }, { 0x6d, 0x30 }, { 0x6e, 0x4b }, { 0x6f, 0x60 },
  46         { 0x70, 0x70 }, { 0x71, 0x70 }, { 0x72, 0x70 }, { 0x73, 0x70 },
  47         { 0x74, 0x60 }, { 0x75, 0x60 }, { 0x76, 0x50 }, { 0x77, 0x48 },
  48         { 0x78, 0x3a }, { 0x79, 0x2e }, { 0x7a, 0x28 }, { 0x7b, 0x22 },
  49
  50         /* Gamma curve T */
  51         { 0x7c, 0x04 }, { 0x7d, 0x07 }, { 0x7e, 0x10 }, { 0x7f, 0x28 },
  52         { 0x80, 0x36 }, { 0x81, 0x44 }, { 0x82, 0x52 }, { 0x83, 0x60 },
  53         { 0x84, 0x6c }, { 0x85, 0x78 }, { 0x86, 0x8c }, { 0x87, 0x9e },
  54         { 0x88, 0xbb }, { 0x89, 0xd2 }, { 0x8a, 0xe6 },
  55 };
  56
  57 /* Configurations
  58  * NOTE: for YUV, alter the following registers:
  59  *              COM12 |= OV9640_COM12_YUV_AVG
  60  *
  61  *       for RGB, alter the following registers:
  62  *              COM7  |= OV9640_COM7_RGB
  63  *              COM13 |= OV9640_COM13_RGB_AVG
  64  *              COM15 |= proper RGB color encoding mode
  65  */
  66 static const struct ov9640_reg ov9640_regs_qqcif[] = {
  67         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x0f) },
  68         { OV9640_COM1,  OV9640_COM1_QQFMT | OV9640_COM1_HREF_2SKIP },
  69         { OV9640_COM4,  OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
  70         { OV9640_COM7,  OV9640_COM7_QCIF },
  71         { OV9640_COM12, OV9640_COM12_RSVD },
  72         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
  73         { OV9640_COM15, OV9640_COM15_OR_10F0 },
  74 };
  75
  76 static const struct ov9640_reg ov9640_regs_qqvga[] = {
  77         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x07) },
  78         { OV9640_COM1,  OV9640_COM1_QQFMT | OV9640_COM1_HREF_2SKIP },
  79         { OV9640_COM4,  OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
  80         { OV9640_COM7,  OV9640_COM7_QVGA },
  81         { OV9640_COM12, OV9640_COM12_RSVD },
  82         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
  83         { OV9640_COM15, OV9640_COM15_OR_10F0 },
  84 };
  85
  86 static const struct ov9640_reg ov9640_regs_qcif[] = {
  87         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x07) },
  88         { OV9640_COM4,  OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
  89         { OV9640_COM7,  OV9640_COM7_QCIF },
  90         { OV9640_COM12, OV9640_COM12_RSVD },
  91         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
  92         { OV9640_COM15, OV9640_COM15_OR_10F0 },
  93 };
  94
  95 static const struct ov9640_reg ov9640_regs_qvga[] = {
  96         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x03) },
  97         { OV9640_COM4,  OV9640_COM4_QQ_VP | OV9640_COM4_RSVD },
  98         { OV9640_COM7,  OV9640_COM7_QVGA },
  99         { OV9640_COM12, OV9640_COM12_RSVD },
 100         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
 101         { OV9640_COM15, OV9640_COM15_OR_10F0 },
 102 };
 103
 104 static const struct ov9640_reg ov9640_regs_cif[] = {
 105         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x03) },
 106         { OV9640_COM3,  OV9640_COM3_VP },
 107         { OV9640_COM7,  OV9640_COM7_CIF },
 108         { OV9640_COM12, OV9640_COM12_RSVD },
 109         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
 110         { OV9640_COM15, OV9640_COM15_OR_10F0 },
 111 };
 112
 113 static const struct ov9640_reg ov9640_regs_vga[] = {
 114         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x01) },
 115         { OV9640_COM3,  OV9640_COM3_VP },
 116         { OV9640_COM7,  OV9640_COM7_VGA },
 117         { OV9640_COM12, OV9640_COM12_RSVD },
 118         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
 119         { OV9640_COM15, OV9640_COM15_OR_10F0 },
 120 };
 121
 122 static const struct ov9640_reg ov9640_regs_sxga[] = {
 123         { OV9640_CLKRC, OV9640_CLKRC_DPLL_EN | OV9640_CLKRC_DIV(0x01) },
 124         { OV9640_COM3,  OV9640_COM3_VP },
 125         { OV9640_COM7,  0 },
 126         { OV9640_COM12, OV9640_COM12_RSVD },
 127         { OV9640_COM13, OV9640_COM13_GAMMA_RAW | OV9640_COM13_MATRIX_EN },
 128         { OV9640_COM15, OV9640_COM15_OR_10F0 },
 129 };
 130
 131 static const struct ov9640_reg ov9640_regs_yuv[] = {
 132         { OV9640_MTX1,  0x58 },
 133         { OV9640_MTX2,  0x48 },
 134         { OV9640_MTX3,  0x10 },
 135         { OV9640_MTX4,  0x28 },
 136         { OV9640_MTX5,  0x48 },
 137         { OV9640_MTX6,  0x70 },
 138         { OV9640_MTX7,  0x40 },
 139         { OV9640_MTX8,  0x40 },
 140         { OV9640_MTX9,  0x40 },
 141         { OV9640_MTXS,  0x0f },
 142 };
 143
 144 static const struct ov9640_reg ov9640_regs_rgb[] = {
 145         { OV9640_MTX1,  0x71 },
 146         { OV9640_MTX2,  0x3e },
 147         { OV9640_MTX3,  0x0c },
 148         { OV9640_MTX4,  0x33 },
 149         { OV9640_MTX5,  0x72 },
 150         { OV9640_MTX6,  0x00 },
 151         { OV9640_MTX7,  0x2b },
 152         { OV9640_MTX8,  0x66 },
 153         { OV9640_MTX9,  0xd2 },
 154         { OV9640_MTXS,  0x65 },
 155 };
 156
 157 static enum v4l2_mbus_pixelcode ov9640_codes[] = {
 158         V4L2_MBUS_FMT_UYVY8_2X8,
 159         V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
 160         V4L2_MBUS_FMT_RGB565_2X8_LE,
 161 };
 162
 163 static const struct v4l2_queryctrl ov9640_controls[] = {
 164         {
 165                 .id             = V4L2_CID_VFLIP,
 166                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 167                 .name           = "Flip Vertically",
 168                 .minimum        = 0,
 169                 .maximum        = 1,
 170                 .step           = 1,
 171                 .default_value  = 0,
 172         },
 173         {
 174                 .id             = V4L2_CID_HFLIP,
 175                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
 176                 .name           = "Flip Horizontally",
 177                 .minimum        = 0,
 178                 .maximum        = 1,
 179                 .step           = 1,
 180                 .default_value  = 0,
 181         },
 182 };
 183
 184 /* read a register */
 185 static int ov9640_reg_read(struct i2c_client *client, u8 reg, u8 *val)
 186 {
 187         int ret;
 188         u8 data = reg;
 189         struct i2c_msg msg = {
 190                 .addr   = client->addr,
 191                 .flags  = 0,
 192                 .len    = 1,
 193                 .buf    = &data,
 194         };
 195
 196         ret = i2c_transfer(client->adapter, &msg, 1);
 197         if (ret < 0)
 198                 goto err;
 199
 200         msg.flags = I2C_M_RD;
 201         ret = i2c_transfer(client->adapter, &msg, 1);
 202         if (ret < 0)
 203                 goto err;
 204
 205         *val = data;
 206         return 0;
 207
 208 err:
 209         dev_err(&client->dev, "Failed reading register 0x%02x!\n", reg);
 210         return ret;
 211 }
 212
 213 /* write a register */
 214 static int ov9640_reg_write(struct i2c_client *client, u8 reg, u8 val)
 215 {
 216         int ret;
 217         u8 _val;
 218         unsigned char data[2] = { reg, val };
 219         struct i2c_msg msg = {
 220                 .addr   = client->addr,
 221                 .flags  = 0,
 222                 .len    = 2,
 223                 .buf    = data,
 224         };
 225
 226         ret = i2c_transfer(client->adapter, &msg, 1);
 227         if (ret < 0) {
 228                 dev_err(&client->dev, "Failed writing register 0x%02x!\n", reg);
 229                 return ret;
 230         }
 231
 232         /* we have to read the register back ... no idea why, maybe HW bug */
 233         ret = ov9640_reg_read(client, reg, &_val);
 234         if (ret)
 235                 dev_err(&client->dev,
 236                         "Failed reading back register 0x%02x!\n", reg);
 237
 238         return 0;
 239 }
 240
 241
 242 /* Read a register, alter its bits, write it back */
 243 static int ov9640_reg_rmw(struct i2c_client *client, u8 reg, u8 set, u8 unset)
 244 {
 245         u8 val;
 246         int ret;
 247
 248         ret = ov9640_reg_read(client, reg, &val);
 249         if (ret) {
 250                 dev_err(&client->dev,
 251                         "[Read]-Modify-Write of register %02x failed!\n", reg);
 252                 return val;
 253         }
 254
 255         val |= set;
 256         val &= ~unset;
 257
 258         ret = ov9640_reg_write(client, reg, val);
 259         if (ret)
 260                 dev_err(&client->dev,
 261                         "Read-Modify-[Write] of register %02x failed!\n", reg);
 262
 263         return ret;
 264 }
 265
 266 /* Soft reset the camera. This has nothing to do with the RESET pin! */
 267 static int ov9640_reset(struct i2c_client *client)
 268 {
 269         int ret;
 270
 271         ret = ov9640_reg_write(client, OV9640_COM7, OV9640_COM7_SCCB_RESET);
 272         if (ret)
 273                 dev_err(&client->dev,
 274                         "An error occured while entering soft reset!\n");
 275
 276         return ret;
 277 }
 278
 279 /* Start/Stop streaming from the device */
 280 static int ov9640_s_stream(struct v4l2_subdev *sd, int enable)
 281 {
 282         return 0;
 283 }
 284
 285 /* Alter bus settings on camera side */
 286 static int ov9640_set_bus_param(struct soc_camera_device *icd,
 287                                 unsigned long flags)
 288 {
 289         return 0;
 290 }
 291
 292 /* Request bus settings on camera side */
 293 static unsigned long ov9640_query_bus_param(struct soc_camera_device *icd)
 294 {
 295         struct soc_camera_link *icl = to_soc_camera_link(icd);
 296
 297         /*
 298          * REVISIT: the camera probably can do 10 bit transfers, but I don't
 299          *          have those pins connected on my hardware.
 300          */
 301         unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
 302                 SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
 303                 SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
 304
 305         return soc_camera_apply_sensor_flags(icl, flags);
 306 }
 307
 308 /* Get status of additional camera capabilities */
 309 static int ov9640_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 310 {
 311         struct i2c_client *client = v4l2_get_subdevdata(sd);
 312         struct ov9640_priv *priv = container_of(i2c_get_clientdata(client),
 313                                         struct ov9640_priv, subdev);
 314
 315         switch (ctrl->id) {
 316         case V4L2_CID_VFLIP:
 317                 ctrl->value = priv->flag_vflip;
 318                 break;
 319         case V4L2_CID_HFLIP:
 320                 ctrl->value = priv->flag_hflip;
 321                 break;
 322         }
 323         return 0;
 324 }
 325
 326 /* Set status of additional camera capabilities */
 327 static int ov9640_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 328 {
 329         struct i2c_client *client = v4l2_get_subdevdata(sd);
 330         struct ov9640_priv *priv = container_of(i2c_get_clientdata(client),
 331                                         struct ov9640_priv, subdev);
 332
 333         int ret = 0;
 334
 335         switch (ctrl->id) {
 336         case V4L2_CID_VFLIP:
 337                 priv->flag_vflip = ctrl->value;
 338                 if (ctrl->value)
 339                         ret = ov9640_reg_rmw(client, OV9640_MVFP,
 340                                                         OV9640_MVFP_V, 0);
 341                 else
 342                         ret = ov9640_reg_rmw(client, OV9640_MVFP,
 343                                                         0, OV9640_MVFP_V);
 344                 break;
 345         case V4L2_CID_HFLIP:
 346                 priv->flag_hflip = ctrl->value;
 347                 if (ctrl->value)
 348                         ret = ov9640_reg_rmw(client, OV9640_MVFP,
 349                                                         OV9640_MVFP_H, 0);
 350                 else
 351                         ret = ov9640_reg_rmw(client, OV9640_MVFP,
 352                                                         0, OV9640_MVFP_H);
 353                 break;
 354         }
 355
 356         return ret;
 357 }
 358
 359 /* Get chip identification */
 360 static int ov9640_g_chip_ident(struct v4l2_subdev *sd,
 361                                 struct v4l2_dbg_chip_ident *id)
 362 {
 363         struct i2c_client *client = v4l2_get_subdevdata(sd);
 364         struct ov9640_priv *priv = container_of(i2c_get_clientdata(client),
 365                                         struct ov9640_priv, subdev);
 366
 367         id->ident       = priv->model;
 368         id->revision    = priv->revision;
 369
 370         return 0;
 371 }
 372
 373 #ifdef CONFIG_VIDEO_ADV_DEBUG
 374 static int ov9640_get_register(struct v4l2_subdev *sd,
 375                                 struct v4l2_dbg_register *reg)
 376 {
 377         struct i2c_client *client = v4l2_get_subdevdata(sd);
 378         int ret;
 379         u8 val;
 380
 381         if (reg->reg & ~0xff)
 382                 return -EINVAL;
 383
 384         reg->size = 1;
 385
 386         ret = ov9640_reg_read(client, reg->reg, &val);
 387         if (ret)
 388                 return ret;
 389
 390         reg->val = (__u64)val;
 391
 392         return 0;
 393 }
 394
 395 static int ov9640_set_register(struct v4l2_subdev *sd,
 396                                 struct v4l2_dbg_register *reg)
 397 {
 398         struct i2c_client *client = v4l2_get_subdevdata(sd);
 399
 400         if (reg->reg & ~0xff || reg->val & ~0xff)
 401                 return -EINVAL;
 402
 403         return ov9640_reg_write(client, reg->reg, reg->val);
 404 }
 405 #endif
 406
 407 /* select nearest higher resolution for capture */
 408 static void ov9640_res_roundup(u32 *width, u32 *height)
 409 {
 410         int i;
 411         enum { QQCIF, QQVGA, QCIF, QVGA, CIF, VGA, SXGA };
 412         int res_x[] = { 88, 160, 176, 320, 352, 640, 1280 };
 413         int res_y[] = { 72, 120, 144, 240, 288, 480, 960 };
 414
 415         for (i = 0; i < ARRAY_SIZE(res_x); i++) {
 416                 if (res_x[i] >= *width && res_y[i] >= *height) {
 417                         *width = res_x[i];
 418                         *height = res_y[i];
 419                         return;
 420                 }
 421         }
 422
 423         *width = res_x[SXGA];
 424         *height = res_y[SXGA];
 425 }
 426
 427 /* Prepare necessary register changes depending on color encoding */
 428 static void ov9640_alter_regs(enum v4l2_mbus_pixelcode code,
 429                               struct ov9640_reg_alt *alt)
 430 {
 431         switch (code) {
 432         default:
 433         case V4L2_MBUS_FMT_UYVY8_2X8:
 434                 alt->com12      = OV9640_COM12_YUV_AVG;
 435                 alt->com13      = OV9640_COM13_Y_DELAY_EN |
 436                                         OV9640_COM13_YUV_DLY(0x01);
 437                 break;
 438         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
 439                 alt->com7       = OV9640_COM7_RGB;
 440                 alt->com13      = OV9640_COM13_RGB_AVG;
 441                 alt->com15      = OV9640_COM15_RGB_555;
 442                 break;
 443         case V4L2_MBUS_FMT_RGB565_2X8_LE:
 444                 alt->com7       = OV9640_COM7_RGB;
 445                 alt->com13      = OV9640_COM13_RGB_AVG;
 446                 alt->com15      = OV9640_COM15_RGB_565;
 447                 break;
 448         };
 449 }
 450
 451 /* Setup registers according to resolution and color encoding */
 452 static int ov9640_write_regs(struct i2c_client *client, u32 width,
 453                 enum v4l2_mbus_pixelcode code, struct ov9640_reg_alt *alts)
 454 {
 455         const struct ov9640_reg *ov9640_regs, *matrix_regs;
 456         int                     ov9640_regs_len, matrix_regs_len;
 457         int                     i, ret;
 458         u8                      val;
 459
 460         /* select register configuration for given resolution */
 461         switch (width) {
 462         case W_QQCIF:
 463                 ov9640_regs     = ov9640_regs_qqcif;
 464                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_qqcif);
 465                 break;
 466         case W_QQVGA:
 467                 ov9640_regs     = ov9640_regs_qqvga;
 468                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_qqvga);
 469                 break;
 470         case W_QCIF:
 471                 ov9640_regs     = ov9640_regs_qcif;
 472                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_qcif);
 473                 break;
 474         case W_QVGA:
 475                 ov9640_regs     = ov9640_regs_qvga;
 476                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_qvga);
 477                 break;
 478         case W_CIF:
 479                 ov9640_regs     = ov9640_regs_cif;
 480                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_cif);
 481                 break;
 482         case W_VGA:
 483                 ov9640_regs     = ov9640_regs_vga;
 484                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_vga);
 485                 break;
 486         case W_SXGA:
 487                 ov9640_regs     = ov9640_regs_sxga;
 488                 ov9640_regs_len = ARRAY_SIZE(ov9640_regs_sxga);
 489                 break;
 490         default:
 491                 dev_err(&client->dev, "Failed to select resolution!\n");
 492                 return -EINVAL;
 493         }
 494
 495         /* select color matrix configuration for given color encoding */
 496         if (code == V4L2_MBUS_FMT_UYVY8_2X8) {
 497                 matrix_regs     = ov9640_regs_yuv;
 498                 matrix_regs_len = ARRAY_SIZE(ov9640_regs_yuv);
 499         } else {
 500                 matrix_regs     = ov9640_regs_rgb;
 501                 matrix_regs_len = ARRAY_SIZE(ov9640_regs_rgb);
 502         }
 503
 504         /* write register settings into the module */
 505         for (i = 0; i < ov9640_regs_len; i++) {
 506                 val = ov9640_regs[i].val;
 507
 508                 switch (ov9640_regs[i].reg) {
 509                 case OV9640_COM7:
 510                         val |= alts->com7;
 511                         break;
 512                 case OV9640_COM12:
 513                         val |= alts->com12;
 514                         break;
 515                 case OV9640_COM13:
 516                         val |= alts->com13;
 517                         break;
 518                 case OV9640_COM15:
 519                         val |= alts->com15;
 520                         break;
 521                 }
 522
 523                 ret = ov9640_reg_write(client, ov9640_regs[i].reg, val);
 524                 if (ret)
 525                         return ret;
 526         }
 527
 528         /* write color matrix configuration into the module */
 529         for (i = 0; i < matrix_regs_len; i++) {
 530                 ret = ov9640_reg_write(client, matrix_regs[i].reg,
 531                                                 matrix_regs[i].val);
 532                 if (ret)
 533                         return ret;
 534         }
 535
 536         return 0;
 537 }
 538
 539 /* program default register values */
 540 static int ov9640_prog_dflt(struct i2c_client *client)
 541 {
 542         int i, ret;
 543
 544         for (i = 0; i < ARRAY_SIZE(ov9640_regs_dflt); i++) {
 545                 ret = ov9640_reg_write(client, ov9640_regs_dflt[i].reg,
 546                                                 ov9640_regs_dflt[i].val);
 547                 if (ret)
 548                         return ret;
 549         }
 550
 551         /* wait for the changes to actually happen, 140ms are not enough yet */
 552         mdelay(150);
 553
 554         return 0;
 555 }
 556
 557 /* set the format we will capture in */
 558 static int ov9640_s_fmt(struct v4l2_subdev *sd,
 559                         struct v4l2_mbus_framefmt *mf)
 560 {
 561         struct i2c_client *client = v4l2_get_subdevdata(sd);
 562         struct ov9640_reg_alt alts = {0};
 563         enum v4l2_colorspace cspace;
 564         enum v4l2_mbus_pixelcode code = mf->code;
 565         int ret;
 566
 567         ov9640_res_roundup(&mf->width, &mf->height);
 568         ov9640_alter_regs(mf->code, &alts);
 569
 570         ov9640_reset(client);
 571
 572         ret = ov9640_prog_dflt(client);
 573         if (ret)
 574                 return ret;
 575
 576         switch (code) {
 577         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
 578         case V4L2_MBUS_FMT_RGB565_2X8_LE:
 579                 cspace = V4L2_COLORSPACE_SRGB;
 580                 break;
 581         default:
 582                 code = V4L2_MBUS_FMT_UYVY8_2X8;
 583         case V4L2_MBUS_FMT_UYVY8_2X8:
 584                 cspace = V4L2_COLORSPACE_JPEG;
 585         }
 586
 587         ret = ov9640_write_regs(client, mf->width, code, &alts);
 588         if (!ret) {
 589                 mf->code        = code;
 590                 mf->colorspace  = cspace;
 591         }
 592
 593         return ret;
 594 }
 595
 596 static int ov9640_try_fmt(struct v4l2_subdev *sd,
 597                           struct v4l2_mbus_framefmt *mf)
 598 {
 599         ov9640_res_roundup(&mf->width, &mf->height);
 600
 601         mf->field = V4L2_FIELD_NONE;
 602
 603         switch (mf->code) {
 604         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
 605         case V4L2_MBUS_FMT_RGB565_2X8_LE:
 606                 mf->colorspace = V4L2_COLORSPACE_SRGB;
 607                 break;
 608         default:
 609                 mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
 610         case V4L2_MBUS_FMT_UYVY8_2X8:
 611                 mf->colorspace = V4L2_COLORSPACE_JPEG;
 612         }
 613
 614         return 0;
 615 }
 616
 617 static int ov9640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
 618                            enum v4l2_mbus_pixelcode *code)
 619 {
 620         if (index >= ARRAY_SIZE(ov9640_codes))
 621                 return -EINVAL;
 622
 623         *code = ov9640_codes[index];
 624         return 0;
 625 }
 626
 627 static int ov9640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
 628 {
 629         a->c.left       = 0;
 630         a->c.top        = 0;
 631         a->c.width      = W_SXGA;
 632         a->c.height     = H_SXGA;
 633         a->type         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 634
 635         return 0;
 636 }
 637
 638 static int ov9640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 639 {
 640         a->bounds.left                  = 0;
 641         a->bounds.top                   = 0;
 642         a->bounds.width                 = W_SXGA;
 643         a->bounds.height                = H_SXGA;
 644         a->defrect                      = a->bounds;
 645         a->type                         = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 646         a->pixelaspect.numerator        = 1;
 647         a->pixelaspect.denominator      = 1;
 648
 649         return 0;
 650 }
 651
 652
 653
 654 static int ov9640_video_probe(struct soc_camera_device *icd,
 655                                 struct i2c_client *client)
 656 {
 657         struct ov9640_priv *priv = i2c_get_clientdata(client);
 658         u8              pid, ver, midh, midl;
 659         const char      *devname;
 660         int             ret = 0;
 661
 662         /*
 663          * We must have a parent by now. And it cannot be a wrong one.
 664          * So this entire test is completely redundant.
 665          */
 666         if (!icd->dev.parent ||
 667             to_soc_camera_host(icd->dev.parent)->nr != icd->iface) {
 668                 dev_err(&client->dev, "Parent missing or invalid!\n");
 669                 ret = -ENODEV;
 670                 goto err;
 671         }
 672
 673         /*
 674          * check and show product ID and manufacturer ID
 675          */
 676
 677         ret = ov9640_reg_read(client, OV9640_PID, &pid);
 678         if (ret)
 679                 goto err;
 680
 681         ret = ov9640_reg_read(client, OV9640_VER, &ver);
 682         if (ret)
 683                 goto err;
 684
 685         ret = ov9640_reg_read(client, OV9640_MIDH, &midh);
 686         if (ret)
 687                 goto err;
 688
 689         ret = ov9640_reg_read(client, OV9640_MIDL, &midl);
 690         if (ret)
 691                 goto err;
 692
 693         switch (VERSION(pid, ver)) {
 694         case OV9640_V2:
 695                 devname         = "ov9640";
 696                 priv->model     = V4L2_IDENT_OV9640;
 697                 priv->revision  = 2;
 698         case OV9640_V3:
 699                 devname         = "ov9640";
 700                 priv->model     = V4L2_IDENT_OV9640;
 701                 priv->revision  = 3;
 702                 break;
 703         default:
 704                 dev_err(&client->dev, "Product ID error %x:%x\n", pid, ver);
 705                 ret = -ENODEV;
 706                 goto err;
 707         }
 708
 709         dev_info(&client->dev, "%s Product ID %0x:%0x Manufacturer ID %x:%x\n",
 710                  devname, pid, ver, midh, midl);
 711
 712 err:
 713         return ret;
 714 }
 715
 716 static struct soc_camera_ops ov9640_ops = {
 717         .set_bus_param          = ov9640_set_bus_param,
 718         .query_bus_param        = ov9640_query_bus_param,
 719         .controls               = ov9640_controls,
 720         .num_controls           = ARRAY_SIZE(ov9640_controls),
 721 };
 722
 723 static struct v4l2_subdev_core_ops ov9640_core_ops = {
 724         .g_ctrl                 = ov9640_g_ctrl,
 725         .s_ctrl                 = ov9640_s_ctrl,
 726         .g_chip_ident           = ov9640_g_chip_ident,
 727 #ifdef CONFIG_VIDEO_ADV_DEBUG
 728         .g_register             = ov9640_get_register,
 729         .s_register             = ov9640_set_register,
 730 #endif
 731
 732 };
 733
 734 static struct v4l2_subdev_video_ops ov9640_video_ops = {
 735         .s_stream       = ov9640_s_stream,
 736         .s_mbus_fmt     = ov9640_s_fmt,
 737         .try_mbus_fmt   = ov9640_try_fmt,
 738         .enum_mbus_fmt  = ov9640_enum_fmt,
 739         .cropcap        = ov9640_cropcap,
 740         .g_crop         = ov9640_g_crop,
 741
 742 };
 743
 744 static struct v4l2_subdev_ops ov9640_subdev_ops = {
 745         .core   = &ov9640_core_ops,
 746         .video  = &ov9640_video_ops,
 747 };
 748
 749 /*
 750  * i2c_driver function
 751  */
 752 static int ov9640_probe(struct i2c_client *client,
 753                         const struct i2c_device_id *did)
 754 {
 755         struct ov9640_priv *priv;
 756         struct soc_camera_device *icd   = client->dev.platform_data;
 757         struct soc_camera_link *icl;
 758         int ret;
 759
 760         if (!icd) {
 761                 dev_err(&client->dev, "Missing soc-camera data!\n");
 762                 return -EINVAL;
 763         }
 764
 765         icl = to_soc_camera_link(icd);
 766         if (!icl) {
 767                 dev_err(&client->dev, "Missing platform_data for driver\n");
 768                 return -EINVAL;
 769         }
 770
 771         priv = kzalloc(sizeof(struct ov9640_priv), GFP_KERNEL);
 772         if (!priv) {
 773                 dev_err(&client->dev,
 774                         "Failed to allocate memory for private data!\n");
 775                 return -ENOMEM;
 776         }
 777
 778         v4l2_i2c_subdev_init(&priv->subdev, client, &ov9640_subdev_ops);
 779
 780         icd->ops        = &ov9640_ops;
 781
 782         ret = ov9640_video_probe(icd, client);
 783
 784         if (ret) {
 785                 icd->ops = NULL;
 786                 kfree(priv);
 787         }
 788
 789         return ret;
 790 }
 791
 792 static int ov9640_remove(struct i2c_client *client)
 793 {
 794         struct ov9640_priv *priv = i2c_get_clientdata(client);
 795
 796         kfree(priv);
 797         return 0;
 798 }
 799
 800 static const struct i2c_device_id ov9640_id[] = {
 801         { "ov9640", 0 },
 802         { }
 803 };
 804 MODULE_DEVICE_TABLE(i2c, ov9640_id);
 805
 806 static struct i2c_driver ov9640_i2c_driver = {
 807         .driver = {
 808                 .name = "ov9640",
 809         },
 810         .probe    = ov9640_probe,
 811         .remove   = ov9640_remove,
 812         .id_table = ov9640_id,
 813 };
 814
 815 static int __init ov9640_module_init(void)
 816 {
 817         return i2c_add_driver(&ov9640_i2c_driver);
 818 }
 819
 820 static void __exit ov9640_module_exit(void)
 821 {
 822         i2c_del_driver(&ov9640_i2c_driver);
 823 }
 824
 825 module_init(ov9640_module_init);
 826 module_exit(ov9640_module_exit);
 827
 828 MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV96xx");
 829 MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
 830 MODULE_LICENSE("GPL v2");