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