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

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