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