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

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