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

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