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