drivers/media/video/smiapp/smiapp-core.c

   1 /*
   2  * drivers/media/video/smiapp/smiapp-core.c
   3  *
   4  * Generic driver for SMIA/SMIA++ compliant camera modules
   5  *
   6  * Copyright (C) 2010--2012 Nokia Corporation
   7  * Contact: Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>
   8  *
   9  * Based on smiapp driver by Vimarsh Zutshi
  10  * Based on jt8ev1.c by Vimarsh Zutshi
  11  * Based on smia-sensor.c by Tuukka Toivonen <tuukkat76@gmail.com>
  12  *
  13  * This program is free software; you can redistribute it and/or
  14  * modify it under the terms of the GNU General Public License
  15  * version 2 as published by the Free Software Foundation.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of
  19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  20  * General Public License for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License
  23  * along with this program; if not, write to the Free Software
  24  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  25  * 02110-1301 USA
  26  *
  27  */
  28
  29 #include <linux/clk.h>
  30 #include <linux/delay.h>
  31 #include <linux/device.h>
  32 #include <linux/gpio.h>
  33 #include <linux/module.h>
  34 #include <linux/slab.h>
  35 #include <linux/regulator/consumer.h>
  36 #include <linux/slab.h>
  37 #include <linux/v4l2-mediabus.h>
  38 #include <media/v4l2-device.h>
  39
  40 #include "smiapp.h"
  41
  42 #define SMIAPP_ALIGN_DIM(dim, flags)    \
  43         ((flags) & V4L2_SEL_FLAG_GE     \
  44          ? ALIGN((dim), 2)              \
  45          : (dim) & ~1)
  46
  47 /*
  48  * smiapp_module_idents - supported camera modules
  49  */
  50 static const struct smiapp_module_ident smiapp_module_idents[] = {
  51         SMIAPP_IDENT_L(0x01, 0x022b, -1, "vs6555"),
  52         SMIAPP_IDENT_L(0x01, 0x022e, -1, "vw6558"),
  53         SMIAPP_IDENT_L(0x07, 0x7698, -1, "ovm7698"),
  54         SMIAPP_IDENT_L(0x0b, 0x4242, -1, "smiapp-003"),
  55         SMIAPP_IDENT_L(0x0c, 0x208a, -1, "tcm8330md"),
  56         SMIAPP_IDENT_LQ(0x0c, 0x2134, -1, "tcm8500md", &smiapp_tcm8500md_quirk),
  57         SMIAPP_IDENT_L(0x0c, 0x213e, -1, "et8en2"),
  58         SMIAPP_IDENT_L(0x0c, 0x2184, -1, "tcm8580md"),
  59         SMIAPP_IDENT_LQ(0x0c, 0x560f, -1, "jt8ew9", &smiapp_jt8ew9_quirk),
  60         SMIAPP_IDENT_LQ(0x10, 0x4141, -1, "jt8ev1", &smiapp_jt8ev1_quirk),
  61         SMIAPP_IDENT_LQ(0x10, 0x4241, -1, "imx125es", &smiapp_imx125es_quirk),
  62 };
  63
  64 /*
  65  *
  66  * Dynamic Capability Identification
  67  *
  68  */
  69
  70 static int smiapp_read_frame_fmt(struct smiapp_sensor *sensor)
  71 {
  72         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
  73         u32 fmt_model_type, fmt_model_subtype, ncol_desc, nrow_desc;
  74         unsigned int i;
  75         int rval;
  76         int line_count = 0;
  77         int embedded_start = -1, embedded_end = -1;
  78         int image_start = 0;
  79
  80         rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_TYPE,
  81                            &fmt_model_type);
  82         if (rval)
  83                 return rval;
  84
  85         rval = smiapp_read(sensor, SMIAPP_REG_U8_FRAME_FORMAT_MODEL_SUBTYPE,
  86                            &fmt_model_subtype);
  87         if (rval)
  88                 return rval;
  89
  90         ncol_desc = (fmt_model_subtype
  91                      & SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_MASK)
  92                 >> SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NCOLS_SHIFT;
  93         nrow_desc = fmt_model_subtype
  94                 & SMIAPP_FRAME_FORMAT_MODEL_SUBTYPE_NROWS_MASK;
  95
  96         dev_dbg(&client->dev, "format_model_type %s\n",
  97                 fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE
  98                 ? "2 byte" :
  99                 fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE
 100                 ? "4 byte" : "is simply bad");
 101
 102         for (i = 0; i < ncol_desc + nrow_desc; i++) {
 103                 u32 desc;
 104                 u32 pixelcode;
 105                 u32 pixels;
 106                 char *which;
 107                 char *what;
 108
 109                 if (fmt_model_type == SMIAPP_FRAME_FORMAT_MODEL_TYPE_2BYTE) {
 110                         rval = smiapp_read(
 111                                 sensor,
 112                                 SMIAPP_REG_U16_FRAME_FORMAT_DESCRIPTOR_2(i),
 113                                 &desc);
 114                         if (rval)
 115                                 return rval;
 116
 117                         pixelcode =
 118                                 (desc
 119                                  & SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_MASK)
 120                                 >> SMIAPP_FRAME_FORMAT_DESC_2_PIXELCODE_SHIFT;
 121                         pixels = desc & SMIAPP_FRAME_FORMAT_DESC_2_PIXELS_MASK;
 122                 } else if (fmt_model_type
 123                            == SMIAPP_FRAME_FORMAT_MODEL_TYPE_4BYTE) {
 124                         rval = smiapp_read(
 125                                 sensor,
 126                                 SMIAPP_REG_U32_FRAME_FORMAT_DESCRIPTOR_4(i),
 127                                 &desc);
 128                         if (rval)
 129                                 return rval;
 130
 131                         pixelcode =
 132                                 (desc
 133                                  & SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_MASK)
 134                                 >> SMIAPP_FRAME_FORMAT_DESC_4_PIXELCODE_SHIFT;
 135                         pixels = desc & SMIAPP_FRAME_FORMAT_DESC_4_PIXELS_MASK;
 136                 } else {
 137                         dev_dbg(&client->dev,
 138                                 "invalid frame format model type %d\n",
 139                                 fmt_model_type);
 140                         return -EINVAL;
 141                 }
 142
 143                 if (i < ncol_desc)
 144                         which = "columns";
 145                 else
 146                         which = "rows";
 147
 148                 switch (pixelcode) {
 149                 case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED:
 150                         what = "embedded";
 151                         break;
 152                 case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DUMMY:
 153                         what = "dummy";
 154                         break;
 155                 case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_BLACK:
 156                         what = "black";
 157                         break;
 158                 case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_DARK:
 159                         what = "dark";
 160                         break;
 161                 case SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE:
 162                         what = "visible";
 163                         break;
 164                 default:
 165                         what = "invalid";
 166                         dev_dbg(&client->dev, "pixelcode %d\n", pixelcode);
 167                         break;
 168                 }
 169
 170                 dev_dbg(&client->dev, "%s pixels: %d %s\n",
 171                         what, pixels, which);
 172
 173                 if (i < ncol_desc)
 174                         continue;
 175
 176                 /* Handle row descriptors */
 177                 if (pixelcode
 178                     == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_EMBEDDED) {
 179                         embedded_start = line_count;
 180                 } else {
 181                         if (pixelcode == SMIAPP_FRAME_FORMAT_DESC_PIXELCODE_VISIBLE
 182                             || pixels >= sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES] / 2)
 183                                 image_start = line_count;
 184                         if (embedded_start != -1 && embedded_end == -1)
 185                                 embedded_end = line_count;
 186                 }
 187                 line_count += pixels;
 188         }
 189
 190         if (embedded_start == -1 || embedded_end == -1) {
 191                 embedded_start = 0;
 192                 embedded_end = 0;
 193         }
 194
 195         dev_dbg(&client->dev, "embedded data from lines %d to %d\n",
 196                 embedded_start, embedded_end);
 197         dev_dbg(&client->dev, "image data starts at line %d\n", image_start);
 198
 199         return 0;
 200 }
 201
 202 static int smiapp_pll_configure(struct smiapp_sensor *sensor)
 203 {
 204         struct smiapp_pll *pll = &sensor->pll;
 205         int rval;
 206
 207         rval = smiapp_write(
 208                 sensor, SMIAPP_REG_U16_VT_PIX_CLK_DIV, pll->vt_pix_clk_div);
 209         if (rval < 0)
 210                 return rval;
 211
 212         rval = smiapp_write(
 213                 sensor, SMIAPP_REG_U16_VT_SYS_CLK_DIV, pll->vt_sys_clk_div);
 214         if (rval < 0)
 215                 return rval;
 216
 217         rval = smiapp_write(
 218                 sensor, SMIAPP_REG_U16_PRE_PLL_CLK_DIV, pll->pre_pll_clk_div);
 219         if (rval < 0)
 220                 return rval;
 221
 222         rval = smiapp_write(
 223                 sensor, SMIAPP_REG_U16_PLL_MULTIPLIER, pll->pll_multiplier);
 224         if (rval < 0)
 225                 return rval;
 226
 227         /* Lane op clock ratio does not apply here. */
 228         rval = smiapp_write(
 229                 sensor, SMIAPP_REG_U32_REQUESTED_LINK_BIT_RATE_MBPS,
 230                 DIV_ROUND_UP(pll->op_sys_clk_freq_hz, 1000000 / 256 / 256));
 231         if (rval < 0 || sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0)
 232                 return rval;
 233
 234         rval = smiapp_write(
 235                 sensor, SMIAPP_REG_U16_OP_PIX_CLK_DIV, pll->op_pix_clk_div);
 236         if (rval < 0)
 237                 return rval;
 238
 239         return smiapp_write(
 240                 sensor, SMIAPP_REG_U16_OP_SYS_CLK_DIV, pll->op_sys_clk_div);
 241 }
 242
 243 static int smiapp_pll_update(struct smiapp_sensor *sensor)
 244 {
 245         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 246         struct smiapp_pll_limits lim = {
 247                 .min_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_PRE_PLL_CLK_DIV],
 248                 .max_pre_pll_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_PRE_PLL_CLK_DIV],
 249                 .min_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_IP_FREQ_HZ],
 250                 .max_pll_ip_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_IP_FREQ_HZ],
 251                 .min_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MIN_PLL_MULTIPLIER],
 252                 .max_pll_multiplier = sensor->limits[SMIAPP_LIMIT_MAX_PLL_MULTIPLIER],
 253                 .min_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_PLL_OP_FREQ_HZ],
 254                 .max_pll_op_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_PLL_OP_FREQ_HZ],
 255
 256                 .min_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV],
 257                 .max_op_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV],
 258                 .min_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV],
 259                 .max_op_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV],
 260                 .min_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_FREQ_HZ],
 261                 .max_op_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_FREQ_HZ],
 262                 .min_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_FREQ_HZ],
 263                 .max_op_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_FREQ_HZ],
 264
 265                 .min_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_DIV],
 266                 .max_vt_sys_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_DIV],
 267                 .min_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_DIV],
 268                 .max_vt_pix_clk_div = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_DIV],
 269                 .min_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_SYS_CLK_FREQ_HZ],
 270                 .max_vt_sys_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_SYS_CLK_FREQ_HZ],
 271                 .min_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MIN_VT_PIX_CLK_FREQ_HZ],
 272                 .max_vt_pix_clk_freq_hz = sensor->limits[SMIAPP_LIMIT_MAX_VT_PIX_CLK_FREQ_HZ],
 273
 274                 .min_line_length_pck_bin = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN],
 275                 .min_line_length_pck = sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK],
 276         };
 277         struct smiapp_pll *pll = &sensor->pll;
 278         int rval;
 279
 280         memset(&sensor->pll, 0, sizeof(sensor->pll));
 281
 282         pll->lanes = sensor->platform_data->lanes;
 283         pll->ext_clk_freq_hz = sensor->platform_data->ext_clk;
 284
 285         if (sensor->minfo.smiapp_profile == SMIAPP_PROFILE_0) {
 286                 /*
 287                  * Fill in operational clock divisors limits from the
 288                  * video timing ones. On profile 0 sensors the
 289                  * requirements regarding them are essentially the
 290                  * same as on VT ones.
 291                  */
 292                 lim.min_op_sys_clk_div = lim.min_vt_sys_clk_div;
 293                 lim.max_op_sys_clk_div = lim.max_vt_sys_clk_div;
 294                 lim.min_op_pix_clk_div = lim.min_vt_pix_clk_div;
 295                 lim.max_op_pix_clk_div = lim.max_vt_pix_clk_div;
 296                 lim.min_op_sys_clk_freq_hz = lim.min_vt_sys_clk_freq_hz;
 297                 lim.max_op_sys_clk_freq_hz = lim.max_vt_sys_clk_freq_hz;
 298                 lim.min_op_pix_clk_freq_hz = lim.min_vt_pix_clk_freq_hz;
 299                 lim.max_op_pix_clk_freq_hz = lim.max_vt_pix_clk_freq_hz;
 300                 /* Profile 0 sensors have no separate OP clock branch. */
 301                 pll->flags |= SMIAPP_PLL_FLAG_NO_OP_CLOCKS;
 302         }
 303
 304         if (smiapp_needs_quirk(sensor,
 305                                SMIAPP_QUIRK_FLAG_OP_PIX_CLOCK_PER_LANE))
 306                 pll->flags |= SMIAPP_PLL_FLAG_OP_PIX_CLOCK_PER_LANE;
 307
 308         pll->binning_horizontal = sensor->binning_horizontal;
 309         pll->binning_vertical = sensor->binning_vertical;
 310         pll->link_freq =
 311                 sensor->link_freq->qmenu_int[sensor->link_freq->val];
 312         pll->scale_m = sensor->scale_m;
 313         pll->scale_n = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
 314         pll->bits_per_pixel = sensor->csi_format->compressed;
 315
 316         rval = smiapp_pll_calculate(&client->dev, &lim, pll);
 317         if (rval < 0)
 318                 return rval;
 319
 320         sensor->pixel_rate_parray->cur.val64 = pll->vt_pix_clk_freq_hz;
 321         sensor->pixel_rate_csi->cur.val64 = pll->pixel_rate_csi;
 322
 323         return 0;
 324 }
 325
 326
 327 /*
 328  *
 329  * V4L2 Controls handling
 330  *
 331  */
 332
 333 static void __smiapp_update_exposure_limits(struct smiapp_sensor *sensor)
 334 {
 335         struct v4l2_ctrl *ctrl = sensor->exposure;
 336         int max;
 337
 338         max = sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
 339                 + sensor->vblank->val
 340                 - sensor->limits[SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN];
 341
 342         ctrl->maximum = max;
 343         if (ctrl->default_value > max)
 344                 ctrl->default_value = max;
 345         if (ctrl->val > max)
 346                 ctrl->val = max;
 347         if (ctrl->cur.val > max)
 348                 ctrl->cur.val = max;
 349 }
 350
 351 /*
 352  * Order matters.
 353  *
 354  * 1. Bits-per-pixel, descending.
 355  * 2. Bits-per-pixel compressed, descending.
 356  * 3. Pixel order, same as in pixel_order_str. Formats for all four pixel
 357  *    orders must be defined.
 358  */
 359 static const struct smiapp_csi_data_format smiapp_csi_data_formats[] = {
 360         { V4L2_MBUS_FMT_SGRBG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GRBG, },
 361         { V4L2_MBUS_FMT_SRGGB12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_RGGB, },
 362         { V4L2_MBUS_FMT_SBGGR12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_BGGR, },
 363         { V4L2_MBUS_FMT_SGBRG12_1X12, 12, 12, SMIAPP_PIXEL_ORDER_GBRG, },
 364         { V4L2_MBUS_FMT_SGRBG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GRBG, },
 365         { V4L2_MBUS_FMT_SRGGB10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_RGGB, },
 366         { V4L2_MBUS_FMT_SBGGR10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_BGGR, },
 367         { V4L2_MBUS_FMT_SGBRG10_1X10, 10, 10, SMIAPP_PIXEL_ORDER_GBRG, },
 368         { V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GRBG, },
 369         { V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_RGGB, },
 370         { V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_BGGR, },
 371         { V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8, 10, 8, SMIAPP_PIXEL_ORDER_GBRG, },
 372         { V4L2_MBUS_FMT_SGRBG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GRBG, },
 373         { V4L2_MBUS_FMT_SRGGB8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_RGGB, },
 374         { V4L2_MBUS_FMT_SBGGR8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_BGGR, },
 375         { V4L2_MBUS_FMT_SGBRG8_1X8, 8, 8, SMIAPP_PIXEL_ORDER_GBRG, },
 376 };
 377
 378 const char *pixel_order_str[] = { "GRBG", "RGGB", "BGGR", "GBRG" };
 379
 380 #define to_csi_format_idx(fmt) (((unsigned long)(fmt)                   \
 381                                  - (unsigned long)smiapp_csi_data_formats) \
 382                                 / sizeof(*smiapp_csi_data_formats))
 383
 384 static u32 smiapp_pixel_order(struct smiapp_sensor *sensor)
 385 {
 386         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 387         int flip = 0;
 388
 389         if (sensor->hflip) {
 390                 if (sensor->hflip->val)
 391                         flip |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
 392
 393                 if (sensor->vflip->val)
 394                         flip |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
 395         }
 396
 397         flip ^= sensor->hvflip_inv_mask;
 398
 399         dev_dbg(&client->dev, "flip %d\n", flip);
 400         return sensor->default_pixel_order ^ flip;
 401 }
 402
 403 static void smiapp_update_mbus_formats(struct smiapp_sensor *sensor)
 404 {
 405         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 406         unsigned int csi_format_idx =
 407                 to_csi_format_idx(sensor->csi_format) & ~3;
 408         unsigned int internal_csi_format_idx =
 409                 to_csi_format_idx(sensor->internal_csi_format) & ~3;
 410         unsigned int pixel_order = smiapp_pixel_order(sensor);
 411
 412         sensor->mbus_frame_fmts =
 413                 sensor->default_mbus_frame_fmts << pixel_order;
 414         sensor->csi_format =
 415                 &smiapp_csi_data_formats[csi_format_idx + pixel_order];
 416         sensor->internal_csi_format =
 417                 &smiapp_csi_data_formats[internal_csi_format_idx
 418                                          + pixel_order];
 419
 420         BUG_ON(max(internal_csi_format_idx, csi_format_idx) + pixel_order
 421                >= ARRAY_SIZE(smiapp_csi_data_formats));
 422         BUG_ON(min(internal_csi_format_idx, csi_format_idx) < 0);
 423
 424         dev_dbg(&client->dev, "new pixel order %s\n",
 425                 pixel_order_str[pixel_order]);
 426 }
 427
 428 static int smiapp_set_ctrl(struct v4l2_ctrl *ctrl)
 429 {
 430         struct smiapp_sensor *sensor =
 431                 container_of(ctrl->handler, struct smiapp_subdev, ctrl_handler)
 432                         ->sensor;
 433         u32 orient = 0;
 434         int exposure;
 435         int rval;
 436
 437         switch (ctrl->id) {
 438         case V4L2_CID_ANALOGUE_GAIN:
 439                 return smiapp_write(
 440                         sensor,
 441                         SMIAPP_REG_U16_ANALOGUE_GAIN_CODE_GLOBAL, ctrl->val);
 442
 443         case V4L2_CID_EXPOSURE:
 444                 return smiapp_write(
 445                         sensor,
 446                         SMIAPP_REG_U16_COARSE_INTEGRATION_TIME, ctrl->val);
 447
 448         case V4L2_CID_HFLIP:
 449         case V4L2_CID_VFLIP:
 450                 if (sensor->streaming)
 451                         return -EBUSY;
 452
 453                 if (sensor->hflip->val)
 454                         orient |= SMIAPP_IMAGE_ORIENTATION_HFLIP;
 455
 456                 if (sensor->vflip->val)
 457                         orient |= SMIAPP_IMAGE_ORIENTATION_VFLIP;
 458
 459                 orient ^= sensor->hvflip_inv_mask;
 460                 rval = smiapp_write(sensor,
 461                                     SMIAPP_REG_U8_IMAGE_ORIENTATION,
 462                                     orient);
 463                 if (rval < 0)
 464                         return rval;
 465
 466                 smiapp_update_mbus_formats(sensor);
 467
 468                 return 0;
 469
 470         case V4L2_CID_VBLANK:
 471                 exposure = sensor->exposure->val;
 472
 473                 __smiapp_update_exposure_limits(sensor);
 474
 475                 if (exposure > sensor->exposure->maximum) {
 476                         sensor->exposure->val =
 477                                 sensor->exposure->maximum;
 478                         rval = smiapp_set_ctrl(
 479                                 sensor->exposure);
 480                         if (rval < 0)
 481                                 return rval;
 482                 }
 483
 484                 return smiapp_write(
 485                         sensor, SMIAPP_REG_U16_FRAME_LENGTH_LINES,
 486                         sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
 487                         + ctrl->val);
 488
 489         case V4L2_CID_HBLANK:
 490                 return smiapp_write(
 491                         sensor, SMIAPP_REG_U16_LINE_LENGTH_PCK,
 492                         sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
 493                         + ctrl->val);
 494
 495         case V4L2_CID_LINK_FREQ:
 496                 if (sensor->streaming)
 497                         return -EBUSY;
 498
 499                 return smiapp_pll_update(sensor);
 500
 501         default:
 502                 return -EINVAL;
 503         }
 504 }
 505
 506 static const struct v4l2_ctrl_ops smiapp_ctrl_ops = {
 507         .s_ctrl = smiapp_set_ctrl,
 508 };
 509
 510 static int smiapp_init_controls(struct smiapp_sensor *sensor)
 511 {
 512         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 513         unsigned int max;
 514         int rval;
 515
 516         rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 7);
 517         if (rval)
 518                 return rval;
 519         sensor->pixel_array->ctrl_handler.lock = &sensor->mutex;
 520
 521         sensor->analog_gain = v4l2_ctrl_new_std(
 522                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 523                 V4L2_CID_ANALOGUE_GAIN,
 524                 sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN],
 525                 sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MAX],
 526                 max(sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_STEP], 1U),
 527                 sensor->limits[SMIAPP_LIMIT_ANALOGUE_GAIN_CODE_MIN]);
 528
 529         /* Exposure limits will be updated soon, use just something here. */
 530         sensor->exposure = v4l2_ctrl_new_std(
 531                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 532                 V4L2_CID_EXPOSURE, 0, 0, 1, 0);
 533
 534         sensor->hflip = v4l2_ctrl_new_std(
 535                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 536                 V4L2_CID_HFLIP, 0, 1, 1, 0);
 537         sensor->vflip = v4l2_ctrl_new_std(
 538                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 539                 V4L2_CID_VFLIP, 0, 1, 1, 0);
 540
 541         sensor->vblank = v4l2_ctrl_new_std(
 542                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 543                 V4L2_CID_VBLANK, 0, 1, 1, 0);
 544
 545         if (sensor->vblank)
 546                 sensor->vblank->flags |= V4L2_CTRL_FLAG_UPDATE;
 547
 548         sensor->hblank = v4l2_ctrl_new_std(
 549                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 550                 V4L2_CID_HBLANK, 0, 1, 1, 0);
 551
 552         if (sensor->hblank)
 553                 sensor->hblank->flags |= V4L2_CTRL_FLAG_UPDATE;
 554
 555         sensor->pixel_rate_parray = v4l2_ctrl_new_std(
 556                 &sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 557                 V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
 558
 559         if (sensor->pixel_array->ctrl_handler.error) {
 560                 dev_err(&client->dev,
 561                         "pixel array controls initialization failed (%d)\n",
 562                         sensor->pixel_array->ctrl_handler.error);
 563                 rval = sensor->pixel_array->ctrl_handler.error;
 564                 goto error;
 565         }
 566
 567         sensor->pixel_array->sd.ctrl_handler =
 568                 &sensor->pixel_array->ctrl_handler;
 569
 570         v4l2_ctrl_cluster(2, &sensor->hflip);
 571
 572         rval = v4l2_ctrl_handler_init(&sensor->src->ctrl_handler, 0);
 573         if (rval)
 574                 goto error;
 575         sensor->src->ctrl_handler.lock = &sensor->mutex;
 576
 577         for (max = 0; sensor->platform_data->op_sys_clock[max + 1]; max++);
 578
 579         sensor->link_freq = v4l2_ctrl_new_int_menu(
 580                 &sensor->src->ctrl_handler, &smiapp_ctrl_ops,
 581                 V4L2_CID_LINK_FREQ, max, 0,
 582                 sensor->platform_data->op_sys_clock);
 583
 584         sensor->pixel_rate_csi = v4l2_ctrl_new_std(
 585                 &sensor->src->ctrl_handler, &smiapp_ctrl_ops,
 586                 V4L2_CID_PIXEL_RATE, 0, 0, 1, 0);
 587
 588         if (sensor->src->ctrl_handler.error) {
 589                 dev_err(&client->dev,
 590                         "src controls initialization failed (%d)\n",
 591                         sensor->src->ctrl_handler.error);
 592                 rval = sensor->src->ctrl_handler.error;
 593                 goto error;
 594         }
 595
 596         sensor->src->sd.ctrl_handler =
 597                 &sensor->src->ctrl_handler;
 598
 599         return 0;
 600
 601 error:
 602         v4l2_ctrl_handler_free(&sensor->pixel_array->ctrl_handler);
 603         v4l2_ctrl_handler_free(&sensor->src->ctrl_handler);
 604
 605         return rval;
 606 }
 607
 608 static void smiapp_free_controls(struct smiapp_sensor *sensor)
 609 {
 610         unsigned int i;
 611
 612         for (i = 0; i < sensor->ssds_used; i++)
 613                 v4l2_ctrl_handler_free(&sensor->ssds[i].ctrl_handler);
 614 }
 615
 616 static int smiapp_get_limits(struct smiapp_sensor *sensor, int const *limit,
 617                              unsigned int n)
 618 {
 619         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 620         unsigned int i;
 621         u32 val;
 622         int rval;
 623
 624         for (i = 0; i < n; i++) {
 625                 rval = smiapp_read(
 626                         sensor, smiapp_reg_limits[limit[i]].addr, &val);
 627                 if (rval)
 628                         return rval;
 629                 sensor->limits[limit[i]] = val;
 630                 dev_dbg(&client->dev, "0x%8.8x \"%s\" = %d, 0x%x\n",
 631                         smiapp_reg_limits[limit[i]].addr,
 632                         smiapp_reg_limits[limit[i]].what, val, val);
 633         }
 634
 635         return 0;
 636 }
 637
 638 static int smiapp_get_all_limits(struct smiapp_sensor *sensor)
 639 {
 640         unsigned int i;
 641         int rval;
 642
 643         for (i = 0; i < SMIAPP_LIMIT_LAST; i++) {
 644                 rval = smiapp_get_limits(sensor, &i, 1);
 645                 if (rval < 0)
 646                         return rval;
 647         }
 648
 649         if (sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] == 0)
 650                 smiapp_replace_limit(sensor, SMIAPP_LIMIT_SCALER_N_MIN, 16);
 651
 652         return 0;
 653 }
 654
 655 static int smiapp_get_limits_binning(struct smiapp_sensor *sensor)
 656 {
 657         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 658         static u32 const limits[] = {
 659                 SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN,
 660                 SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN,
 661                 SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN,
 662                 SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN,
 663                 SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN,
 664                 SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN_BIN,
 665                 SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN_BIN,
 666         };
 667         static u32 const limits_replace[] = {
 668                 SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES,
 669                 SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES,
 670                 SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK,
 671                 SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK,
 672                 SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK,
 673                 SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MIN,
 674                 SMIAPP_LIMIT_FINE_INTEGRATION_TIME_MAX_MARGIN,
 675         };
 676         unsigned int i;
 677         int rval;
 678
 679         if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY] ==
 680             SMIAPP_BINNING_CAPABILITY_NO) {
 681                 for (i = 0; i < ARRAY_SIZE(limits); i++)
 682                         sensor->limits[limits[i]] =
 683                                 sensor->limits[limits_replace[i]];
 684
 685                 return 0;
 686         }
 687
 688         rval = smiapp_get_limits(sensor, limits, ARRAY_SIZE(limits));
 689         if (rval < 0)
 690                 return rval;
 691
 692         /*
 693          * Sanity check whether the binning limits are valid. If not,
 694          * use the non-binning ones.
 695          */
 696         if (sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN]
 697             && sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN]
 698             && sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN])
 699                 return 0;
 700
 701         for (i = 0; i < ARRAY_SIZE(limits); i++) {
 702                 dev_dbg(&client->dev,
 703                         "replace limit 0x%8.8x \"%s\" = %d, 0x%x\n",
 704                         smiapp_reg_limits[limits[i]].addr,
 705                         smiapp_reg_limits[limits[i]].what,
 706                         sensor->limits[limits_replace[i]],
 707                         sensor->limits[limits_replace[i]]);
 708                 sensor->limits[limits[i]] =
 709                         sensor->limits[limits_replace[i]];
 710         }
 711
 712         return 0;
 713 }
 714
 715 static int smiapp_get_mbus_formats(struct smiapp_sensor *sensor)
 716 {
 717         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 718         unsigned int type, n;
 719         unsigned int i, pixel_order;
 720         int rval;
 721
 722         rval = smiapp_read(
 723                 sensor, SMIAPP_REG_U8_DATA_FORMAT_MODEL_TYPE, &type);
 724         if (rval)
 725                 return rval;
 726
 727         dev_dbg(&client->dev, "data_format_model_type %d\n", type);
 728
 729         rval = smiapp_read(sensor, SMIAPP_REG_U8_PIXEL_ORDER,
 730                            &pixel_order);
 731         if (rval)
 732                 return rval;
 733
 734         if (pixel_order >= ARRAY_SIZE(pixel_order_str)) {
 735                 dev_dbg(&client->dev, "bad pixel order %d\n", pixel_order);
 736                 return -EINVAL;
 737         }
 738
 739         dev_dbg(&client->dev, "pixel order %d (%s)\n", pixel_order,
 740                 pixel_order_str[pixel_order]);
 741
 742         switch (type) {
 743         case SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL:
 744                 n = SMIAPP_DATA_FORMAT_MODEL_TYPE_NORMAL_N;
 745                 break;
 746         case SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED:
 747                 n = SMIAPP_DATA_FORMAT_MODEL_TYPE_EXTENDED_N;
 748                 break;
 749         default:
 750                 return -EINVAL;
 751         }
 752
 753         sensor->default_pixel_order = pixel_order;
 754         sensor->mbus_frame_fmts = 0;
 755
 756         for (i = 0; i < n; i++) {
 757                 unsigned int fmt, j;
 758
 759                 rval = smiapp_read(
 760                         sensor,
 761                         SMIAPP_REG_U16_DATA_FORMAT_DESCRIPTOR(i), &fmt);
 762                 if (rval)
 763                         return rval;
 764
 765                 dev_dbg(&client->dev, "bpp %d, compressed %d\n",
 766                         fmt >> 8, (u8)fmt);
 767
 768                 for (j = 0; j < ARRAY_SIZE(smiapp_csi_data_formats); j++) {
 769                         const struct smiapp_csi_data_format *f =
 770                                 &smiapp_csi_data_formats[j];
 771
 772                         if (f->pixel_order != SMIAPP_PIXEL_ORDER_GRBG)
 773                                 continue;
 774
 775                         if (f->width != fmt >> 8 || f->compressed != (u8)fmt)
 776                                 continue;
 777
 778                         dev_dbg(&client->dev, "jolly good! %d\n", j);
 779
 780                         sensor->default_mbus_frame_fmts |= 1 << j;
 781                         if (!sensor->csi_format) {
 782                                 sensor->csi_format = f;
 783                                 sensor->internal_csi_format = f;
 784                         }
 785                 }
 786         }
 787
 788         if (!sensor->csi_format) {
 789                 dev_err(&client->dev, "no supported mbus code found\n");
 790                 return -EINVAL;
 791         }
 792
 793         smiapp_update_mbus_formats(sensor);
 794
 795         return 0;
 796 }
 797
 798 static void smiapp_update_blanking(struct smiapp_sensor *sensor)
 799 {
 800         struct v4l2_ctrl *vblank = sensor->vblank;
 801         struct v4l2_ctrl *hblank = sensor->hblank;
 802
 803         vblank->minimum =
 804                 max_t(int,
 805                       sensor->limits[SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES],
 806                       sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN] -
 807                       sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height);
 808         vblank->maximum =
 809                 sensor->limits[SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN] -
 810                 sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height;
 811
 812         vblank->val = clamp_t(int, vblank->val,
 813                               vblank->minimum, vblank->maximum);
 814         vblank->default_value = vblank->minimum;
 815         vblank->val = vblank->val;
 816         vblank->cur.val = vblank->val;
 817
 818         hblank->minimum =
 819                 max_t(int,
 820                       sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN] -
 821                       sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width,
 822                       sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN]);
 823         hblank->maximum =
 824                 sensor->limits[SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN] -
 825                 sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width;
 826
 827         hblank->val = clamp_t(int, hblank->val,
 828                               hblank->minimum, hblank->maximum);
 829         hblank->default_value = hblank->minimum;
 830         hblank->val = hblank->val;
 831         hblank->cur.val = hblank->val;
 832
 833         __smiapp_update_exposure_limits(sensor);
 834 }
 835
 836 static int smiapp_update_mode(struct smiapp_sensor *sensor)
 837 {
 838         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 839         unsigned int binning_mode;
 840         int rval;
 841
 842         dev_dbg(&client->dev, "frame size: %dx%d\n",
 843                 sensor->src->crop[SMIAPP_PAD_SRC].width,
 844                 sensor->src->crop[SMIAPP_PAD_SRC].height);
 845         dev_dbg(&client->dev, "csi format width: %d\n",
 846                 sensor->csi_format->width);
 847
 848         /* Binning has to be set up here; it affects limits */
 849         if (sensor->binning_horizontal == 1 &&
 850             sensor->binning_vertical == 1) {
 851                 binning_mode = 0;
 852         } else {
 853                 u8 binning_type =
 854                         (sensor->binning_horizontal << 4)
 855                         | sensor->binning_vertical;
 856
 857                 rval = smiapp_write(
 858                         sensor, SMIAPP_REG_U8_BINNING_TYPE, binning_type);
 859                 if (rval < 0)
 860                         return rval;
 861
 862                 binning_mode = 1;
 863         }
 864         rval = smiapp_write(sensor, SMIAPP_REG_U8_BINNING_MODE, binning_mode);
 865         if (rval < 0)
 866                 return rval;
 867
 868         /* Get updated limits due to binning */
 869         rval = smiapp_get_limits_binning(sensor);
 870         if (rval < 0)
 871                 return rval;
 872
 873         rval = smiapp_pll_update(sensor);
 874         if (rval < 0)
 875                 return rval;
 876
 877         /* Output from pixel array, including blanking */
 878         smiapp_update_blanking(sensor);
 879
 880         dev_dbg(&client->dev, "vblank\t\t%d\n", sensor->vblank->val);
 881         dev_dbg(&client->dev, "hblank\t\t%d\n", sensor->hblank->val);
 882
 883         dev_dbg(&client->dev, "real timeperframe\t100/%d\n",
 884                 sensor->pll.vt_pix_clk_freq_hz /
 885                 ((sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width
 886                   + sensor->hblank->val) *
 887                  (sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height
 888                   + sensor->vblank->val) / 100));
 889
 890         return 0;
 891 }
 892
 893 /*
 894  *
 895  * SMIA++ NVM handling
 896  *
 897  */
 898 static int smiapp_read_nvm(struct smiapp_sensor *sensor,
 899                            unsigned char *nvm)
 900 {
 901         u32 i, s, p, np, v;
 902         int rval = 0, rval2;
 903
 904         np = sensor->nvm_size / SMIAPP_NVM_PAGE_SIZE;
 905         for (p = 0; p < np; p++) {
 906                 rval = smiapp_write(
 907                         sensor,
 908                         SMIAPP_REG_U8_DATA_TRANSFER_IF_1_PAGE_SELECT, p);
 909                 if (rval)
 910                         goto out;
 911
 912                 rval = smiapp_write(sensor,
 913                                     SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL,
 914                                     SMIAPP_DATA_TRANSFER_IF_1_CTRL_EN |
 915                                     SMIAPP_DATA_TRANSFER_IF_1_CTRL_RD_EN);
 916                 if (rval)
 917                         goto out;
 918
 919                 for (i = 0; i < 1000; i++) {
 920                         rval = smiapp_read(
 921                                 sensor,
 922                                 SMIAPP_REG_U8_DATA_TRANSFER_IF_1_STATUS, &s);
 923
 924                         if (rval)
 925                                 goto out;
 926
 927                         if (s & SMIAPP_DATA_TRANSFER_IF_1_STATUS_RD_READY)
 928                                 break;
 929
 930                         if (--i == 0) {
 931                                 rval = -ETIMEDOUT;
 932                                 goto out;
 933                         }
 934
 935                 }
 936
 937                 for (i = 0; i < SMIAPP_NVM_PAGE_SIZE; i++) {
 938                         rval = smiapp_read(
 939                                 sensor,
 940                                 SMIAPP_REG_U8_DATA_TRANSFER_IF_1_DATA_0 + i,
 941                                 &v);
 942                         if (rval)
 943                                 goto out;
 944
 945                         *nvm++ = v;
 946                 }
 947         }
 948
 949 out:
 950         rval2 = smiapp_write(sensor, SMIAPP_REG_U8_DATA_TRANSFER_IF_1_CTRL, 0);
 951         if (rval < 0)
 952                 return rval;
 953         else
 954                 return rval2;
 955 }
 956
 957 /*
 958  *
 959  * SMIA++ CCI address control
 960  *
 961  */
 962 static int smiapp_change_cci_addr(struct smiapp_sensor *sensor)
 963 {
 964         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
 965         int rval;
 966         u32 val;
 967
 968         client->addr = sensor->platform_data->i2c_addr_dfl;
 969
 970         rval = smiapp_write(sensor,
 971                             SMIAPP_REG_U8_CCI_ADDRESS_CONTROL,
 972                             sensor->platform_data->i2c_addr_alt << 1);
 973         if (rval)
 974                 return rval;
 975
 976         client->addr = sensor->platform_data->i2c_addr_alt;
 977
 978         /* verify addr change went ok */
 979         rval = smiapp_read(sensor, SMIAPP_REG_U8_CCI_ADDRESS_CONTROL, &val);
 980         if (rval)
 981                 return rval;
 982
 983         if (val != sensor->platform_data->i2c_addr_alt << 1)
 984                 return -ENODEV;
 985
 986         return 0;
 987 }
 988
 989 /*
 990  *
 991  * SMIA++ Mode Control
 992  *
 993  */
 994 static int smiapp_setup_flash_strobe(struct smiapp_sensor *sensor)
 995 {
 996         struct smiapp_flash_strobe_parms *strobe_setup;
 997         unsigned int ext_freq = sensor->platform_data->ext_clk;
 998         u32 tmp;
 999         u32 strobe_adjustment;
1000         u32 strobe_width_high_rs;
1001         int rval;
1002
1003         strobe_setup = sensor->platform_data->strobe_setup;
1004
1005         /*
1006          * How to calculate registers related to strobe length. Please
1007          * do not change, or if you do at least know what you're
1008          * doing. :-)
1009          *
1010          * Sakari Ailus <sakari.ailus@maxwell.research.nokia.com> 2010-10-25
1011          *
1012          * flash_strobe_length [us] / 10^6 = (tFlash_strobe_width_ctrl
1013          *      / EXTCLK freq [Hz]) * flash_strobe_adjustment
1014          *
1015          * tFlash_strobe_width_ctrl E N, [1 - 0xffff]
1016          * flash_strobe_adjustment E N, [1 - 0xff]
1017          *
1018          * The formula above is written as below to keep it on one
1019          * line:
1020          *
1021          * l / 10^6 = w / e * a
1022          *
1023          * Let's mark w * a by x:
1024          *
1025          * x = w * a
1026          *
1027          * Thus, we get:
1028          *
1029          * x = l * e / 10^6
1030          *
1031          * The strobe width must be at least as long as requested,
1032          * thus rounding upwards is needed.
1033          *
1034          * x = (l * e + 10^6 - 1) / 10^6
1035          * -----------------------------
1036          *
1037          * Maximum possible accuracy is wanted at all times. Thus keep
1038          * a as small as possible.
1039          *
1040          * Calculate a, assuming maximum w, with rounding upwards:
1041          *
1042          * a = (x + (2^16 - 1) - 1) / (2^16 - 1)
1043          * -------------------------------------
1044          *
1045          * Thus, we also get w, with that a, with rounding upwards:
1046          *
1047          * w = (x + a - 1) / a
1048          * -------------------
1049          *
1050          * To get limits:
1051          *
1052          * x E [1, (2^16 - 1) * (2^8 - 1)]
1053          *
1054          * Substituting maximum x to the original formula (with rounding),
1055          * the maximum l is thus
1056          *
1057          * (2^16 - 1) * (2^8 - 1) * 10^6 = l * e + 10^6 - 1
1058          *
1059          * l = (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / e
1060          * --------------------------------------------------
1061          *
1062          * flash_strobe_length must be clamped between 1 and
1063          * (10^6 * (2^16 - 1) * (2^8 - 1) - 10^6 + 1) / EXTCLK freq.
1064          *
1065          * Then,
1066          *
1067          * flash_strobe_adjustment = ((flash_strobe_length *
1068          *      EXTCLK freq + 10^6 - 1) / 10^6 + (2^16 - 1) - 1) / (2^16 - 1)
1069          *
1070          * tFlash_strobe_width_ctrl = ((flash_strobe_length *
1071          *      EXTCLK freq + 10^6 - 1) / 10^6 +
1072          *      flash_strobe_adjustment - 1) / flash_strobe_adjustment
1073          */
1074         tmp = div_u64(1000000ULL * ((1 << 16) - 1) * ((1 << 8) - 1) -
1075                       1000000 + 1, ext_freq);
1076         strobe_setup->strobe_width_high_us =
1077                 clamp_t(u32, strobe_setup->strobe_width_high_us, 1, tmp);
1078
1079         tmp = div_u64(((u64)strobe_setup->strobe_width_high_us * (u64)ext_freq +
1080                         1000000 - 1), 1000000ULL);
1081         strobe_adjustment = (tmp + (1 << 16) - 1 - 1) / ((1 << 16) - 1);
1082         strobe_width_high_rs = (tmp + strobe_adjustment - 1) /
1083                                 strobe_adjustment;
1084
1085         rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_MODE_RS,
1086                             strobe_setup->mode);
1087         if (rval < 0)
1088                 goto out;
1089
1090         rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_STROBE_ADJUSTMENT,
1091                             strobe_adjustment);
1092         if (rval < 0)
1093                 goto out;
1094
1095         rval = smiapp_write(
1096                 sensor, SMIAPP_REG_U16_TFLASH_STROBE_WIDTH_HIGH_RS_CTRL,
1097                 strobe_width_high_rs);
1098         if (rval < 0)
1099                 goto out;
1100
1101         rval = smiapp_write(sensor, SMIAPP_REG_U16_TFLASH_STROBE_DELAY_RS_CTRL,
1102                             strobe_setup->strobe_delay);
1103         if (rval < 0)
1104                 goto out;
1105
1106         rval = smiapp_write(sensor, SMIAPP_REG_U16_FLASH_STROBE_START_POINT,
1107                             strobe_setup->stobe_start_point);
1108         if (rval < 0)
1109                 goto out;
1110
1111         rval = smiapp_write(sensor, SMIAPP_REG_U8_FLASH_TRIGGER_RS,
1112                             strobe_setup->trigger);
1113
1114 out:
1115         sensor->platform_data->strobe_setup->trigger = 0;
1116
1117         return rval;
1118 }
1119
1120 /* -----------------------------------------------------------------------------
1121  * Power management
1122  */
1123
1124 static int smiapp_power_on(struct smiapp_sensor *sensor)
1125 {
1126         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
1127         unsigned int sleep;
1128         int rval;
1129
1130         rval = regulator_enable(sensor->vana);
1131         if (rval) {
1132                 dev_err(&client->dev, "failed to enable vana regulator\n");
1133                 return rval;
1134         }
1135         usleep_range(1000, 1000);
1136
1137         if (sensor->platform_data->set_xclk)
1138                 rval = sensor->platform_data->set_xclk(
1139                         &sensor->src->sd, sensor->platform_data->ext_clk);
1140         else
1141                 rval = clk_enable(sensor->ext_clk);
1142         if (rval < 0) {
1143                 dev_dbg(&client->dev, "failed to set xclk\n");
1144                 goto out_xclk_fail;
1145         }
1146         usleep_range(1000, 1000);
1147
1148         if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
1149                 gpio_set_value(sensor->platform_data->xshutdown, 1);
1150
1151         sleep = SMIAPP_RESET_DELAY(sensor->platform_data->ext_clk);
1152         usleep_range(sleep, sleep);
1153
1154         /*
1155          * Failures to respond to the address change command have been noticed.
1156          * Those failures seem to be caused by the sensor requiring a longer
1157          * boot time than advertised. An additional 10ms delay seems to work
1158          * around the issue, but the SMIA++ I2C write retry hack makes the delay
1159          * unnecessary. The failures need to be investigated to find a proper
1160          * fix, and a delay will likely need to be added here if the I2C write
1161          * retry hack is reverted before the root cause of the boot time issue
1162          * is found.
1163          */
1164
1165         if (sensor->platform_data->i2c_addr_alt) {
1166                 rval = smiapp_change_cci_addr(sensor);
1167                 if (rval) {
1168                         dev_err(&client->dev, "cci address change error\n");
1169                         goto out_cci_addr_fail;
1170                 }
1171         }
1172
1173         rval = smiapp_write(sensor, SMIAPP_REG_U8_SOFTWARE_RESET,
1174                             SMIAPP_SOFTWARE_RESET);
1175         if (rval < 0) {
1176                 dev_err(&client->dev, "software reset failed\n");
1177                 goto out_cci_addr_fail;
1178         }
1179
1180         if (sensor->platform_data->i2c_addr_alt) {
1181                 rval = smiapp_change_cci_addr(sensor);
1182                 if (rval) {
1183                         dev_err(&client->dev, "cci address change error\n");
1184                         goto out_cci_addr_fail;
1185                 }
1186         }
1187
1188         rval = smiapp_write(sensor, SMIAPP_REG_U16_COMPRESSION_MODE,
1189                             SMIAPP_COMPRESSION_MODE_SIMPLE_PREDICTOR);
1190         if (rval) {
1191                 dev_err(&client->dev, "compression mode set failed\n");
1192                 goto out_cci_addr_fail;
1193         }
1194
1195         rval = smiapp_write(
1196                 sensor, SMIAPP_REG_U16_EXTCLK_FREQUENCY_MHZ,
1197                 sensor->platform_data->ext_clk / (1000000 / (1 << 8)));
1198         if (rval) {
1199                 dev_err(&client->dev, "extclk frequency set failed\n");
1200                 goto out_cci_addr_fail;
1201         }
1202
1203         rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_LANE_MODE,
1204                             sensor->platform_data->lanes - 1);
1205         if (rval) {
1206                 dev_err(&client->dev, "csi lane mode set failed\n");
1207                 goto out_cci_addr_fail;
1208         }
1209
1210         rval = smiapp_write(sensor, SMIAPP_REG_U8_FAST_STANDBY_CTRL,
1211                             SMIAPP_FAST_STANDBY_CTRL_IMMEDIATE);
1212         if (rval) {
1213                 dev_err(&client->dev, "fast standby set failed\n");
1214                 goto out_cci_addr_fail;
1215         }
1216
1217         rval = smiapp_write(sensor, SMIAPP_REG_U8_CSI_SIGNALLING_MODE,
1218                             sensor->platform_data->csi_signalling_mode);
1219         if (rval) {
1220                 dev_err(&client->dev, "csi signalling mode set failed\n");
1221                 goto out_cci_addr_fail;
1222         }
1223
1224         /* DPHY control done by sensor based on requested link rate */
1225         rval = smiapp_write(sensor, SMIAPP_REG_U8_DPHY_CTRL,
1226                             SMIAPP_DPHY_CTRL_UI);
1227         if (rval < 0)
1228                 return rval;
1229
1230         rval = smiapp_call_quirk(sensor, post_poweron);
1231         if (rval) {
1232                 dev_err(&client->dev, "post_poweron quirks failed\n");
1233                 goto out_cci_addr_fail;
1234         }
1235
1236         /* Are we still initialising...? If yes, return here. */
1237         if (!sensor->pixel_array)
1238                 return 0;
1239
1240         rval = v4l2_ctrl_handler_setup(
1241                 &sensor->pixel_array->ctrl_handler);
1242         if (rval)
1243                 goto out_cci_addr_fail;
1244
1245         rval = v4l2_ctrl_handler_setup(&sensor->src->ctrl_handler);
1246         if (rval)
1247                 goto out_cci_addr_fail;
1248
1249         mutex_lock(&sensor->mutex);
1250         rval = smiapp_update_mode(sensor);
1251         mutex_unlock(&sensor->mutex);
1252         if (rval < 0)
1253                 goto out_cci_addr_fail;
1254
1255         return 0;
1256
1257 out_cci_addr_fail:
1258         if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
1259                 gpio_set_value(sensor->platform_data->xshutdown, 0);
1260         if (sensor->platform_data->set_xclk)
1261                 sensor->platform_data->set_xclk(&sensor->src->sd, 0);
1262         else
1263                 clk_disable(sensor->ext_clk);
1264
1265 out_xclk_fail:
1266         regulator_disable(sensor->vana);
1267         return rval;
1268 }
1269
1270 static void smiapp_power_off(struct smiapp_sensor *sensor)
1271 {
1272         /*
1273          * Currently power/clock to lens are enable/disabled separately
1274          * but they are essentially the same signals. So if the sensor is
1275          * powered off while the lens is powered on the sensor does not
1276          * really see a power off and next time the cci address change
1277          * will fail. So do a soft reset explicitly here.
1278          */
1279         if (sensor->platform_data->i2c_addr_alt)
1280                 smiapp_write(sensor,
1281                              SMIAPP_REG_U8_SOFTWARE_RESET,
1282                              SMIAPP_SOFTWARE_RESET);
1283
1284         if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
1285                 gpio_set_value(sensor->platform_data->xshutdown, 0);
1286         if (sensor->platform_data->set_xclk)
1287                 sensor->platform_data->set_xclk(&sensor->src->sd, 0);
1288         else
1289                 clk_disable(sensor->ext_clk);
1290         usleep_range(5000, 5000);
1291         regulator_disable(sensor->vana);
1292         sensor->streaming = 0;
1293 }
1294
1295 static int smiapp_set_power(struct v4l2_subdev *subdev, int on)
1296 {
1297         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1298         int ret = 0;
1299
1300         mutex_lock(&sensor->power_mutex);
1301
1302         /*
1303          * If the power count is modified from 0 to != 0 or from != 0
1304          * to 0, update the power state.
1305          */
1306         if (!sensor->power_count == !on)
1307                 goto out;
1308
1309         if (on) {
1310                 /* Power on and perform initialisation. */
1311                 ret = smiapp_power_on(sensor);
1312                 if (ret < 0)
1313                         goto out;
1314         } else {
1315                 smiapp_power_off(sensor);
1316         }
1317
1318         /* Update the power count. */
1319         sensor->power_count += on ? 1 : -1;
1320         WARN_ON(sensor->power_count < 0);
1321
1322 out:
1323         mutex_unlock(&sensor->power_mutex);
1324         return ret;
1325 }
1326
1327 /* -----------------------------------------------------------------------------
1328  * Video stream management
1329  */
1330
1331 static int smiapp_start_streaming(struct smiapp_sensor *sensor)
1332 {
1333         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
1334         int rval;
1335
1336         mutex_lock(&sensor->mutex);
1337
1338         rval = smiapp_write(sensor, SMIAPP_REG_U16_CSI_DATA_FORMAT,
1339                             (sensor->csi_format->width << 8) |
1340                             sensor->csi_format->compressed);
1341         if (rval)
1342                 goto out;
1343
1344         rval = smiapp_pll_configure(sensor);
1345         if (rval)
1346                 goto out;
1347
1348         /* Analog crop start coordinates */
1349         rval = smiapp_write(sensor, SMIAPP_REG_U16_X_ADDR_START,
1350                             sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left);
1351         if (rval < 0)
1352                 goto out;
1353
1354         rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_ADDR_START,
1355                             sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top);
1356         if (rval < 0)
1357                 goto out;
1358
1359         /* Analog crop end coordinates */
1360         rval = smiapp_write(
1361                 sensor, SMIAPP_REG_U16_X_ADDR_END,
1362                 sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].left
1363                 + sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width - 1);
1364         if (rval < 0)
1365                 goto out;
1366
1367         rval = smiapp_write(
1368                 sensor, SMIAPP_REG_U16_Y_ADDR_END,
1369                 sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].top
1370                 + sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height - 1);
1371         if (rval < 0)
1372                 goto out;
1373
1374         /*
1375          * Output from pixel array, including blanking, is set using
1376          * controls below. No need to set here.
1377          */
1378
1379         /* Digital crop */
1380         if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
1381             == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
1382                 rval = smiapp_write(
1383                         sensor, SMIAPP_REG_U16_DIGITAL_CROP_X_OFFSET,
1384                         sensor->scaler->crop[SMIAPP_PAD_SINK].left);
1385                 if (rval < 0)
1386                         goto out;
1387
1388                 rval = smiapp_write(
1389                         sensor, SMIAPP_REG_U16_DIGITAL_CROP_Y_OFFSET,
1390                         sensor->scaler->crop[SMIAPP_PAD_SINK].top);
1391                 if (rval < 0)
1392                         goto out;
1393
1394                 rval = smiapp_write(
1395                         sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_WIDTH,
1396                         sensor->scaler->crop[SMIAPP_PAD_SINK].width);
1397                 if (rval < 0)
1398                         goto out;
1399
1400                 rval = smiapp_write(
1401                         sensor, SMIAPP_REG_U16_DIGITAL_CROP_IMAGE_HEIGHT,
1402                         sensor->scaler->crop[SMIAPP_PAD_SINK].height);
1403                 if (rval < 0)
1404                         goto out;
1405         }
1406
1407         /* Scaling */
1408         if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
1409             != SMIAPP_SCALING_CAPABILITY_NONE) {
1410                 rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALING_MODE,
1411                                     sensor->scaling_mode);
1412                 if (rval < 0)
1413                         goto out;
1414
1415                 rval = smiapp_write(sensor, SMIAPP_REG_U16_SCALE_M,
1416                                     sensor->scale_m);
1417                 if (rval < 0)
1418                         goto out;
1419         }
1420
1421         /* Output size from sensor */
1422         rval = smiapp_write(sensor, SMIAPP_REG_U16_X_OUTPUT_SIZE,
1423                             sensor->src->crop[SMIAPP_PAD_SRC].width);
1424         if (rval < 0)
1425                 goto out;
1426         rval = smiapp_write(sensor, SMIAPP_REG_U16_Y_OUTPUT_SIZE,
1427                             sensor->src->crop[SMIAPP_PAD_SRC].height);
1428         if (rval < 0)
1429                 goto out;
1430
1431         if ((sensor->flash_capability &
1432              (SMIAPP_FLASH_MODE_CAPABILITY_SINGLE_STROBE |
1433               SMIAPP_FLASH_MODE_CAPABILITY_MULTIPLE_STROBE)) &&
1434             sensor->platform_data->strobe_setup != NULL &&
1435             sensor->platform_data->strobe_setup->trigger != 0) {
1436                 rval = smiapp_setup_flash_strobe(sensor);
1437                 if (rval)
1438                         goto out;
1439         }
1440
1441         rval = smiapp_call_quirk(sensor, pre_streamon);
1442         if (rval) {
1443                 dev_err(&client->dev, "pre_streamon quirks failed\n");
1444                 goto out;
1445         }
1446
1447         rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
1448                             SMIAPP_MODE_SELECT_STREAMING);
1449
1450 out:
1451         mutex_unlock(&sensor->mutex);
1452
1453         return rval;
1454 }
1455
1456 static int smiapp_stop_streaming(struct smiapp_sensor *sensor)
1457 {
1458         struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
1459         int rval;
1460
1461         mutex_lock(&sensor->mutex);
1462         rval = smiapp_write(sensor, SMIAPP_REG_U8_MODE_SELECT,
1463                             SMIAPP_MODE_SELECT_SOFTWARE_STANDBY);
1464         if (rval)
1465                 goto out;
1466
1467         rval = smiapp_call_quirk(sensor, post_streamoff);
1468         if (rval)
1469                 dev_err(&client->dev, "post_streamoff quirks failed\n");
1470
1471 out:
1472         mutex_unlock(&sensor->mutex);
1473         return rval;
1474 }
1475
1476 /* -----------------------------------------------------------------------------
1477  * V4L2 subdev video operations
1478  */
1479
1480 static int smiapp_set_stream(struct v4l2_subdev *subdev, int enable)
1481 {
1482         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1483         int rval;
1484
1485         if (sensor->streaming == enable)
1486                 return 0;
1487
1488         if (enable) {
1489                 sensor->streaming = 1;
1490                 rval = smiapp_start_streaming(sensor);
1491                 if (rval < 0)
1492                         sensor->streaming = 0;
1493         } else {
1494                 rval = smiapp_stop_streaming(sensor);
1495                 sensor->streaming = 0;
1496         }
1497
1498         return rval;
1499 }
1500
1501 static int smiapp_enum_mbus_code(struct v4l2_subdev *subdev,
1502                                  struct v4l2_subdev_fh *fh,
1503                                  struct v4l2_subdev_mbus_code_enum *code)
1504 {
1505         struct i2c_client *client = v4l2_get_subdevdata(subdev);
1506         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1507         unsigned int i;
1508         int idx = -1;
1509         int rval = -EINVAL;
1510
1511         mutex_lock(&sensor->mutex);
1512
1513         dev_err(&client->dev, "subdev %s, pad %d, index %d\n",
1514                 subdev->name, code->pad, code->index);
1515
1516         if (subdev != &sensor->src->sd || code->pad != SMIAPP_PAD_SRC) {
1517                 if (code->index)
1518                         goto out;
1519
1520                 code->code = sensor->internal_csi_format->code;
1521                 rval = 0;
1522                 goto out;
1523         }
1524
1525         for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
1526                 if (sensor->mbus_frame_fmts & (1 << i))
1527                         idx++;
1528
1529                 if (idx == code->index) {
1530                         code->code = smiapp_csi_data_formats[i].code;
1531                         dev_err(&client->dev, "found index %d, i %d, code %x\n",
1532                                 code->index, i, code->code);
1533                         rval = 0;
1534                         break;
1535                 }
1536         }
1537
1538 out:
1539         mutex_unlock(&sensor->mutex);
1540
1541         return rval;
1542 }
1543
1544 static u32 __smiapp_get_mbus_code(struct v4l2_subdev *subdev,
1545                                   unsigned int pad)
1546 {
1547         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1548
1549         if (subdev == &sensor->src->sd && pad == SMIAPP_PAD_SRC)
1550                 return sensor->csi_format->code;
1551         else
1552                 return sensor->internal_csi_format->code;
1553 }
1554
1555 static int __smiapp_get_format(struct v4l2_subdev *subdev,
1556                                struct v4l2_subdev_fh *fh,
1557                                struct v4l2_subdev_format *fmt)
1558 {
1559         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
1560
1561         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1562                 fmt->format = *v4l2_subdev_get_try_format(fh, fmt->pad);
1563         } else {
1564                 struct v4l2_rect *r;
1565
1566                 if (fmt->pad == ssd->source_pad)
1567                         r = &ssd->crop[ssd->source_pad];
1568                 else
1569                         r = &ssd->sink_fmt;
1570
1571                 fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
1572                 fmt->format.width = r->width;
1573                 fmt->format.height = r->height;
1574         }
1575
1576         return 0;
1577 }
1578
1579 static int smiapp_get_format(struct v4l2_subdev *subdev,
1580                              struct v4l2_subdev_fh *fh,
1581                              struct v4l2_subdev_format *fmt)
1582 {
1583         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1584         int rval;
1585
1586         mutex_lock(&sensor->mutex);
1587         rval = __smiapp_get_format(subdev, fh, fmt);
1588         mutex_unlock(&sensor->mutex);
1589
1590         return rval;
1591 }
1592
1593 static void smiapp_get_crop_compose(struct v4l2_subdev *subdev,
1594                                     struct v4l2_subdev_fh *fh,
1595                                     struct v4l2_rect **crops,
1596                                     struct v4l2_rect **comps, int which)
1597 {
1598         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
1599         unsigned int i;
1600
1601         if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1602                 if (crops)
1603                         for (i = 0; i < subdev->entity.num_pads; i++)
1604                                 crops[i] = &ssd->crop[i];
1605                 if (comps)
1606                         *comps = &ssd->compose;
1607         } else {
1608                 if (crops) {
1609                         for (i = 0; i < subdev->entity.num_pads; i++) {
1610                                 crops[i] = v4l2_subdev_get_try_crop(fh, i);
1611                                 BUG_ON(!crops[i]);
1612                         }
1613                 }
1614                 if (comps) {
1615                         *comps = v4l2_subdev_get_try_compose(fh,
1616                                                              SMIAPP_PAD_SINK);
1617                         BUG_ON(!*comps);
1618                 }
1619         }
1620 }
1621
1622 /* Changes require propagation only on sink pad. */
1623 static void smiapp_propagate(struct v4l2_subdev *subdev,
1624                              struct v4l2_subdev_fh *fh, int which,
1625                              int target)
1626 {
1627         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1628         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
1629         struct v4l2_rect *comp, *crops[SMIAPP_PADS];
1630
1631         smiapp_get_crop_compose(subdev, fh, crops, &comp, which);
1632
1633         switch (target) {
1634         case V4L2_SEL_TGT_CROP:
1635                 comp->width = crops[SMIAPP_PAD_SINK]->width;
1636                 comp->height = crops[SMIAPP_PAD_SINK]->height;
1637                 if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1638                         if (ssd == sensor->scaler) {
1639                                 sensor->scale_m =
1640                                         sensor->limits[
1641                                                 SMIAPP_LIMIT_SCALER_N_MIN];
1642                                 sensor->scaling_mode =
1643                                         SMIAPP_SCALING_MODE_NONE;
1644                         } else if (ssd == sensor->binner) {
1645                                 sensor->binning_horizontal = 1;
1646                                 sensor->binning_vertical = 1;
1647                         }
1648                 }
1649                 /* Fall through */
1650         case V4L2_SEL_TGT_COMPOSE:
1651                 *crops[SMIAPP_PAD_SRC] = *comp;
1652                 break;
1653         default:
1654                 BUG();
1655         }
1656 }
1657
1658 static const struct smiapp_csi_data_format
1659 *smiapp_validate_csi_data_format(struct smiapp_sensor *sensor, u32 code)
1660 {
1661         const struct smiapp_csi_data_format *csi_format = sensor->csi_format;
1662         unsigned int i;
1663
1664         for (i = 0; i < ARRAY_SIZE(smiapp_csi_data_formats); i++) {
1665                 if (sensor->mbus_frame_fmts & (1 << i)
1666                     && smiapp_csi_data_formats[i].code == code)
1667                         return &smiapp_csi_data_formats[i];
1668         }
1669
1670         return csi_format;
1671 }
1672
1673 static int smiapp_set_format(struct v4l2_subdev *subdev,
1674                              struct v4l2_subdev_fh *fh,
1675                              struct v4l2_subdev_format *fmt)
1676 {
1677         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1678         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
1679         struct v4l2_rect *crops[SMIAPP_PADS];
1680
1681         mutex_lock(&sensor->mutex);
1682
1683         /*
1684          * Media bus code is changeable on src subdev's source pad. On
1685          * other source pads we just get format here.
1686          */
1687         if (fmt->pad == ssd->source_pad) {
1688                 u32 code = fmt->format.code;
1689                 int rval = __smiapp_get_format(subdev, fh, fmt);
1690
1691                 if (!rval && subdev == &sensor->src->sd) {
1692                         const struct smiapp_csi_data_format *csi_format =
1693                                 smiapp_validate_csi_data_format(sensor, code);
1694                         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1695                                 sensor->csi_format = csi_format;
1696                         fmt->format.code = csi_format->code;
1697                 }
1698
1699                 mutex_unlock(&sensor->mutex);
1700                 return rval;
1701         }
1702
1703         /* Sink pad. Width and height are changeable here. */
1704         fmt->format.code = __smiapp_get_mbus_code(subdev, fmt->pad);
1705         fmt->format.width &= ~1;
1706         fmt->format.height &= ~1;
1707
1708         fmt->format.width =
1709                 clamp(fmt->format.width,
1710                       sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
1711                       sensor->limits[SMIAPP_LIMIT_MAX_X_OUTPUT_SIZE]);
1712         fmt->format.height =
1713                 clamp(fmt->format.height,
1714                       sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
1715                       sensor->limits[SMIAPP_LIMIT_MAX_Y_OUTPUT_SIZE]);
1716
1717         smiapp_get_crop_compose(subdev, fh, crops, NULL, fmt->which);
1718
1719         crops[ssd->sink_pad]->left = 0;
1720         crops[ssd->sink_pad]->top = 0;
1721         crops[ssd->sink_pad]->width = fmt->format.width;
1722         crops[ssd->sink_pad]->height = fmt->format.height;
1723         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1724                 ssd->sink_fmt = *crops[ssd->sink_pad];
1725         smiapp_propagate(subdev, fh, fmt->which,
1726                          V4L2_SEL_TGT_CROP);
1727
1728         mutex_unlock(&sensor->mutex);
1729
1730         return 0;
1731 }
1732
1733 /*
1734  * Calculate goodness of scaled image size compared to expected image
1735  * size and flags provided.
1736  */
1737 #define SCALING_GOODNESS                100000
1738 #define SCALING_GOODNESS_EXTREME        100000000
1739 static int scaling_goodness(struct v4l2_subdev *subdev, int w, int ask_w,
1740                             int h, int ask_h, u32 flags)
1741 {
1742         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1743         struct i2c_client *client = v4l2_get_subdevdata(subdev);
1744         int val = 0;
1745
1746         w &= ~1;
1747         ask_w &= ~1;
1748         h &= ~1;
1749         ask_h &= ~1;
1750
1751         if (flags & V4L2_SEL_FLAG_GE) {
1752                 if (w < ask_w)
1753                         val -= SCALING_GOODNESS;
1754                 if (h < ask_h)
1755                         val -= SCALING_GOODNESS;
1756         }
1757
1758         if (flags & V4L2_SEL_FLAG_LE) {
1759                 if (w > ask_w)
1760                         val -= SCALING_GOODNESS;
1761                 if (h > ask_h)
1762                         val -= SCALING_GOODNESS;
1763         }
1764
1765         val -= abs(w - ask_w);
1766         val -= abs(h - ask_h);
1767
1768         if (w < sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE])
1769                 val -= SCALING_GOODNESS_EXTREME;
1770
1771         dev_dbg(&client->dev, "w %d ask_w %d h %d ask_h %d goodness %d\n",
1772                 w, ask_h, h, ask_h, val);
1773
1774         return val;
1775 }
1776
1777 static void smiapp_set_compose_binner(struct v4l2_subdev *subdev,
1778                                       struct v4l2_subdev_fh *fh,
1779                                       struct v4l2_subdev_selection *sel,
1780                                       struct v4l2_rect **crops,
1781                                       struct v4l2_rect *comp)
1782 {
1783         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1784         unsigned int i;
1785         unsigned int binh = 1, binv = 1;
1786         unsigned int best = scaling_goodness(
1787                 subdev,
1788                 crops[SMIAPP_PAD_SINK]->width, sel->r.width,
1789                 crops[SMIAPP_PAD_SINK]->height, sel->r.height, sel->flags);
1790
1791         for (i = 0; i < sensor->nbinning_subtypes; i++) {
1792                 int this = scaling_goodness(
1793                         subdev,
1794                         crops[SMIAPP_PAD_SINK]->width
1795                         / sensor->binning_subtypes[i].horizontal,
1796                         sel->r.width,
1797                         crops[SMIAPP_PAD_SINK]->height
1798                         / sensor->binning_subtypes[i].vertical,
1799                         sel->r.height, sel->flags);
1800
1801                 if (this > best) {
1802                         binh = sensor->binning_subtypes[i].horizontal;
1803                         binv = sensor->binning_subtypes[i].vertical;
1804                         best = this;
1805                 }
1806         }
1807         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1808                 sensor->binning_vertical = binv;
1809                 sensor->binning_horizontal = binh;
1810         }
1811
1812         sel->r.width = (crops[SMIAPP_PAD_SINK]->width / binh) & ~1;
1813         sel->r.height = (crops[SMIAPP_PAD_SINK]->height / binv) & ~1;
1814 }
1815
1816 /*
1817  * Calculate best scaling ratio and mode for given output resolution.
1818  *
1819  * Try all of these: horizontal ratio, vertical ratio and smallest
1820  * size possible (horizontally).
1821  *
1822  * Also try whether horizontal scaler or full scaler gives a better
1823  * result.
1824  */
1825 static void smiapp_set_compose_scaler(struct v4l2_subdev *subdev,
1826                                       struct v4l2_subdev_fh *fh,
1827                                       struct v4l2_subdev_selection *sel,
1828                                       struct v4l2_rect **crops,
1829                                       struct v4l2_rect *comp)
1830 {
1831         struct i2c_client *client = v4l2_get_subdevdata(subdev);
1832         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1833         u32 min, max, a, b, max_m;
1834         u32 scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
1835         int mode = SMIAPP_SCALING_MODE_HORIZONTAL;
1836         u32 try[4];
1837         u32 ntry = 0;
1838         unsigned int i;
1839         int best = INT_MIN;
1840
1841         sel->r.width = min_t(unsigned int, sel->r.width,
1842                              crops[SMIAPP_PAD_SINK]->width);
1843         sel->r.height = min_t(unsigned int, sel->r.height,
1844                               crops[SMIAPP_PAD_SINK]->height);
1845
1846         a = crops[SMIAPP_PAD_SINK]->width
1847                 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.width;
1848         b = crops[SMIAPP_PAD_SINK]->height
1849                 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN] / sel->r.height;
1850         max_m = crops[SMIAPP_PAD_SINK]->width
1851                 * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]
1852                 / sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE];
1853
1854         a = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
1855                 max(a, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
1856         b = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
1857                 max(b, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
1858         max_m = min(sensor->limits[SMIAPP_LIMIT_SCALER_M_MAX],
1859                     max(max_m, sensor->limits[SMIAPP_LIMIT_SCALER_M_MIN]));
1860
1861         dev_dbg(&client->dev, "scaling: a %d b %d max_m %d\n", a, b, max_m);
1862
1863         min = min(max_m, min(a, b));
1864         max = min(max_m, max(a, b));
1865
1866         try[ntry] = min;
1867         ntry++;
1868         if (min != max) {
1869                 try[ntry] = max;
1870                 ntry++;
1871         }
1872         if (max != max_m) {
1873                 try[ntry] = min + 1;
1874                 ntry++;
1875                 if (min != max) {
1876                         try[ntry] = max + 1;
1877                         ntry++;
1878                 }
1879         }
1880
1881         for (i = 0; i < ntry; i++) {
1882                 int this = scaling_goodness(
1883                         subdev,
1884                         crops[SMIAPP_PAD_SINK]->width
1885                         / try[i]
1886                         * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
1887                         sel->r.width,
1888                         crops[SMIAPP_PAD_SINK]->height,
1889                         sel->r.height,
1890                         sel->flags);
1891
1892                 dev_dbg(&client->dev, "trying factor %d (%d)\n", try[i], i);
1893
1894                 if (this > best) {
1895                         scale_m = try[i];
1896                         mode = SMIAPP_SCALING_MODE_HORIZONTAL;
1897                         best = this;
1898                 }
1899
1900                 if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
1901                     == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
1902                         continue;
1903
1904                 this = scaling_goodness(
1905                         subdev, crops[SMIAPP_PAD_SINK]->width
1906                         / try[i]
1907                         * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
1908                         sel->r.width,
1909                         crops[SMIAPP_PAD_SINK]->height
1910                         / try[i]
1911                         * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN],
1912                         sel->r.height,
1913                         sel->flags);
1914
1915                 if (this > best) {
1916                         scale_m = try[i];
1917                         mode = SMIAPP_SCALING_MODE_BOTH;
1918                         best = this;
1919                 }
1920         }
1921
1922         sel->r.width =
1923                 (crops[SMIAPP_PAD_SINK]->width
1924                  / scale_m
1925                  * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN]) & ~1;
1926         if (mode == SMIAPP_SCALING_MODE_BOTH)
1927                 sel->r.height =
1928                         (crops[SMIAPP_PAD_SINK]->height
1929                          / scale_m
1930                          * sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN])
1931                         & ~1;
1932         else
1933                 sel->r.height = crops[SMIAPP_PAD_SINK]->height;
1934
1935         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1936                 sensor->scale_m = scale_m;
1937                 sensor->scaling_mode = mode;
1938         }
1939 }
1940 /* We're only called on source pads. This function sets scaling. */
1941 static int smiapp_set_compose(struct v4l2_subdev *subdev,
1942                               struct v4l2_subdev_fh *fh,
1943                               struct v4l2_subdev_selection *sel)
1944 {
1945         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1946         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
1947         struct v4l2_rect *comp, *crops[SMIAPP_PADS];
1948
1949         smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
1950
1951         sel->r.top = 0;
1952         sel->r.left = 0;
1953
1954         if (ssd == sensor->binner)
1955                 smiapp_set_compose_binner(subdev, fh, sel, crops, comp);
1956         else
1957                 smiapp_set_compose_scaler(subdev, fh, sel, crops, comp);
1958
1959         *comp = sel->r;
1960         smiapp_propagate(subdev, fh, sel->which,
1961                          V4L2_SEL_TGT_COMPOSE);
1962
1963         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1964                 return smiapp_update_mode(sensor);
1965
1966         return 0;
1967 }
1968
1969 static int __smiapp_sel_supported(struct v4l2_subdev *subdev,
1970                                   struct v4l2_subdev_selection *sel)
1971 {
1972         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
1973         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
1974
1975         /* We only implement crop in three places. */
1976         switch (sel->target) {
1977         case V4L2_SEL_TGT_CROP:
1978         case V4L2_SEL_TGT_CROP_BOUNDS:
1979                 if (ssd == sensor->pixel_array
1980                     && sel->pad == SMIAPP_PA_PAD_SRC)
1981                         return 0;
1982                 if (ssd == sensor->src
1983                     && sel->pad == SMIAPP_PAD_SRC)
1984                         return 0;
1985                 if (ssd == sensor->scaler
1986                     && sel->pad == SMIAPP_PAD_SINK
1987                     && sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
1988                     == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP)
1989                         return 0;
1990                 return -EINVAL;
1991         case V4L2_SEL_TGT_COMPOSE:
1992         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1993                 if (sel->pad == ssd->source_pad)
1994                         return -EINVAL;
1995                 if (ssd == sensor->binner)
1996                         return 0;
1997                 if (ssd == sensor->scaler
1998                     && sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
1999                     != SMIAPP_SCALING_CAPABILITY_NONE)
2000                         return 0;
2001                 /* Fall through */
2002         default:
2003                 return -EINVAL;
2004         }
2005 }
2006
2007 static int smiapp_set_crop(struct v4l2_subdev *subdev,
2008                            struct v4l2_subdev_fh *fh,
2009                            struct v4l2_subdev_selection *sel)
2010 {
2011         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2012         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
2013         struct v4l2_rect *src_size, *crops[SMIAPP_PADS];
2014         struct v4l2_rect _r;
2015
2016         smiapp_get_crop_compose(subdev, fh, crops, NULL, sel->which);
2017
2018         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
2019                 if (sel->pad == ssd->sink_pad)
2020                         src_size = &ssd->sink_fmt;
2021                 else
2022                         src_size = &ssd->compose;
2023         } else {
2024                 if (sel->pad == ssd->sink_pad) {
2025                         _r.left = 0;
2026                         _r.top = 0;
2027                         _r.width = v4l2_subdev_get_try_format(fh, sel->pad)
2028                                 ->width;
2029                         _r.height = v4l2_subdev_get_try_format(fh, sel->pad)
2030                                 ->height;
2031                         src_size = &_r;
2032                 } else {
2033                         src_size =
2034                                 v4l2_subdev_get_try_compose(
2035                                         fh, ssd->sink_pad);
2036                 }
2037         }
2038
2039         if (ssd == sensor->src && sel->pad == SMIAPP_PAD_SRC) {
2040                 sel->r.left = 0;
2041                 sel->r.top = 0;
2042         }
2043
2044         sel->r.width = min(sel->r.width, src_size->width);
2045         sel->r.height = min(sel->r.height, src_size->height);
2046
2047         sel->r.left = min(sel->r.left, src_size->width - sel->r.width);
2048         sel->r.top = min(sel->r.top, src_size->height - sel->r.height);
2049
2050         *crops[sel->pad] = sel->r;
2051
2052         if (ssd != sensor->pixel_array && sel->pad == SMIAPP_PAD_SINK)
2053                 smiapp_propagate(subdev, fh, sel->which,
2054                                  V4L2_SEL_TGT_CROP);
2055
2056         return 0;
2057 }
2058
2059 static int __smiapp_get_selection(struct v4l2_subdev *subdev,
2060                                   struct v4l2_subdev_fh *fh,
2061                                   struct v4l2_subdev_selection *sel)
2062 {
2063         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2064         struct smiapp_subdev *ssd = to_smiapp_subdev(subdev);
2065         struct v4l2_rect *comp, *crops[SMIAPP_PADS];
2066         struct v4l2_rect sink_fmt;
2067         int ret;
2068
2069         ret = __smiapp_sel_supported(subdev, sel);
2070         if (ret)
2071                 return ret;
2072
2073         smiapp_get_crop_compose(subdev, fh, crops, &comp, sel->which);
2074
2075         if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
2076                 sink_fmt = ssd->sink_fmt;
2077         } else {
2078                 struct v4l2_mbus_framefmt *fmt =
2079                         v4l2_subdev_get_try_format(fh, ssd->sink_pad);
2080
2081                 sink_fmt.left = 0;
2082                 sink_fmt.top = 0;
2083                 sink_fmt.width = fmt->width;
2084                 sink_fmt.height = fmt->height;
2085         }
2086
2087         switch (sel->target) {
2088         case V4L2_SEL_TGT_CROP_BOUNDS:
2089                 if (ssd == sensor->pixel_array) {
2090                         sel->r.width =
2091                                 sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
2092                         sel->r.height =
2093                                 sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
2094                 } else if (sel->pad == ssd->sink_pad) {
2095                         sel->r = sink_fmt;
2096                 } else {
2097                         sel->r = *comp;
2098                 }
2099                 break;
2100         case V4L2_SEL_TGT_CROP:
2101         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
2102                 sel->r = *crops[sel->pad];
2103                 break;
2104         case V4L2_SEL_TGT_COMPOSE:
2105                 sel->r = *comp;
2106                 break;
2107         }
2108
2109         return 0;
2110 }
2111
2112 static int smiapp_get_selection(struct v4l2_subdev *subdev,
2113                                 struct v4l2_subdev_fh *fh,
2114                                 struct v4l2_subdev_selection *sel)
2115 {
2116         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2117         int rval;
2118
2119         mutex_lock(&sensor->mutex);
2120         rval = __smiapp_get_selection(subdev, fh, sel);
2121         mutex_unlock(&sensor->mutex);
2122
2123         return rval;
2124 }
2125 static int smiapp_set_selection(struct v4l2_subdev *subdev,
2126                                 struct v4l2_subdev_fh *fh,
2127                                 struct v4l2_subdev_selection *sel)
2128 {
2129         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2130         int ret;
2131
2132         ret = __smiapp_sel_supported(subdev, sel);
2133         if (ret)
2134                 return ret;
2135
2136         mutex_lock(&sensor->mutex);
2137
2138         sel->r.left = max(0, sel->r.left & ~1);
2139         sel->r.top = max(0, sel->r.top & ~1);
2140         sel->r.width = max(0, SMIAPP_ALIGN_DIM(sel->r.width, sel->flags));
2141         sel->r.height = max(0, SMIAPP_ALIGN_DIM(sel->r.height, sel->flags));
2142
2143         sel->r.width = max_t(unsigned int,
2144                              sensor->limits[SMIAPP_LIMIT_MIN_X_OUTPUT_SIZE],
2145                              sel->r.width);
2146         sel->r.height = max_t(unsigned int,
2147                               sensor->limits[SMIAPP_LIMIT_MIN_Y_OUTPUT_SIZE],
2148                               sel->r.height);
2149
2150         switch (sel->target) {
2151         case V4L2_SEL_TGT_CROP:
2152                 ret = smiapp_set_crop(subdev, fh, sel);
2153                 break;
2154         case V4L2_SEL_TGT_COMPOSE:
2155                 ret = smiapp_set_compose(subdev, fh, sel);
2156                 break;
2157         default:
2158                 BUG();
2159         }
2160
2161         mutex_unlock(&sensor->mutex);
2162         return ret;
2163 }
2164
2165 static int smiapp_get_skip_frames(struct v4l2_subdev *subdev, u32 *frames)
2166 {
2167         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2168
2169         *frames = sensor->frame_skip;
2170         return 0;
2171 }
2172
2173 /* -----------------------------------------------------------------------------
2174  * sysfs attributes
2175  */
2176
2177 static ssize_t
2178 smiapp_sysfs_nvm_read(struct device *dev, struct device_attribute *attr,
2179                       char *buf)
2180 {
2181         struct v4l2_subdev *subdev = i2c_get_clientdata(to_i2c_client(dev));
2182         struct i2c_client *client = v4l2_get_subdevdata(subdev);
2183         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2184         unsigned int nbytes;
2185
2186         if (!sensor->dev_init_done)
2187                 return -EBUSY;
2188
2189         if (!sensor->nvm_size) {
2190                 /* NVM not read yet - read it now */
2191                 sensor->nvm_size = sensor->platform_data->nvm_size;
2192                 if (smiapp_set_power(subdev, 1) < 0)
2193                         return -ENODEV;
2194                 if (smiapp_read_nvm(sensor, sensor->nvm)) {
2195                         dev_err(&client->dev, "nvm read failed\n");
2196                         return -ENODEV;
2197                 }
2198                 smiapp_set_power(subdev, 0);
2199         }
2200         /*
2201          * NVM is still way below a PAGE_SIZE, so we can safely
2202          * assume this for now.
2203          */
2204         nbytes = min_t(unsigned int, sensor->nvm_size, PAGE_SIZE);
2205         memcpy(buf, sensor->nvm, nbytes);
2206
2207         return nbytes;
2208 }
2209 static DEVICE_ATTR(nvm, S_IRUGO, smiapp_sysfs_nvm_read, NULL);
2210
2211 /* -----------------------------------------------------------------------------
2212  * V4L2 subdev core operations
2213  */
2214
2215 static int smiapp_identify_module(struct v4l2_subdev *subdev)
2216 {
2217         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2218         struct i2c_client *client = v4l2_get_subdevdata(subdev);
2219         struct smiapp_module_info *minfo = &sensor->minfo;
2220         unsigned int i;
2221         int rval = 0;
2222
2223         minfo->name = SMIAPP_NAME;
2224
2225         /* Module info */
2226         rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MANUFACTURER_ID,
2227                                  &minfo->manufacturer_id);
2228         if (!rval)
2229                 rval = smiapp_read_8only(sensor, SMIAPP_REG_U16_MODEL_ID,
2230                                          &minfo->model_id);
2231         if (!rval)
2232                 rval = smiapp_read_8only(sensor,
2233                                          SMIAPP_REG_U8_REVISION_NUMBER_MAJOR,
2234                                          &minfo->revision_number_major);
2235         if (!rval)
2236                 rval = smiapp_read_8only(sensor,
2237                                          SMIAPP_REG_U8_REVISION_NUMBER_MINOR,
2238                                          &minfo->revision_number_minor);
2239         if (!rval)
2240                 rval = smiapp_read_8only(sensor,
2241                                          SMIAPP_REG_U8_MODULE_DATE_YEAR,
2242                                          &minfo->module_year);
2243         if (!rval)
2244                 rval = smiapp_read_8only(sensor,
2245                                          SMIAPP_REG_U8_MODULE_DATE_MONTH,
2246                                          &minfo->module_month);
2247         if (!rval)
2248                 rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_MODULE_DATE_DAY,
2249                                          &minfo->module_day);
2250
2251         /* Sensor info */
2252         if (!rval)
2253                 rval = smiapp_read_8only(sensor,
2254                                          SMIAPP_REG_U8_SENSOR_MANUFACTURER_ID,
2255                                          &minfo->sensor_manufacturer_id);
2256         if (!rval)
2257                 rval = smiapp_read_8only(sensor,
2258                                          SMIAPP_REG_U16_SENSOR_MODEL_ID,
2259                                          &minfo->sensor_model_id);
2260         if (!rval)
2261                 rval = smiapp_read_8only(sensor,
2262                                          SMIAPP_REG_U8_SENSOR_REVISION_NUMBER,
2263                                          &minfo->sensor_revision_number);
2264         if (!rval)
2265                 rval = smiapp_read_8only(sensor,
2266                                          SMIAPP_REG_U8_SENSOR_FIRMWARE_VERSION,
2267                                          &minfo->sensor_firmware_version);
2268
2269         /* SMIA */
2270         if (!rval)
2271                 rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIA_VERSION,
2272                                          &minfo->smia_version);
2273         if (!rval)
2274                 rval = smiapp_read_8only(sensor, SMIAPP_REG_U8_SMIAPP_VERSION,
2275                                          &minfo->smiapp_version);
2276
2277         if (rval) {
2278                 dev_err(&client->dev, "sensor detection failed\n");
2279                 return -ENODEV;
2280         }
2281
2282         dev_dbg(&client->dev, "module 0x%2.2x-0x%4.4x\n",
2283                 minfo->manufacturer_id, minfo->model_id);
2284
2285         dev_dbg(&client->dev,
2286                 "module revision 0x%2.2x-0x%2.2x date %2.2d-%2.2d-%2.2d\n",
2287                 minfo->revision_number_major, minfo->revision_number_minor,
2288                 minfo->module_year, minfo->module_month, minfo->module_day);
2289
2290         dev_dbg(&client->dev, "sensor 0x%2.2x-0x%4.4x\n",
2291                 minfo->sensor_manufacturer_id, minfo->sensor_model_id);
2292
2293         dev_dbg(&client->dev,
2294                 "sensor revision 0x%2.2x firmware version 0x%2.2x\n",
2295                 minfo->sensor_revision_number, minfo->sensor_firmware_version);
2296
2297         dev_dbg(&client->dev, "smia version %2.2d smiapp version %2.2d\n",
2298                 minfo->smia_version, minfo->smiapp_version);
2299
2300         /*
2301          * Some modules have bad data in the lvalues below. Hope the
2302          * rvalues have better stuff. The lvalues are module
2303          * parameters whereas the rvalues are sensor parameters.
2304          */
2305         if (!minfo->manufacturer_id && !minfo->model_id) {
2306                 minfo->manufacturer_id = minfo->sensor_manufacturer_id;
2307                 minfo->model_id = minfo->sensor_model_id;
2308                 minfo->revision_number_major = minfo->sensor_revision_number;
2309         }
2310
2311         for (i = 0; i < ARRAY_SIZE(smiapp_module_idents); i++) {
2312                 if (smiapp_module_idents[i].manufacturer_id
2313                     != minfo->manufacturer_id)
2314                         continue;
2315                 if (smiapp_module_idents[i].model_id != minfo->model_id)
2316                         continue;
2317                 if (smiapp_module_idents[i].flags
2318                     & SMIAPP_MODULE_IDENT_FLAG_REV_LE) {
2319                         if (smiapp_module_idents[i].revision_number_major
2320                             < minfo->revision_number_major)
2321                                 continue;
2322                 } else {
2323                         if (smiapp_module_idents[i].revision_number_major
2324                             != minfo->revision_number_major)
2325                                 continue;
2326                 }
2327
2328                 minfo->name = smiapp_module_idents[i].name;
2329                 minfo->quirk = smiapp_module_idents[i].quirk;
2330                 break;
2331         }
2332
2333         if (i >= ARRAY_SIZE(smiapp_module_idents))
2334                 dev_warn(&client->dev,
2335                          "no quirks for this module; let's hope it's fully compliant\n");
2336
2337         dev_dbg(&client->dev, "the sensor is called %s, ident %2.2x%4.4x%2.2x\n",
2338                 minfo->name, minfo->manufacturer_id, minfo->model_id,
2339                 minfo->revision_number_major);
2340
2341         strlcpy(subdev->name, sensor->minfo.name, sizeof(subdev->name));
2342
2343         return 0;
2344 }
2345
2346 static const struct v4l2_subdev_ops smiapp_ops;
2347 static const struct v4l2_subdev_internal_ops smiapp_internal_ops;
2348 static const struct media_entity_operations smiapp_entity_ops;
2349
2350 static int smiapp_registered(struct v4l2_subdev *subdev)
2351 {
2352         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2353         struct i2c_client *client = v4l2_get_subdevdata(subdev);
2354         struct smiapp_subdev *last = NULL;
2355         u32 tmp;
2356         unsigned int i;
2357         int rval;
2358
2359         sensor->vana = regulator_get(&client->dev, "VANA");
2360         if (IS_ERR(sensor->vana)) {
2361                 dev_err(&client->dev, "could not get regulator for vana\n");
2362                 return -ENODEV;
2363         }
2364
2365         if (!sensor->platform_data->set_xclk) {
2366                 sensor->ext_clk = clk_get(&client->dev,
2367                                           sensor->platform_data->ext_clk_name);
2368                 if (IS_ERR(sensor->ext_clk)) {
2369                         dev_err(&client->dev, "could not get clock %s\n",
2370                                 sensor->platform_data->ext_clk_name);
2371                         rval = -ENODEV;
2372                         goto out_clk_get;
2373                 }
2374
2375                 rval = clk_set_rate(sensor->ext_clk,
2376                                     sensor->platform_data->ext_clk);
2377                 if (rval < 0) {
2378                         dev_err(&client->dev,
2379                                 "unable to set clock %s freq to %u\n",
2380                                 sensor->platform_data->ext_clk_name,
2381                                 sensor->platform_data->ext_clk);
2382                         rval = -ENODEV;
2383                         goto out_clk_set_rate;
2384                 }
2385         }
2386
2387         if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN) {
2388                 if (gpio_request_one(sensor->platform_data->xshutdown, 0,
2389                                      "SMIA++ xshutdown") != 0) {
2390                         dev_err(&client->dev,
2391                                 "unable to acquire reset gpio %d\n",
2392                                 sensor->platform_data->xshutdown);
2393                         rval = -ENODEV;
2394                         goto out_clk_set_rate;
2395                 }
2396         }
2397
2398         rval = smiapp_power_on(sensor);
2399         if (rval) {
2400                 rval = -ENODEV;
2401                 goto out_smiapp_power_on;
2402         }
2403
2404         rval = smiapp_identify_module(subdev);
2405         if (rval) {
2406                 rval = -ENODEV;
2407                 goto out_power_off;
2408         }
2409
2410         rval = smiapp_get_all_limits(sensor);
2411         if (rval) {
2412                 rval = -ENODEV;
2413                 goto out_power_off;
2414         }
2415
2416         /*
2417          * Handle Sensor Module orientation on the board.
2418          *
2419          * The application of H-FLIP and V-FLIP on the sensor is modified by
2420          * the sensor orientation on the board.
2421          *
2422          * For SMIAPP_BOARD_SENSOR_ORIENT_180 the default behaviour is to set
2423          * both H-FLIP and V-FLIP for normal operation which also implies
2424          * that a set/unset operation for user space HFLIP and VFLIP v4l2
2425          * controls will need to be internally inverted.
2426          *
2427          * Rotation also changes the bayer pattern.
2428          */
2429         if (sensor->platform_data->module_board_orient ==
2430             SMIAPP_MODULE_BOARD_ORIENT_180)
2431                 sensor->hvflip_inv_mask = SMIAPP_IMAGE_ORIENTATION_HFLIP |
2432                                           SMIAPP_IMAGE_ORIENTATION_VFLIP;
2433
2434         rval = smiapp_get_mbus_formats(sensor);
2435         if (rval) {
2436                 rval = -ENODEV;
2437                 goto out_power_off;
2438         }
2439
2440         if (sensor->limits[SMIAPP_LIMIT_BINNING_CAPABILITY]) {
2441                 u32 val;
2442
2443                 rval = smiapp_read(sensor,
2444                                    SMIAPP_REG_U8_BINNING_SUBTYPES, &val);
2445                 if (rval < 0) {
2446                         rval = -ENODEV;
2447                         goto out_power_off;
2448                 }
2449                 sensor->nbinning_subtypes = min_t(u8, val,
2450                                                   SMIAPP_BINNING_SUBTYPES);
2451
2452                 for (i = 0; i < sensor->nbinning_subtypes; i++) {
2453                         rval = smiapp_read(
2454                                 sensor, SMIAPP_REG_U8_BINNING_TYPE_n(i), &val);
2455                         if (rval < 0) {
2456                                 rval = -ENODEV;
2457                                 goto out_power_off;
2458                         }
2459                         sensor->binning_subtypes[i] =
2460                                 *(struct smiapp_binning_subtype *)&val;
2461
2462                         dev_dbg(&client->dev, "binning %xx%x\n",
2463                                 sensor->binning_subtypes[i].horizontal,
2464                                 sensor->binning_subtypes[i].vertical);
2465                 }
2466         }
2467         sensor->binning_horizontal = 1;
2468         sensor->binning_vertical = 1;
2469
2470         /* SMIA++ NVM initialization - it will be read from the sensor
2471          * when it is first requested by userspace.
2472          */
2473         if (sensor->minfo.smiapp_version && sensor->platform_data->nvm_size) {
2474                 sensor->nvm = kzalloc(sensor->platform_data->nvm_size,
2475                                       GFP_KERNEL);
2476                 if (sensor->nvm == NULL) {
2477                         dev_err(&client->dev, "nvm buf allocation failed\n");
2478                         rval = -ENOMEM;
2479                         goto out_power_off;
2480                 }
2481
2482                 if (device_create_file(&client->dev, &dev_attr_nvm) != 0) {
2483                         dev_err(&client->dev, "sysfs nvm entry failed\n");
2484                         rval = -EBUSY;
2485                         goto out_power_off;
2486                 }
2487         }
2488
2489         rval = smiapp_call_quirk(sensor, limits);
2490         if (rval) {
2491                 dev_err(&client->dev, "limits quirks failed\n");
2492                 goto out_nvm_release;
2493         }
2494
2495         /* We consider this as profile 0 sensor if any of these are zero. */
2496         if (!sensor->limits[SMIAPP_LIMIT_MIN_OP_SYS_CLK_DIV] ||
2497             !sensor->limits[SMIAPP_LIMIT_MAX_OP_SYS_CLK_DIV] ||
2498             !sensor->limits[SMIAPP_LIMIT_MIN_OP_PIX_CLK_DIV] ||
2499             !sensor->limits[SMIAPP_LIMIT_MAX_OP_PIX_CLK_DIV]) {
2500                 sensor->minfo.smiapp_profile = SMIAPP_PROFILE_0;
2501         } else if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
2502                    != SMIAPP_SCALING_CAPABILITY_NONE) {
2503                 if (sensor->limits[SMIAPP_LIMIT_SCALING_CAPABILITY]
2504                     == SMIAPP_SCALING_CAPABILITY_HORIZONTAL)
2505                         sensor->minfo.smiapp_profile = SMIAPP_PROFILE_1;
2506                 else
2507                         sensor->minfo.smiapp_profile = SMIAPP_PROFILE_2;
2508                 sensor->scaler = &sensor->ssds[sensor->ssds_used];
2509                 sensor->ssds_used++;
2510         } else if (sensor->limits[SMIAPP_LIMIT_DIGITAL_CROP_CAPABILITY]
2511                    == SMIAPP_DIGITAL_CROP_CAPABILITY_INPUT_CROP) {
2512                 sensor->scaler = &sensor->ssds[sensor->ssds_used];
2513                 sensor->ssds_used++;
2514         }
2515         sensor->binner = &sensor->ssds[sensor->ssds_used];
2516         sensor->ssds_used++;
2517         sensor->pixel_array = &sensor->ssds[sensor->ssds_used];
2518         sensor->ssds_used++;
2519
2520         sensor->scale_m = sensor->limits[SMIAPP_LIMIT_SCALER_N_MIN];
2521
2522         for (i = 0; i < SMIAPP_SUBDEVS; i++) {
2523                 struct {
2524                         struct smiapp_subdev *ssd;
2525                         char *name;
2526                 } const __this[] = {
2527                         { sensor->scaler, "scaler", },
2528                         { sensor->binner, "binner", },
2529                         { sensor->pixel_array, "pixel array", },
2530                 }, *_this = &__this[i];
2531                 struct smiapp_subdev *this = _this->ssd;
2532
2533                 if (!this)
2534                         continue;
2535
2536                 if (this != sensor->src)
2537                         v4l2_subdev_init(&this->sd, &smiapp_ops);
2538
2539                 this->sensor = sensor;
2540
2541                 if (this == sensor->pixel_array) {
2542                         this->npads = 1;
2543                 } else {
2544                         this->npads = 2;
2545                         this->source_pad = 1;
2546                 }
2547
2548                 snprintf(this->sd.name,
2549                          sizeof(this->sd.name), "%s %s",
2550                          sensor->minfo.name, _this->name);
2551
2552                 this->sink_fmt.width =
2553                         sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
2554                 this->sink_fmt.height =
2555                         sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
2556                 this->compose.width = this->sink_fmt.width;
2557                 this->compose.height = this->sink_fmt.height;
2558                 this->crop[this->source_pad] = this->compose;
2559                 this->pads[this->source_pad].flags = MEDIA_PAD_FL_SOURCE;
2560                 if (this != sensor->pixel_array) {
2561                         this->crop[this->sink_pad] = this->compose;
2562                         this->pads[this->sink_pad].flags = MEDIA_PAD_FL_SINK;
2563                 }
2564
2565                 this->sd.entity.ops = &smiapp_entity_ops;
2566
2567                 if (last == NULL) {
2568                         last = this;
2569                         continue;
2570                 }
2571
2572                 this->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
2573                 this->sd.internal_ops = &smiapp_internal_ops;
2574                 this->sd.owner = NULL;
2575                 v4l2_set_subdevdata(&this->sd, client);
2576
2577                 rval = media_entity_init(&this->sd.entity,
2578                                          this->npads, this->pads, 0);
2579                 if (rval) {
2580                         dev_err(&client->dev,
2581                                 "media_entity_init failed\n");
2582                         goto out_nvm_release;
2583                 }
2584
2585                 rval = media_entity_create_link(&this->sd.entity,
2586                                                 this->source_pad,
2587                                                 &last->sd.entity,
2588                                                 last->sink_pad,
2589                                                 MEDIA_LNK_FL_ENABLED |
2590                                                 MEDIA_LNK_FL_IMMUTABLE);
2591                 if (rval) {
2592                         dev_err(&client->dev,
2593                                 "media_entity_create_link failed\n");
2594                         goto out_nvm_release;
2595                 }
2596
2597                 rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev,
2598                                                    &this->sd);
2599                 if (rval) {
2600                         dev_err(&client->dev,
2601                                 "v4l2_device_register_subdev failed\n");
2602                         goto out_nvm_release;
2603                 }
2604
2605                 last = this;
2606         }
2607
2608         dev_dbg(&client->dev, "profile %d\n", sensor->minfo.smiapp_profile);
2609
2610         sensor->pixel_array->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
2611
2612         /* final steps */
2613         smiapp_read_frame_fmt(sensor);
2614         rval = smiapp_init_controls(sensor);
2615         if (rval < 0)
2616                 goto out_nvm_release;
2617
2618         rval = smiapp_update_mode(sensor);
2619         if (rval) {
2620                 dev_err(&client->dev, "update mode failed\n");
2621                 goto out_nvm_release;
2622         }
2623
2624         sensor->streaming = false;
2625         sensor->dev_init_done = true;
2626
2627         /* check flash capability */
2628         rval = smiapp_read(sensor, SMIAPP_REG_U8_FLASH_MODE_CAPABILITY, &tmp);
2629         sensor->flash_capability = tmp;
2630         if (rval)
2631                 goto out_nvm_release;
2632
2633         smiapp_power_off(sensor);
2634
2635         return 0;
2636
2637 out_nvm_release:
2638         device_remove_file(&client->dev, &dev_attr_nvm);
2639
2640 out_power_off:
2641         kfree(sensor->nvm);
2642         sensor->nvm = NULL;
2643         smiapp_power_off(sensor);
2644
2645 out_smiapp_power_on:
2646         if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
2647                 gpio_free(sensor->platform_data->xshutdown);
2648
2649 out_clk_set_rate:
2650         clk_put(sensor->ext_clk);
2651         sensor->ext_clk = NULL;
2652
2653 out_clk_get:
2654         regulator_put(sensor->vana);
2655         sensor->vana = NULL;
2656         return rval;
2657 }
2658
2659 static int smiapp_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
2660 {
2661         struct smiapp_subdev *ssd = to_smiapp_subdev(sd);
2662         struct smiapp_sensor *sensor = ssd->sensor;
2663         u32 mbus_code =
2664                 smiapp_csi_data_formats[smiapp_pixel_order(sensor)].code;
2665         unsigned int i;
2666
2667         mutex_lock(&sensor->mutex);
2668
2669         for (i = 0; i < ssd->npads; i++) {
2670                 struct v4l2_mbus_framefmt *try_fmt =
2671                         v4l2_subdev_get_try_format(fh, i);
2672                 struct v4l2_rect *try_crop = v4l2_subdev_get_try_crop(fh, i);
2673                 struct v4l2_rect *try_comp;
2674
2675                 try_fmt->width = sensor->limits[SMIAPP_LIMIT_X_ADDR_MAX] + 1;
2676                 try_fmt->height = sensor->limits[SMIAPP_LIMIT_Y_ADDR_MAX] + 1;
2677                 try_fmt->code = mbus_code;
2678
2679                 try_crop->top = 0;
2680                 try_crop->left = 0;
2681                 try_crop->width = try_fmt->width;
2682                 try_crop->height = try_fmt->height;
2683
2684                 if (ssd != sensor->pixel_array)
2685                         continue;
2686
2687                 try_comp = v4l2_subdev_get_try_compose(fh, i);
2688                 *try_comp = *try_crop;
2689         }
2690
2691         mutex_unlock(&sensor->mutex);
2692
2693         return smiapp_set_power(sd, 1);
2694 }
2695
2696 static int smiapp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
2697 {
2698         return smiapp_set_power(sd, 0);
2699 }
2700
2701 static const struct v4l2_subdev_video_ops smiapp_video_ops = {
2702         .s_stream = smiapp_set_stream,
2703 };
2704
2705 static const struct v4l2_subdev_core_ops smiapp_core_ops = {
2706         .s_power = smiapp_set_power,
2707 };
2708
2709 static const struct v4l2_subdev_pad_ops smiapp_pad_ops = {
2710         .enum_mbus_code = smiapp_enum_mbus_code,
2711         .get_fmt = smiapp_get_format,
2712         .set_fmt = smiapp_set_format,
2713         .get_selection = smiapp_get_selection,
2714         .set_selection = smiapp_set_selection,
2715 };
2716
2717 static const struct v4l2_subdev_sensor_ops smiapp_sensor_ops = {
2718         .g_skip_frames = smiapp_get_skip_frames,
2719 };
2720
2721 static const struct v4l2_subdev_ops smiapp_ops = {
2722         .core = &smiapp_core_ops,
2723         .video = &smiapp_video_ops,
2724         .pad = &smiapp_pad_ops,
2725         .sensor = &smiapp_sensor_ops,
2726 };
2727
2728 static const struct media_entity_operations smiapp_entity_ops = {
2729         .link_validate = v4l2_subdev_link_validate,
2730 };
2731
2732 static const struct v4l2_subdev_internal_ops smiapp_internal_src_ops = {
2733         .registered = smiapp_registered,
2734         .open = smiapp_open,
2735         .close = smiapp_close,
2736 };
2737
2738 static const struct v4l2_subdev_internal_ops smiapp_internal_ops = {
2739         .open = smiapp_open,
2740         .close = smiapp_close,
2741 };
2742
2743 /* -----------------------------------------------------------------------------
2744  * I2C Driver
2745  */
2746
2747 #ifdef CONFIG_PM
2748
2749 static int smiapp_suspend(struct device *dev)
2750 {
2751         struct i2c_client *client = to_i2c_client(dev);
2752         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
2753         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2754         bool streaming;
2755
2756         BUG_ON(mutex_is_locked(&sensor->mutex));
2757
2758         if (sensor->power_count == 0)
2759                 return 0;
2760
2761         if (sensor->streaming)
2762                 smiapp_stop_streaming(sensor);
2763
2764         streaming = sensor->streaming;
2765
2766         smiapp_power_off(sensor);
2767
2768         /* save state for resume */
2769         sensor->streaming = streaming;
2770
2771         return 0;
2772 }
2773
2774 static int smiapp_resume(struct device *dev)
2775 {
2776         struct i2c_client *client = to_i2c_client(dev);
2777         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
2778         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2779         int rval;
2780
2781         if (sensor->power_count == 0)
2782                 return 0;
2783
2784         rval = smiapp_power_on(sensor);
2785         if (rval)
2786                 return rval;
2787
2788         if (sensor->streaming)
2789                 rval = smiapp_start_streaming(sensor);
2790
2791         return rval;
2792 }
2793
2794 #else
2795
2796 #define smiapp_suspend  NULL
2797 #define smiapp_resume   NULL
2798
2799 #endif /* CONFIG_PM */
2800
2801 static int smiapp_probe(struct i2c_client *client,
2802                         const struct i2c_device_id *devid)
2803 {
2804         struct smiapp_sensor *sensor;
2805         int rval;
2806
2807         if (client->dev.platform_data == NULL)
2808                 return -ENODEV;
2809
2810         sensor = kzalloc(sizeof(*sensor), GFP_KERNEL);
2811         if (sensor == NULL)
2812                 return -ENOMEM;
2813
2814         sensor->platform_data = client->dev.platform_data;
2815         mutex_init(&sensor->mutex);
2816         mutex_init(&sensor->power_mutex);
2817         sensor->src = &sensor->ssds[sensor->ssds_used];
2818
2819         v4l2_i2c_subdev_init(&sensor->src->sd, client, &smiapp_ops);
2820         sensor->src->sd.internal_ops = &smiapp_internal_src_ops;
2821         sensor->src->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
2822         sensor->src->sensor = sensor;
2823
2824         sensor->src->pads[0].flags = MEDIA_PAD_FL_SOURCE;
2825         rval = media_entity_init(&sensor->src->sd.entity, 2,
2826                                  sensor->src->pads, 0);
2827         if (rval < 0)
2828                 kfree(sensor);
2829
2830         return rval;
2831 }
2832
2833 static int __exit smiapp_remove(struct i2c_client *client)
2834 {
2835         struct v4l2_subdev *subdev = i2c_get_clientdata(client);
2836         struct smiapp_sensor *sensor = to_smiapp_sensor(subdev);
2837         unsigned int i;
2838
2839         if (sensor->power_count) {
2840                 if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
2841                         gpio_set_value(sensor->platform_data->xshutdown, 0);
2842                 if (sensor->platform_data->set_xclk)
2843                         sensor->platform_data->set_xclk(&sensor->src->sd, 0);
2844                 else
2845                         clk_disable(sensor->ext_clk);
2846                 sensor->power_count = 0;
2847         }
2848
2849         if (sensor->nvm) {
2850                 device_remove_file(&client->dev, &dev_attr_nvm);
2851                 kfree(sensor->nvm);
2852         }
2853
2854         for (i = 0; i < sensor->ssds_used; i++) {
2855                 media_entity_cleanup(&sensor->ssds[i].sd.entity);
2856                 v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
2857         }
2858         smiapp_free_controls(sensor);
2859         if (sensor->platform_data->xshutdown != SMIAPP_NO_XSHUTDOWN)
2860                 gpio_free(sensor->platform_data->xshutdown);
2861         if (sensor->ext_clk)
2862                 clk_put(sensor->ext_clk);
2863         if (sensor->vana)
2864                 regulator_put(sensor->vana);
2865
2866         kfree(sensor);
2867
2868         return 0;
2869 }
2870
2871 static const struct i2c_device_id smiapp_id_table[] = {
2872         { SMIAPP_NAME, 0 },
2873         { },
2874 };
2875 MODULE_DEVICE_TABLE(i2c, smiapp_id_table);
2876
2877 static const struct dev_pm_ops smiapp_pm_ops = {
2878         .suspend        = smiapp_suspend,
2879         .resume         = smiapp_resume,
2880 };
2881
2882 static struct i2c_driver smiapp_i2c_driver = {
2883         .driver = {
2884                 .name = SMIAPP_NAME,
2885                 .pm = &smiapp_pm_ops,
2886         },
2887         .probe  = smiapp_probe,
2888         .remove = __exit_p(smiapp_remove),
2889         .id_table = smiapp_id_table,
2890 };
2891
2892 module_i2c_driver(smiapp_i2c_driver);
2893
2894 MODULE_AUTHOR("Sakari Ailus <sakari.ailus@maxwell.research.nokia.com>");
2895 MODULE_DESCRIPTION("Generic SMIA/SMIA++ camera module driver");
2896 MODULE_LICENSE("GPL");