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