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