drivers/media/platform/atmel/atmel-isc-base.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Microchip Image Sensor Controller (ISC) common driver base
   4  *
   5  * Copyright (C) 2016-2019 Microchip Technology, Inc.
   6  *
   7  * Author: Songjun Wu
   8  * Author: Eugen Hristev <eugen.hristev@microchip.com>
   9  *
  10  */
  11
  12 #include <linux/clk.h>
  13 #include <linux/clkdev.h>
  14 #include <linux/clk-provider.h>
  15 #include <linux/delay.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/math64.h>
  18 #include <linux/module.h>
  19 #include <linux/of.h>
  20 #include <linux/of_graph.h>
  21 #include <linux/platform_device.h>
  22 #include <linux/pm_runtime.h>
  23 #include <linux/regmap.h>
  24 #include <linux/videodev2.h>
  25 #include <linux/atmel-isc-media.h>
  26
  27 #include <media/v4l2-ctrls.h>
  28 #include <media/v4l2-device.h>
  29 #include <media/v4l2-event.h>
  30 #include <media/v4l2-image-sizes.h>
  31 #include <media/v4l2-ioctl.h>
  32 #include <media/v4l2-fwnode.h>
  33 #include <media/v4l2-subdev.h>
  34 #include <media/videobuf2-dma-contig.h>
  35
  36 #include "atmel-isc-regs.h"
  37 #include "atmel-isc.h"
  38
  39 static unsigned int debug;
  40 module_param(debug, int, 0644);
  41 MODULE_PARM_DESC(debug, "debug level (0-2)");
  42
  43 static unsigned int sensor_preferred = 1;
  44 module_param(sensor_preferred, uint, 0644);
  45 MODULE_PARM_DESC(sensor_preferred,
  46                  "Sensor is preferred to output the specified format (1-on 0-off), default 1");
  47
  48 /* This is a list of the formats that the ISC can *output* */
  49 const struct isc_format controller_formats[] = {
  50         {
  51                 .fourcc         = V4L2_PIX_FMT_ARGB444,
  52         },
  53         {
  54                 .fourcc         = V4L2_PIX_FMT_ARGB555,
  55         },
  56         {
  57                 .fourcc         = V4L2_PIX_FMT_RGB565,
  58         },
  59         {
  60                 .fourcc         = V4L2_PIX_FMT_ABGR32,
  61         },
  62         {
  63                 .fourcc         = V4L2_PIX_FMT_XBGR32,
  64         },
  65         {
  66                 .fourcc         = V4L2_PIX_FMT_YUV420,
  67         },
  68         {
  69                 .fourcc         = V4L2_PIX_FMT_YUYV,
  70         },
  71         {
  72                 .fourcc         = V4L2_PIX_FMT_YUV422P,
  73         },
  74         {
  75                 .fourcc         = V4L2_PIX_FMT_GREY,
  76         },
  77         {
  78                 .fourcc         = V4L2_PIX_FMT_Y10,
  79         },
  80 };
  81
  82 /* This is a list of formats that the ISC can receive as *input* */
  83 struct isc_format formats_list[] = {
  84         {
  85                 .fourcc         = V4L2_PIX_FMT_SBGGR8,
  86                 .mbus_code      = MEDIA_BUS_FMT_SBGGR8_1X8,
  87                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
  88                 .cfa_baycfg     = ISC_BAY_CFG_BGBG,
  89         },
  90         {
  91                 .fourcc         = V4L2_PIX_FMT_SGBRG8,
  92                 .mbus_code      = MEDIA_BUS_FMT_SGBRG8_1X8,
  93                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
  94                 .cfa_baycfg     = ISC_BAY_CFG_GBGB,
  95         },
  96         {
  97                 .fourcc         = V4L2_PIX_FMT_SGRBG8,
  98                 .mbus_code      = MEDIA_BUS_FMT_SGRBG8_1X8,
  99                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
 100                 .cfa_baycfg     = ISC_BAY_CFG_GRGR,
 101         },
 102         {
 103                 .fourcc         = V4L2_PIX_FMT_SRGGB8,
 104                 .mbus_code      = MEDIA_BUS_FMT_SRGGB8_1X8,
 105                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
 106                 .cfa_baycfg     = ISC_BAY_CFG_RGRG,
 107         },
 108         {
 109                 .fourcc         = V4L2_PIX_FMT_SBGGR10,
 110                 .mbus_code      = MEDIA_BUS_FMT_SBGGR10_1X10,
 111                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TEN,
 112                 .cfa_baycfg     = ISC_BAY_CFG_RGRG,
 113         },
 114         {
 115                 .fourcc         = V4L2_PIX_FMT_SGBRG10,
 116                 .mbus_code      = MEDIA_BUS_FMT_SGBRG10_1X10,
 117                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TEN,
 118                 .cfa_baycfg     = ISC_BAY_CFG_GBGB,
 119         },
 120         {
 121                 .fourcc         = V4L2_PIX_FMT_SGRBG10,
 122                 .mbus_code      = MEDIA_BUS_FMT_SGRBG10_1X10,
 123                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TEN,
 124                 .cfa_baycfg     = ISC_BAY_CFG_GRGR,
 125         },
 126         {
 127                 .fourcc         = V4L2_PIX_FMT_SRGGB10,
 128                 .mbus_code      = MEDIA_BUS_FMT_SRGGB10_1X10,
 129                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TEN,
 130                 .cfa_baycfg     = ISC_BAY_CFG_RGRG,
 131         },
 132         {
 133                 .fourcc         = V4L2_PIX_FMT_SBGGR12,
 134                 .mbus_code      = MEDIA_BUS_FMT_SBGGR12_1X12,
 135                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TWELVE,
 136                 .cfa_baycfg     = ISC_BAY_CFG_BGBG,
 137         },
 138         {
 139                 .fourcc         = V4L2_PIX_FMT_SGBRG12,
 140                 .mbus_code      = MEDIA_BUS_FMT_SGBRG12_1X12,
 141                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TWELVE,
 142                 .cfa_baycfg     = ISC_BAY_CFG_GBGB,
 143         },
 144         {
 145                 .fourcc         = V4L2_PIX_FMT_SGRBG12,
 146                 .mbus_code      = MEDIA_BUS_FMT_SGRBG12_1X12,
 147                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TWELVE,
 148                 .cfa_baycfg     = ISC_BAY_CFG_GRGR,
 149         },
 150         {
 151                 .fourcc         = V4L2_PIX_FMT_SRGGB12,
 152                 .mbus_code      = MEDIA_BUS_FMT_SRGGB12_1X12,
 153                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TWELVE,
 154                 .cfa_baycfg     = ISC_BAY_CFG_RGRG,
 155         },
 156         {
 157                 .fourcc         = V4L2_PIX_FMT_GREY,
 158                 .mbus_code      = MEDIA_BUS_FMT_Y8_1X8,
 159                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
 160         },
 161         {
 162                 .fourcc         = V4L2_PIX_FMT_YUYV,
 163                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
 164                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
 165         },
 166         {
 167                 .fourcc         = V4L2_PIX_FMT_RGB565,
 168                 .mbus_code      = MEDIA_BUS_FMT_RGB565_2X8_LE,
 169                 .pfe_cfg0_bps   = ISC_PFE_CFG0_BPS_EIGHT,
 170         },
 171         {
 172                 .fourcc         = V4L2_PIX_FMT_Y10,
 173                 .mbus_code      = MEDIA_BUS_FMT_Y10_1X10,
 174                 .pfe_cfg0_bps   = ISC_PFG_CFG0_BPS_TEN,
 175         },
 176
 177 };
 178
 179 /* Gamma table with gamma 1/2.2 */
 180 const u32 isc_gamma_table[GAMMA_MAX + 1][GAMMA_ENTRIES] = {
 181         /* 0 --> gamma 1/1.8 */
 182         {      0x65,  0x66002F,  0x950025,  0xBB0020,  0xDB001D,  0xF8001A,
 183           0x1130018, 0x12B0017, 0x1420016, 0x1580014, 0x16D0013, 0x1810012,
 184           0x1940012, 0x1A60012, 0x1B80011, 0x1C90010, 0x1DA0010, 0x1EA000F,
 185           0x1FA000F, 0x209000F, 0x218000F, 0x227000E, 0x235000E, 0x243000E,
 186           0x251000E, 0x25F000D, 0x26C000D, 0x279000D, 0x286000D, 0x293000C,
 187           0x2A0000C, 0x2AC000C, 0x2B8000C, 0x2C4000C, 0x2D0000B, 0x2DC000B,
 188           0x2E7000B, 0x2F3000B, 0x2FE000B, 0x309000B, 0x314000B, 0x31F000A,
 189           0x32A000A, 0x334000B, 0x33F000A, 0x349000A, 0x354000A, 0x35E000A,
 190           0x368000A, 0x372000A, 0x37C000A, 0x386000A, 0x3900009, 0x399000A,
 191           0x3A30009, 0x3AD0009, 0x3B60009, 0x3BF000A, 0x3C90009, 0x3D20009,
 192           0x3DB0009, 0x3E40009, 0x3ED0009, 0x3F60009 },
 193
 194         /* 1 --> gamma 1/2 */
 195         {      0x7F,  0x800034,  0xB50028,  0xDE0021, 0x100001E, 0x11E001B,
 196           0x1390019, 0x1520017, 0x16A0015, 0x1800014, 0x1940014, 0x1A80013,
 197           0x1BB0012, 0x1CD0011, 0x1DF0010, 0x1EF0010, 0x200000F, 0x20F000F,
 198           0x21F000E, 0x22D000F, 0x23C000E, 0x24A000E, 0x258000D, 0x265000D,
 199           0x273000C, 0x27F000D, 0x28C000C, 0x299000C, 0x2A5000C, 0x2B1000B,
 200           0x2BC000C, 0x2C8000B, 0x2D3000C, 0x2DF000B, 0x2EA000A, 0x2F5000A,
 201           0x2FF000B, 0x30A000A, 0x314000B, 0x31F000A, 0x329000A, 0x333000A,
 202           0x33D0009, 0x3470009, 0x350000A, 0x35A0009, 0x363000A, 0x36D0009,
 203           0x3760009, 0x37F0009, 0x3880009, 0x3910009, 0x39A0009, 0x3A30009,
 204           0x3AC0008, 0x3B40009, 0x3BD0008, 0x3C60008, 0x3CE0008, 0x3D60009,
 205           0x3DF0008, 0x3E70008, 0x3EF0008, 0x3F70008 },
 206
 207         /* 2 --> gamma 1/2.2 */
 208         {      0x99,  0x9B0038,  0xD4002A,  0xFF0023, 0x122001F, 0x141001B,
 209           0x15D0019, 0x1760017, 0x18E0015, 0x1A30015, 0x1B80013, 0x1CC0012,
 210           0x1DE0011, 0x1F00010, 0x2010010, 0x2110010, 0x221000F, 0x230000F,
 211           0x23F000E, 0x24D000E, 0x25B000D, 0x269000C, 0x276000C, 0x283000C,
 212           0x28F000C, 0x29B000C, 0x2A7000C, 0x2B3000B, 0x2BF000B, 0x2CA000B,
 213           0x2D5000B, 0x2E0000A, 0x2EB000A, 0x2F5000A, 0x2FF000A, 0x30A000A,
 214           0x3140009, 0x31E0009, 0x327000A, 0x3310009, 0x33A0009, 0x3440009,
 215           0x34D0009, 0x3560009, 0x35F0009, 0x3680008, 0x3710008, 0x3790009,
 216           0x3820008, 0x38A0008, 0x3930008, 0x39B0008, 0x3A30008, 0x3AB0008,
 217           0x3B30008, 0x3BB0008, 0x3C30008, 0x3CB0007, 0x3D20008, 0x3DA0007,
 218           0x3E20007, 0x3E90007, 0x3F00008, 0x3F80007 },
 219 };
 220
 221 #define ISC_IS_FORMAT_RAW(mbus_code) \
 222         (((mbus_code) & 0xf000) == 0x3000)
 223
 224 #define ISC_IS_FORMAT_GREY(mbus_code) \
 225         (((mbus_code) == MEDIA_BUS_FMT_Y10_1X10) | \
 226         (((mbus_code) == MEDIA_BUS_FMT_Y8_1X8)))
 227
 228 static inline void isc_update_v4l2_ctrls(struct isc_device *isc)
 229 {
 230         struct isc_ctrls *ctrls = &isc->ctrls;
 231
 232         /* In here we set the v4l2 controls w.r.t. our pipeline config */
 233         v4l2_ctrl_s_ctrl(isc->r_gain_ctrl, ctrls->gain[ISC_HIS_CFG_MODE_R]);
 234         v4l2_ctrl_s_ctrl(isc->b_gain_ctrl, ctrls->gain[ISC_HIS_CFG_MODE_B]);
 235         v4l2_ctrl_s_ctrl(isc->gr_gain_ctrl, ctrls->gain[ISC_HIS_CFG_MODE_GR]);
 236         v4l2_ctrl_s_ctrl(isc->gb_gain_ctrl, ctrls->gain[ISC_HIS_CFG_MODE_GB]);
 237
 238         v4l2_ctrl_s_ctrl(isc->r_off_ctrl, ctrls->offset[ISC_HIS_CFG_MODE_R]);
 239         v4l2_ctrl_s_ctrl(isc->b_off_ctrl, ctrls->offset[ISC_HIS_CFG_MODE_B]);
 240         v4l2_ctrl_s_ctrl(isc->gr_off_ctrl, ctrls->offset[ISC_HIS_CFG_MODE_GR]);
 241         v4l2_ctrl_s_ctrl(isc->gb_off_ctrl, ctrls->offset[ISC_HIS_CFG_MODE_GB]);
 242 }
 243
 244 static inline void isc_update_awb_ctrls(struct isc_device *isc)
 245 {
 246         struct isc_ctrls *ctrls = &isc->ctrls;
 247
 248         /* In here we set our actual hw pipeline config */
 249
 250         regmap_write(isc->regmap, ISC_WB_O_RGR,
 251                      ((ctrls->offset[ISC_HIS_CFG_MODE_R])) |
 252                      ((ctrls->offset[ISC_HIS_CFG_MODE_GR]) << 16));
 253         regmap_write(isc->regmap, ISC_WB_O_BGB,
 254                      ((ctrls->offset[ISC_HIS_CFG_MODE_B])) |
 255                      ((ctrls->offset[ISC_HIS_CFG_MODE_GB]) << 16));
 256         regmap_write(isc->regmap, ISC_WB_G_RGR,
 257                      ctrls->gain[ISC_HIS_CFG_MODE_R] |
 258                      (ctrls->gain[ISC_HIS_CFG_MODE_GR] << 16));
 259         regmap_write(isc->regmap, ISC_WB_G_BGB,
 260                      ctrls->gain[ISC_HIS_CFG_MODE_B] |
 261                      (ctrls->gain[ISC_HIS_CFG_MODE_GB] << 16));
 262 }
 263
 264 static inline void isc_reset_awb_ctrls(struct isc_device *isc)
 265 {
 266         unsigned int c;
 267
 268         for (c = ISC_HIS_CFG_MODE_GR; c <= ISC_HIS_CFG_MODE_B; c++) {
 269                 /* gains have a fixed point at 9 decimals */
 270                 isc->ctrls.gain[c] = 1 << 9;
 271                 /* offsets are in 2's complements */
 272                 isc->ctrls.offset[c] = 0;
 273         }
 274 }
 275
 276 static int isc_wait_clk_stable(struct clk_hw *hw)
 277 {
 278         struct isc_clk *isc_clk = to_isc_clk(hw);
 279         struct regmap *regmap = isc_clk->regmap;
 280         unsigned long timeout = jiffies + usecs_to_jiffies(1000);
 281         unsigned int status;
 282
 283         while (time_before(jiffies, timeout)) {
 284                 regmap_read(regmap, ISC_CLKSR, &status);
 285                 if (!(status & ISC_CLKSR_SIP))
 286                         return 0;
 287
 288                 usleep_range(10, 250);
 289         }
 290
 291         return -ETIMEDOUT;
 292 }
 293
 294 static int isc_clk_prepare(struct clk_hw *hw)
 295 {
 296         struct isc_clk *isc_clk = to_isc_clk(hw);
 297
 298         if (isc_clk->id == ISC_ISPCK)
 299                 pm_runtime_get_sync(isc_clk->dev);
 300
 301         return isc_wait_clk_stable(hw);
 302 }
 303
 304 static void isc_clk_unprepare(struct clk_hw *hw)
 305 {
 306         struct isc_clk *isc_clk = to_isc_clk(hw);
 307
 308         isc_wait_clk_stable(hw);
 309
 310         if (isc_clk->id == ISC_ISPCK)
 311                 pm_runtime_put_sync(isc_clk->dev);
 312 }
 313
 314 static int isc_clk_enable(struct clk_hw *hw)
 315 {
 316         struct isc_clk *isc_clk = to_isc_clk(hw);
 317         u32 id = isc_clk->id;
 318         struct regmap *regmap = isc_clk->regmap;
 319         unsigned long flags;
 320         unsigned int status;
 321
 322         dev_dbg(isc_clk->dev, "ISC CLK: %s, div = %d, parent id = %d\n",
 323                 __func__, isc_clk->div, isc_clk->parent_id);
 324
 325         spin_lock_irqsave(&isc_clk->lock, flags);
 326         regmap_update_bits(regmap, ISC_CLKCFG,
 327                            ISC_CLKCFG_DIV_MASK(id) | ISC_CLKCFG_SEL_MASK(id),
 328                            (isc_clk->div << ISC_CLKCFG_DIV_SHIFT(id)) |
 329                            (isc_clk->parent_id << ISC_CLKCFG_SEL_SHIFT(id)));
 330
 331         regmap_write(regmap, ISC_CLKEN, ISC_CLK(id));
 332         spin_unlock_irqrestore(&isc_clk->lock, flags);
 333
 334         regmap_read(regmap, ISC_CLKSR, &status);
 335         if (status & ISC_CLK(id))
 336                 return 0;
 337         else
 338                 return -EINVAL;
 339 }
 340
 341 static void isc_clk_disable(struct clk_hw *hw)
 342 {
 343         struct isc_clk *isc_clk = to_isc_clk(hw);
 344         u32 id = isc_clk->id;
 345         unsigned long flags;
 346
 347         spin_lock_irqsave(&isc_clk->lock, flags);
 348         regmap_write(isc_clk->regmap, ISC_CLKDIS, ISC_CLK(id));
 349         spin_unlock_irqrestore(&isc_clk->lock, flags);
 350 }
 351
 352 static int isc_clk_is_enabled(struct clk_hw *hw)
 353 {
 354         struct isc_clk *isc_clk = to_isc_clk(hw);
 355         u32 status;
 356
 357         if (isc_clk->id == ISC_ISPCK)
 358                 pm_runtime_get_sync(isc_clk->dev);
 359
 360         regmap_read(isc_clk->regmap, ISC_CLKSR, &status);
 361
 362         if (isc_clk->id == ISC_ISPCK)
 363                 pm_runtime_put_sync(isc_clk->dev);
 364
 365         return status & ISC_CLK(isc_clk->id) ? 1 : 0;
 366 }
 367
 368 static unsigned long
 369 isc_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 370 {
 371         struct isc_clk *isc_clk = to_isc_clk(hw);
 372
 373         return DIV_ROUND_CLOSEST(parent_rate, isc_clk->div + 1);
 374 }
 375
 376 static int isc_clk_determine_rate(struct clk_hw *hw,
 377                                    struct clk_rate_request *req)
 378 {
 379         struct isc_clk *isc_clk = to_isc_clk(hw);
 380         long best_rate = -EINVAL;
 381         int best_diff = -1;
 382         unsigned int i, div;
 383
 384         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 385                 struct clk_hw *parent;
 386                 unsigned long parent_rate;
 387
 388                 parent = clk_hw_get_parent_by_index(hw, i);
 389                 if (!parent)
 390                         continue;
 391
 392                 parent_rate = clk_hw_get_rate(parent);
 393                 if (!parent_rate)
 394                         continue;
 395
 396                 for (div = 1; div < ISC_CLK_MAX_DIV + 2; div++) {
 397                         unsigned long rate;
 398                         int diff;
 399
 400                         rate = DIV_ROUND_CLOSEST(parent_rate, div);
 401                         diff = abs(req->rate - rate);
 402
 403                         if (best_diff < 0 || best_diff > diff) {
 404                                 best_rate = rate;
 405                                 best_diff = diff;
 406                                 req->best_parent_rate = parent_rate;
 407                                 req->best_parent_hw = parent;
 408                         }
 409
 410                         if (!best_diff || rate < req->rate)
 411                                 break;
 412                 }
 413
 414                 if (!best_diff)
 415                         break;
 416         }
 417
 418         dev_dbg(isc_clk->dev,
 419                 "ISC CLK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
 420                 __func__, best_rate,
 421                 __clk_get_name((req->best_parent_hw)->clk),
 422                 req->best_parent_rate);
 423
 424         if (best_rate < 0)
 425                 return best_rate;
 426
 427         req->rate = best_rate;
 428
 429         return 0;
 430 }
 431
 432 static int isc_clk_set_parent(struct clk_hw *hw, u8 index)
 433 {
 434         struct isc_clk *isc_clk = to_isc_clk(hw);
 435
 436         if (index >= clk_hw_get_num_parents(hw))
 437                 return -EINVAL;
 438
 439         isc_clk->parent_id = index;
 440
 441         return 0;
 442 }
 443
 444 static u8 isc_clk_get_parent(struct clk_hw *hw)
 445 {
 446         struct isc_clk *isc_clk = to_isc_clk(hw);
 447
 448         return isc_clk->parent_id;
 449 }
 450
 451 static int isc_clk_set_rate(struct clk_hw *hw,
 452                              unsigned long rate,
 453                              unsigned long parent_rate)
 454 {
 455         struct isc_clk *isc_clk = to_isc_clk(hw);
 456         u32 div;
 457
 458         if (!rate)
 459                 return -EINVAL;
 460
 461         div = DIV_ROUND_CLOSEST(parent_rate, rate);
 462         if (div > (ISC_CLK_MAX_DIV + 1) || !div)
 463                 return -EINVAL;
 464
 465         isc_clk->div = div - 1;
 466
 467         return 0;
 468 }
 469
 470 static const struct clk_ops isc_clk_ops = {
 471         .prepare        = isc_clk_prepare,
 472         .unprepare      = isc_clk_unprepare,
 473         .enable         = isc_clk_enable,
 474         .disable        = isc_clk_disable,
 475         .is_enabled     = isc_clk_is_enabled,
 476         .recalc_rate    = isc_clk_recalc_rate,
 477         .determine_rate = isc_clk_determine_rate,
 478         .set_parent     = isc_clk_set_parent,
 479         .get_parent     = isc_clk_get_parent,
 480         .set_rate       = isc_clk_set_rate,
 481 };
 482
 483 static int isc_clk_register(struct isc_device *isc, unsigned int id)
 484 {
 485         struct regmap *regmap = isc->regmap;
 486         struct device_node *np = isc->dev->of_node;
 487         struct isc_clk *isc_clk;
 488         struct clk_init_data init;
 489         const char *clk_name = np->name;
 490         const char *parent_names[3];
 491         int num_parents;
 492
 493         num_parents = of_clk_get_parent_count(np);
 494         if (num_parents < 1 || num_parents > 3)
 495                 return -EINVAL;
 496
 497         if (num_parents > 2 && id == ISC_ISPCK)
 498                 num_parents = 2;
 499
 500         of_clk_parent_fill(np, parent_names, num_parents);
 501
 502         if (id == ISC_MCK)
 503                 of_property_read_string(np, "clock-output-names", &clk_name);
 504         else
 505                 clk_name = "isc-ispck";
 506
 507         init.parent_names       = parent_names;
 508         init.num_parents        = num_parents;
 509         init.name               = clk_name;
 510         init.ops                = &isc_clk_ops;
 511         init.flags              = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
 512
 513         isc_clk = &isc->isc_clks[id];
 514         isc_clk->hw.init        = &init;
 515         isc_clk->regmap         = regmap;
 516         isc_clk->id             = id;
 517         isc_clk->dev            = isc->dev;
 518         spin_lock_init(&isc_clk->lock);
 519
 520         isc_clk->clk = clk_register(isc->dev, &isc_clk->hw);
 521         if (IS_ERR(isc_clk->clk)) {
 522                 dev_err(isc->dev, "%s: clock register fail\n", clk_name);
 523                 return PTR_ERR(isc_clk->clk);
 524         } else if (id == ISC_MCK)
 525                 of_clk_add_provider(np, of_clk_src_simple_get, isc_clk->clk);
 526
 527         return 0;
 528 }
 529
 530 int isc_clk_init(struct isc_device *isc)
 531 {
 532         unsigned int i;
 533         int ret;
 534
 535         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++)
 536                 isc->isc_clks[i].clk = ERR_PTR(-EINVAL);
 537
 538         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
 539                 ret = isc_clk_register(isc, i);
 540                 if (ret)
 541                         return ret;
 542         }
 543
 544         return 0;
 545 }
 546
 547 void isc_clk_cleanup(struct isc_device *isc)
 548 {
 549         unsigned int i;
 550
 551         of_clk_del_provider(isc->dev->of_node);
 552
 553         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
 554                 struct isc_clk *isc_clk = &isc->isc_clks[i];
 555
 556                 if (!IS_ERR(isc_clk->clk))
 557                         clk_unregister(isc_clk->clk);
 558         }
 559 }
 560
 561 static int isc_queue_setup(struct vb2_queue *vq,
 562                             unsigned int *nbuffers, unsigned int *nplanes,
 563                             unsigned int sizes[], struct device *alloc_devs[])
 564 {
 565         struct isc_device *isc = vb2_get_drv_priv(vq);
 566         unsigned int size = isc->fmt.fmt.pix.sizeimage;
 567
 568         if (*nplanes)
 569                 return sizes[0] < size ? -EINVAL : 0;
 570
 571         *nplanes = 1;
 572         sizes[0] = size;
 573
 574         return 0;
 575 }
 576
 577 static int isc_buffer_prepare(struct vb2_buffer *vb)
 578 {
 579         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 580         struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
 581         unsigned long size = isc->fmt.fmt.pix.sizeimage;
 582
 583         if (vb2_plane_size(vb, 0) < size) {
 584                 v4l2_err(&isc->v4l2_dev, "buffer too small (%lu < %lu)\n",
 585                          vb2_plane_size(vb, 0), size);
 586                 return -EINVAL;
 587         }
 588
 589         vb2_set_plane_payload(vb, 0, size);
 590
 591         vbuf->field = isc->fmt.fmt.pix.field;
 592
 593         return 0;
 594 }
 595
 596 static void isc_start_dma(struct isc_device *isc)
 597 {
 598         struct regmap *regmap = isc->regmap;
 599         u32 sizeimage = isc->fmt.fmt.pix.sizeimage;
 600         u32 dctrl_dview;
 601         dma_addr_t addr0;
 602         u32 h, w;
 603
 604         h = isc->fmt.fmt.pix.height;
 605         w = isc->fmt.fmt.pix.width;
 606
 607         /*
 608          * In case the sensor is not RAW, it will output a pixel (12-16 bits)
 609          * with two samples on the ISC Data bus (which is 8-12)
 610          * ISC will count each sample, so, we need to multiply these values
 611          * by two, to get the real number of samples for the required pixels.
 612          */
 613         if (!ISC_IS_FORMAT_RAW(isc->config.sd_format->mbus_code)) {
 614                 h <<= 1;
 615                 w <<= 1;
 616         }
 617
 618         /*
 619          * We limit the column/row count that the ISC will output according
 620          * to the configured resolution that we want.
 621          * This will avoid the situation where the sensor is misconfigured,
 622          * sending more data, and the ISC will just take it and DMA to memory,
 623          * causing corruption.
 624          */
 625         regmap_write(regmap, ISC_PFE_CFG1,
 626                      (ISC_PFE_CFG1_COLMIN(0) & ISC_PFE_CFG1_COLMIN_MASK) |
 627                      (ISC_PFE_CFG1_COLMAX(w - 1) & ISC_PFE_CFG1_COLMAX_MASK));
 628
 629         regmap_write(regmap, ISC_PFE_CFG2,
 630                      (ISC_PFE_CFG2_ROWMIN(0) & ISC_PFE_CFG2_ROWMIN_MASK) |
 631                      (ISC_PFE_CFG2_ROWMAX(h - 1) & ISC_PFE_CFG2_ROWMAX_MASK));
 632
 633         regmap_update_bits(regmap, ISC_PFE_CFG0,
 634                            ISC_PFE_CFG0_COLEN | ISC_PFE_CFG0_ROWEN,
 635                            ISC_PFE_CFG0_COLEN | ISC_PFE_CFG0_ROWEN);
 636
 637         addr0 = vb2_dma_contig_plane_dma_addr(&isc->cur_frm->vb.vb2_buf, 0);
 638         regmap_write(regmap, ISC_DAD0, addr0);
 639
 640         switch (isc->config.fourcc) {
 641         case V4L2_PIX_FMT_YUV420:
 642                 regmap_write(regmap, ISC_DAD1, addr0 + (sizeimage * 2) / 3);
 643                 regmap_write(regmap, ISC_DAD2, addr0 + (sizeimage * 5) / 6);
 644                 break;
 645         case V4L2_PIX_FMT_YUV422P:
 646                 regmap_write(regmap, ISC_DAD1, addr0 + sizeimage / 2);
 647                 regmap_write(regmap, ISC_DAD2, addr0 + (sizeimage * 3) / 4);
 648                 break;
 649         default:
 650                 break;
 651         }
 652
 653         dctrl_dview = isc->config.dctrl_dview;
 654
 655         regmap_write(regmap, ISC_DCTRL, dctrl_dview | ISC_DCTRL_IE_IS);
 656         spin_lock(&isc->awb_lock);
 657         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_CAPTURE);
 658         spin_unlock(&isc->awb_lock);
 659 }
 660
 661 static void isc_set_pipeline(struct isc_device *isc, u32 pipeline)
 662 {
 663         struct regmap *regmap = isc->regmap;
 664         struct isc_ctrls *ctrls = &isc->ctrls;
 665         u32 val, bay_cfg;
 666         const u32 *gamma;
 667         unsigned int i;
 668
 669         /* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
 670         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
 671                 val = pipeline & BIT(i) ? 1 : 0;
 672                 regmap_field_write(isc->pipeline[i], val);
 673         }
 674
 675         if (!pipeline)
 676                 return;
 677
 678         bay_cfg = isc->config.sd_format->cfa_baycfg;
 679
 680         regmap_write(regmap, ISC_WB_CFG, bay_cfg);
 681         isc_update_awb_ctrls(isc);
 682         isc_update_v4l2_ctrls(isc);
 683
 684         regmap_write(regmap, ISC_CFA_CFG, bay_cfg | ISC_CFA_CFG_EITPOL);
 685
 686         gamma = &isc_gamma_table[ctrls->gamma_index][0];
 687         regmap_bulk_write(regmap, ISC_GAM_BENTRY, gamma, GAMMA_ENTRIES);
 688         regmap_bulk_write(regmap, ISC_GAM_GENTRY, gamma, GAMMA_ENTRIES);
 689         regmap_bulk_write(regmap, ISC_GAM_RENTRY, gamma, GAMMA_ENTRIES);
 690
 691         /* Convert RGB to YUV */
 692         regmap_write(regmap, ISC_CSC_YR_YG, 0x42 | (0x81 << 16));
 693         regmap_write(regmap, ISC_CSC_YB_OY, 0x19 | (0x10 << 16));
 694         regmap_write(regmap, ISC_CSC_CBR_CBG, 0xFDA | (0xFB6 << 16));
 695         regmap_write(regmap, ISC_CSC_CBB_OCB, 0x70 | (0x80 << 16));
 696         regmap_write(regmap, ISC_CSC_CRR_CRG, 0x70 | (0xFA2 << 16));
 697         regmap_write(regmap, ISC_CSC_CRB_OCR, 0xFEE | (0x80 << 16));
 698
 699         regmap_write(regmap, ISC_CBC_BRIGHT, ctrls->brightness);
 700         regmap_write(regmap, ISC_CBC_CONTRAST, ctrls->contrast);
 701 }
 702
 703 static int isc_update_profile(struct isc_device *isc)
 704 {
 705         struct regmap *regmap = isc->regmap;
 706         u32 sr;
 707         int counter = 100;
 708
 709         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_UPPRO);
 710
 711         regmap_read(regmap, ISC_CTRLSR, &sr);
 712         while ((sr & ISC_CTRL_UPPRO) && counter--) {
 713                 usleep_range(1000, 2000);
 714                 regmap_read(regmap, ISC_CTRLSR, &sr);
 715         }
 716
 717         if (counter < 0) {
 718                 v4l2_warn(&isc->v4l2_dev, "Time out to update profile\n");
 719                 return -ETIMEDOUT;
 720         }
 721
 722         return 0;
 723 }
 724
 725 static void isc_set_histogram(struct isc_device *isc, bool enable)
 726 {
 727         struct regmap *regmap = isc->regmap;
 728         struct isc_ctrls *ctrls = &isc->ctrls;
 729
 730         if (enable) {
 731                 regmap_write(regmap, ISC_HIS_CFG,
 732                              ISC_HIS_CFG_MODE_GR |
 733                              (isc->config.sd_format->cfa_baycfg
 734                                         << ISC_HIS_CFG_BAYSEL_SHIFT) |
 735                                         ISC_HIS_CFG_RAR);
 736                 regmap_write(regmap, ISC_HIS_CTRL, ISC_HIS_CTRL_EN);
 737                 regmap_write(regmap, ISC_INTEN, ISC_INT_HISDONE);
 738                 ctrls->hist_id = ISC_HIS_CFG_MODE_GR;
 739                 isc_update_profile(isc);
 740                 regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_HISREQ);
 741
 742                 ctrls->hist_stat = HIST_ENABLED;
 743         } else {
 744                 regmap_write(regmap, ISC_INTDIS, ISC_INT_HISDONE);
 745                 regmap_write(regmap, ISC_HIS_CTRL, ISC_HIS_CTRL_DIS);
 746
 747                 ctrls->hist_stat = HIST_DISABLED;
 748         }
 749 }
 750
 751 static int isc_configure(struct isc_device *isc)
 752 {
 753         struct regmap *regmap = isc->regmap;
 754         u32 pfe_cfg0, rlp_mode, dcfg, mask, pipeline;
 755         struct isc_subdev_entity *subdev = isc->current_subdev;
 756
 757         pfe_cfg0 = isc->config.sd_format->pfe_cfg0_bps;
 758         rlp_mode = isc->config.rlp_cfg_mode;
 759         pipeline = isc->config.bits_pipeline;
 760
 761         dcfg = isc->config.dcfg_imode |
 762                        ISC_DCFG_YMBSIZE_BEATS8 | ISC_DCFG_CMBSIZE_BEATS8;
 763
 764         pfe_cfg0  |= subdev->pfe_cfg0 | ISC_PFE_CFG0_MODE_PROGRESSIVE;
 765         mask = ISC_PFE_CFG0_BPS_MASK | ISC_PFE_CFG0_HPOL_LOW |
 766                ISC_PFE_CFG0_VPOL_LOW | ISC_PFE_CFG0_PPOL_LOW |
 767                ISC_PFE_CFG0_MODE_MASK | ISC_PFE_CFG0_CCIR_CRC |
 768                    ISC_PFE_CFG0_CCIR656;
 769
 770         regmap_update_bits(regmap, ISC_PFE_CFG0, mask, pfe_cfg0);
 771
 772         regmap_update_bits(regmap, ISC_RLP_CFG, ISC_RLP_CFG_MODE_MASK,
 773                            rlp_mode);
 774
 775         regmap_write(regmap, ISC_DCFG, dcfg);
 776
 777         /* Set the pipeline */
 778         isc_set_pipeline(isc, pipeline);
 779
 780         /*
 781          * The current implemented histogram is available for RAW R, B, GB, GR
 782          * channels. We need to check if sensor is outputting RAW BAYER
 783          */
 784         if (isc->ctrls.awb &&
 785             ISC_IS_FORMAT_RAW(isc->config.sd_format->mbus_code))
 786                 isc_set_histogram(isc, true);
 787         else
 788                 isc_set_histogram(isc, false);
 789
 790         /* Update profile */
 791         return isc_update_profile(isc);
 792 }
 793
 794 static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
 795 {
 796         struct isc_device *isc = vb2_get_drv_priv(vq);
 797         struct regmap *regmap = isc->regmap;
 798         struct isc_buffer *buf;
 799         unsigned long flags;
 800         int ret;
 801
 802         /* Enable stream on the sub device */
 803         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 1);
 804         if (ret && ret != -ENOIOCTLCMD) {
 805                 v4l2_err(&isc->v4l2_dev, "stream on failed in subdev %d\n",
 806                          ret);
 807                 goto err_start_stream;
 808         }
 809
 810         pm_runtime_get_sync(isc->dev);
 811
 812         ret = isc_configure(isc);
 813         if (unlikely(ret))
 814                 goto err_configure;
 815
 816         /* Enable DMA interrupt */
 817         regmap_write(regmap, ISC_INTEN, ISC_INT_DDONE);
 818
 819         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 820
 821         isc->sequence = 0;
 822         isc->stop = false;
 823         reinit_completion(&isc->comp);
 824
 825         isc->cur_frm = list_first_entry(&isc->dma_queue,
 826                                         struct isc_buffer, list);
 827         list_del(&isc->cur_frm->list);
 828
 829         isc_start_dma(isc);
 830
 831         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 832
 833         /* if we streaming from RAW, we can do one-shot white balance adj */
 834         if (ISC_IS_FORMAT_RAW(isc->config.sd_format->mbus_code))
 835                 v4l2_ctrl_activate(isc->do_wb_ctrl, true);
 836
 837         return 0;
 838
 839 err_configure:
 840         pm_runtime_put_sync(isc->dev);
 841
 842         v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
 843
 844 err_start_stream:
 845         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 846         list_for_each_entry(buf, &isc->dma_queue, list)
 847                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 848         INIT_LIST_HEAD(&isc->dma_queue);
 849         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 850
 851         return ret;
 852 }
 853
 854 static void isc_stop_streaming(struct vb2_queue *vq)
 855 {
 856         struct isc_device *isc = vb2_get_drv_priv(vq);
 857         unsigned long flags;
 858         struct isc_buffer *buf;
 859         int ret;
 860
 861         v4l2_ctrl_activate(isc->do_wb_ctrl, false);
 862
 863         isc->stop = true;
 864
 865         /* Wait until the end of the current frame */
 866         if (isc->cur_frm && !wait_for_completion_timeout(&isc->comp, 5 * HZ))
 867                 v4l2_err(&isc->v4l2_dev,
 868                          "Timeout waiting for end of the capture\n");
 869
 870         /* Disable DMA interrupt */
 871         regmap_write(isc->regmap, ISC_INTDIS, ISC_INT_DDONE);
 872
 873         pm_runtime_put_sync(isc->dev);
 874
 875         /* Disable stream on the sub device */
 876         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
 877         if (ret && ret != -ENOIOCTLCMD)
 878                 v4l2_err(&isc->v4l2_dev, "stream off failed in subdev\n");
 879
 880         /* Release all active buffers */
 881         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 882         if (unlikely(isc->cur_frm)) {
 883                 vb2_buffer_done(&isc->cur_frm->vb.vb2_buf,
 884                                 VB2_BUF_STATE_ERROR);
 885                 isc->cur_frm = NULL;
 886         }
 887         list_for_each_entry(buf, &isc->dma_queue, list)
 888                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 889         INIT_LIST_HEAD(&isc->dma_queue);
 890         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 891 }
 892
 893 static void isc_buffer_queue(struct vb2_buffer *vb)
 894 {
 895         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 896         struct isc_buffer *buf = container_of(vbuf, struct isc_buffer, vb);
 897         struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
 898         unsigned long flags;
 899
 900         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 901         if (!isc->cur_frm && list_empty(&isc->dma_queue) &&
 902                 vb2_is_streaming(vb->vb2_queue)) {
 903                 isc->cur_frm = buf;
 904                 isc_start_dma(isc);
 905         } else
 906                 list_add_tail(&buf->list, &isc->dma_queue);
 907         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 908 }
 909
 910 static struct isc_format *find_format_by_fourcc(struct isc_device *isc,
 911                                                  unsigned int fourcc)
 912 {
 913         unsigned int num_formats = isc->num_user_formats;
 914         struct isc_format *fmt;
 915         unsigned int i;
 916
 917         for (i = 0; i < num_formats; i++) {
 918                 fmt = isc->user_formats[i];
 919                 if (fmt->fourcc == fourcc)
 920                         return fmt;
 921         }
 922
 923         return NULL;
 924 }
 925
 926 static const struct vb2_ops isc_vb2_ops = {
 927         .queue_setup            = isc_queue_setup,
 928         .wait_prepare           = vb2_ops_wait_prepare,
 929         .wait_finish            = vb2_ops_wait_finish,
 930         .buf_prepare            = isc_buffer_prepare,
 931         .start_streaming        = isc_start_streaming,
 932         .stop_streaming         = isc_stop_streaming,
 933         .buf_queue              = isc_buffer_queue,
 934 };
 935
 936 static int isc_querycap(struct file *file, void *priv,
 937                          struct v4l2_capability *cap)
 938 {
 939         struct isc_device *isc = video_drvdata(file);
 940
 941         strscpy(cap->driver, ATMEL_ISC_NAME, sizeof(cap->driver));
 942         strscpy(cap->card, "Atmel Image Sensor Controller", sizeof(cap->card));
 943         snprintf(cap->bus_info, sizeof(cap->bus_info),
 944                  "platform:%s", isc->v4l2_dev.name);
 945
 946         return 0;
 947 }
 948
 949 static int isc_enum_fmt_vid_cap(struct file *file, void *priv,
 950                                  struct v4l2_fmtdesc *f)
 951 {
 952         u32 index = f->index;
 953         u32 i, supported_index;
 954
 955         if (index < ARRAY_SIZE(controller_formats)) {
 956                 f->pixelformat = controller_formats[index].fourcc;
 957                 return 0;
 958         }
 959
 960         index -= ARRAY_SIZE(controller_formats);
 961
 962         i = 0;
 963         supported_index = 0;
 964
 965         for (i = 0; i < ARRAY_SIZE(formats_list); i++) {
 966                 if (!ISC_IS_FORMAT_RAW(formats_list[i].mbus_code) ||
 967                     !formats_list[i].sd_support)
 968                         continue;
 969                 if (supported_index == index) {
 970                         f->pixelformat = formats_list[i].fourcc;
 971                         return 0;
 972                 }
 973                 supported_index++;
 974         }
 975
 976         return -EINVAL;
 977 }
 978
 979 static int isc_g_fmt_vid_cap(struct file *file, void *priv,
 980                               struct v4l2_format *fmt)
 981 {
 982         struct isc_device *isc = video_drvdata(file);
 983
 984         *fmt = isc->fmt;
 985
 986         return 0;
 987 }
 988
 989 /*
 990  * Checks the current configured format, if ISC can output it,
 991  * considering which type of format the ISC receives from the sensor
 992  */
 993 static int isc_try_validate_formats(struct isc_device *isc)
 994 {
 995         int ret;
 996         bool bayer = false, yuv = false, rgb = false, grey = false;
 997
 998         /* all formats supported by the RLP module are OK */
 999         switch (isc->try_config.fourcc) {
1000         case V4L2_PIX_FMT_SBGGR8:
1001         case V4L2_PIX_FMT_SGBRG8:
1002         case V4L2_PIX_FMT_SGRBG8:
1003         case V4L2_PIX_FMT_SRGGB8:
1004         case V4L2_PIX_FMT_SBGGR10:
1005         case V4L2_PIX_FMT_SGBRG10:
1006         case V4L2_PIX_FMT_SGRBG10:
1007         case V4L2_PIX_FMT_SRGGB10:
1008         case V4L2_PIX_FMT_SBGGR12:
1009         case V4L2_PIX_FMT_SGBRG12:
1010         case V4L2_PIX_FMT_SGRBG12:
1011         case V4L2_PIX_FMT_SRGGB12:
1012                 ret = 0;
1013                 bayer = true;
1014                 break;
1015
1016         case V4L2_PIX_FMT_YUV420:
1017         case V4L2_PIX_FMT_YUV422P:
1018         case V4L2_PIX_FMT_YUYV:
1019                 ret = 0;
1020                 yuv = true;
1021                 break;
1022
1023         case V4L2_PIX_FMT_RGB565:
1024         case V4L2_PIX_FMT_ABGR32:
1025         case V4L2_PIX_FMT_XBGR32:
1026         case V4L2_PIX_FMT_ARGB444:
1027         case V4L2_PIX_FMT_ARGB555:
1028                 ret = 0;
1029                 rgb = true;
1030                 break;
1031         case V4L2_PIX_FMT_GREY:
1032         case V4L2_PIX_FMT_Y10:
1033                 ret = 0;
1034                 grey = true;
1035                 break;
1036         default:
1037         /* any other different formats are not supported */
1038                 ret = -EINVAL;
1039         }
1040         v4l2_dbg(1, debug, &isc->v4l2_dev,
1041                  "Format validation, requested rgb=%u, yuv=%u, grey=%u, bayer=%u\n",
1042                  rgb, yuv, grey, bayer);
1043
1044         /* we cannot output RAW if we do not receive RAW */
1045         if ((bayer) && !ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code))
1046                 return -EINVAL;
1047
1048         /* we cannot output GREY if we do not receive RAW/GREY */
1049         if (grey && !ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code) &&
1050             !ISC_IS_FORMAT_GREY(isc->try_config.sd_format->mbus_code))
1051                 return -EINVAL;
1052
1053         return ret;
1054 }
1055
1056 /*
1057  * Configures the RLP and DMA modules, depending on the output format
1058  * configured for the ISC.
1059  * If direct_dump == true, just dump raw data 8/16 bits depending on format.
1060  */
1061 static int isc_try_configure_rlp_dma(struct isc_device *isc, bool direct_dump)
1062 {
1063         switch (isc->try_config.fourcc) {
1064         case V4L2_PIX_FMT_SBGGR8:
1065         case V4L2_PIX_FMT_SGBRG8:
1066         case V4L2_PIX_FMT_SGRBG8:
1067         case V4L2_PIX_FMT_SRGGB8:
1068                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_DAT8;
1069                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED8;
1070                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1071                 isc->try_config.bpp = 8;
1072                 break;
1073         case V4L2_PIX_FMT_SBGGR10:
1074         case V4L2_PIX_FMT_SGBRG10:
1075         case V4L2_PIX_FMT_SGRBG10:
1076         case V4L2_PIX_FMT_SRGGB10:
1077                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_DAT10;
1078                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED16;
1079                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1080                 isc->try_config.bpp = 16;
1081                 break;
1082         case V4L2_PIX_FMT_SBGGR12:
1083         case V4L2_PIX_FMT_SGBRG12:
1084         case V4L2_PIX_FMT_SGRBG12:
1085         case V4L2_PIX_FMT_SRGGB12:
1086                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_DAT12;
1087                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED16;
1088                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1089                 isc->try_config.bpp = 16;
1090                 break;
1091         case V4L2_PIX_FMT_RGB565:
1092                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_RGB565;
1093                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED16;
1094                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1095                 isc->try_config.bpp = 16;
1096                 break;
1097         case V4L2_PIX_FMT_ARGB444:
1098                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_ARGB444;
1099                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED16;
1100                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1101                 isc->try_config.bpp = 16;
1102                 break;
1103         case V4L2_PIX_FMT_ARGB555:
1104                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_ARGB555;
1105                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED16;
1106                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1107                 isc->try_config.bpp = 16;
1108                 break;
1109         case V4L2_PIX_FMT_ABGR32:
1110         case V4L2_PIX_FMT_XBGR32:
1111                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_ARGB32;
1112                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED32;
1113                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1114                 isc->try_config.bpp = 32;
1115                 break;
1116         case V4L2_PIX_FMT_YUV420:
1117                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_YYCC;
1118                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_YC420P;
1119                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PLANAR;
1120                 isc->try_config.bpp = 12;
1121                 break;
1122         case V4L2_PIX_FMT_YUV422P:
1123                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_YYCC;
1124                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_YC422P;
1125                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PLANAR;
1126                 isc->try_config.bpp = 16;
1127                 break;
1128         case V4L2_PIX_FMT_YUYV:
1129                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_YYCC;
1130                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED32;
1131                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1132                 isc->try_config.bpp = 16;
1133                 break;
1134         case V4L2_PIX_FMT_GREY:
1135                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_DATY8;
1136                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED8;
1137                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1138                 isc->try_config.bpp = 8;
1139                 break;
1140         case V4L2_PIX_FMT_Y10:
1141                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_DATY10;
1142                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED16;
1143                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1144                 isc->try_config.bpp = 16;
1145                 break;
1146         default:
1147                 return -EINVAL;
1148         }
1149
1150         if (direct_dump) {
1151                 isc->try_config.rlp_cfg_mode = ISC_RLP_CFG_MODE_DAT8;
1152                 isc->try_config.dcfg_imode = ISC_DCFG_IMODE_PACKED8;
1153                 isc->try_config.dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
1154                 return 0;
1155         }
1156
1157         return 0;
1158 }
1159
1160 /*
1161  * Configuring pipeline modules, depending on which format the ISC outputs
1162  * and considering which format it has as input from the sensor.
1163  */
1164 static int isc_try_configure_pipeline(struct isc_device *isc)
1165 {
1166         switch (isc->try_config.fourcc) {
1167         case V4L2_PIX_FMT_RGB565:
1168         case V4L2_PIX_FMT_ARGB555:
1169         case V4L2_PIX_FMT_ARGB444:
1170         case V4L2_PIX_FMT_ABGR32:
1171         case V4L2_PIX_FMT_XBGR32:
1172                 /* if sensor format is RAW, we convert inside ISC */
1173                 if (ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code)) {
1174                         isc->try_config.bits_pipeline = CFA_ENABLE |
1175                                 WB_ENABLE | GAM_ENABLES;
1176                 } else {
1177                         isc->try_config.bits_pipeline = 0x0;
1178                 }
1179                 break;
1180         case V4L2_PIX_FMT_YUV420:
1181                 /* if sensor format is RAW, we convert inside ISC */
1182                 if (ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code)) {
1183                         isc->try_config.bits_pipeline = CFA_ENABLE |
1184                                 CSC_ENABLE | WB_ENABLE | GAM_ENABLES |
1185                                 SUB420_ENABLE | SUB422_ENABLE | CBC_ENABLE;
1186                 } else {
1187                         isc->try_config.bits_pipeline = 0x0;
1188                 }
1189                 break;
1190         case V4L2_PIX_FMT_YUV422P:
1191                 /* if sensor format is RAW, we convert inside ISC */
1192                 if (ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code)) {
1193                         isc->try_config.bits_pipeline = CFA_ENABLE |
1194                                 CSC_ENABLE | WB_ENABLE | GAM_ENABLES |
1195                                 SUB422_ENABLE | CBC_ENABLE;
1196                 } else {
1197                         isc->try_config.bits_pipeline = 0x0;
1198                 }
1199                 break;
1200         case V4L2_PIX_FMT_YUYV:
1201                 /* if sensor format is RAW, we convert inside ISC */
1202                 if (ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code)) {
1203                         isc->try_config.bits_pipeline = CFA_ENABLE |
1204                                 CSC_ENABLE | WB_ENABLE | GAM_ENABLES |
1205                                 SUB422_ENABLE | CBC_ENABLE;
1206                 } else {
1207                         isc->try_config.bits_pipeline = 0x0;
1208                 }
1209                 break;
1210         case V4L2_PIX_FMT_GREY:
1211                 if (ISC_IS_FORMAT_RAW(isc->try_config.sd_format->mbus_code)) {
1212                 /* if sensor format is RAW, we convert inside ISC */
1213                         isc->try_config.bits_pipeline = CFA_ENABLE |
1214                                 CSC_ENABLE | WB_ENABLE | GAM_ENABLES |
1215                                 CBC_ENABLE;
1216                 } else {
1217                         isc->try_config.bits_pipeline = 0x0;
1218                 }
1219                 break;
1220         default:
1221                 isc->try_config.bits_pipeline = 0x0;
1222         }
1223         return 0;
1224 }
1225
1226 static void isc_try_fse(struct isc_device *isc,
1227                         struct v4l2_subdev_pad_config *pad_cfg)
1228 {
1229         int ret;
1230         struct v4l2_subdev_frame_size_enum fse = {};
1231
1232         /*
1233          * If we do not know yet which format the subdev is using, we cannot
1234          * do anything.
1235          */
1236         if (!isc->try_config.sd_format)
1237                 return;
1238
1239         fse.code = isc->try_config.sd_format->mbus_code;
1240         fse.which = V4L2_SUBDEV_FORMAT_TRY;
1241
1242         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, enum_frame_size,
1243                                pad_cfg, &fse);
1244         /*
1245          * Attempt to obtain format size from subdev. If not available,
1246          * just use the maximum ISC can receive.
1247          */
1248         if (ret) {
1249                 pad_cfg->try_crop.width = ISC_MAX_SUPPORT_WIDTH;
1250                 pad_cfg->try_crop.height = ISC_MAX_SUPPORT_HEIGHT;
1251         } else {
1252                 pad_cfg->try_crop.width = fse.max_width;
1253                 pad_cfg->try_crop.height = fse.max_height;
1254         }
1255 }
1256
1257 static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
1258                         u32 *code)
1259 {
1260         int i;
1261         struct isc_format *sd_fmt = NULL, *direct_fmt = NULL;
1262         struct v4l2_pix_format *pixfmt = &f->fmt.pix;
1263         struct v4l2_subdev_pad_config pad_cfg = {};
1264         struct v4l2_subdev_format format = {
1265                 .which = V4L2_SUBDEV_FORMAT_TRY,
1266         };
1267         u32 mbus_code;
1268         int ret;
1269         bool rlp_dma_direct_dump = false;
1270
1271         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1272                 return -EINVAL;
1273
1274         /* Step 1: find a RAW format that is supported */
1275         for (i = 0; i < isc->num_user_formats; i++) {
1276                 if (ISC_IS_FORMAT_RAW(isc->user_formats[i]->mbus_code)) {
1277                         sd_fmt = isc->user_formats[i];
1278                         break;
1279                 }
1280         }
1281         /* Step 2: We can continue with this RAW format, or we can look
1282          * for better: maybe sensor supports directly what we need.
1283          */
1284         direct_fmt = find_format_by_fourcc(isc, pixfmt->pixelformat);
1285
1286         /* Step 3: We have both. We decide given the module parameter which
1287          * one to use.
1288          */
1289         if (direct_fmt && sd_fmt && sensor_preferred)
1290                 sd_fmt = direct_fmt;
1291
1292         /* Step 4: we do not have RAW but we have a direct format. Use it. */
1293         if (direct_fmt && !sd_fmt)
1294                 sd_fmt = direct_fmt;
1295
1296         /* Step 5: if we are using a direct format, we need to package
1297          * everything as 8 bit data and just dump it
1298          */
1299         if (sd_fmt == direct_fmt)
1300                 rlp_dma_direct_dump = true;
1301
1302         /* Step 6: We have no format. This can happen if the userspace
1303          * requests some weird/invalid format.
1304          * In this case, default to whatever we have
1305          */
1306         if (!sd_fmt && !direct_fmt) {
1307                 sd_fmt = isc->user_formats[isc->num_user_formats - 1];
1308                 v4l2_dbg(1, debug, &isc->v4l2_dev,
1309                          "Sensor not supporting %.4s, using %.4s\n",
1310                          (char *)&pixfmt->pixelformat, (char *)&sd_fmt->fourcc);
1311         }
1312
1313         if (!sd_fmt) {
1314                 ret = -EINVAL;
1315                 goto isc_try_fmt_err;
1316         }
1317
1318         /* Step 7: Print out what we decided for debugging */
1319         v4l2_dbg(1, debug, &isc->v4l2_dev,
1320                  "Preferring to have sensor using format %.4s\n",
1321                  (char *)&sd_fmt->fourcc);
1322
1323         /* Step 8: at this moment we decided which format the subdev will use */
1324         isc->try_config.sd_format = sd_fmt;
1325
1326         /* Limit to Atmel ISC hardware capabilities */
1327         if (pixfmt->width > ISC_MAX_SUPPORT_WIDTH)
1328                 pixfmt->width = ISC_MAX_SUPPORT_WIDTH;
1329         if (pixfmt->height > ISC_MAX_SUPPORT_HEIGHT)
1330                 pixfmt->height = ISC_MAX_SUPPORT_HEIGHT;
1331
1332         /*
1333          * The mbus format is the one the subdev outputs.
1334          * The pixels will be transferred in this format Sensor -> ISC
1335          */
1336         mbus_code = sd_fmt->mbus_code;
1337
1338         /*
1339          * Validate formats. If the required format is not OK, default to raw.
1340          */
1341
1342         isc->try_config.fourcc = pixfmt->pixelformat;
1343
1344         if (isc_try_validate_formats(isc)) {
1345                 pixfmt->pixelformat = isc->try_config.fourcc = sd_fmt->fourcc;
1346                 /* Re-try to validate the new format */
1347                 ret = isc_try_validate_formats(isc);
1348                 if (ret)
1349                         goto isc_try_fmt_err;
1350         }
1351
1352         ret = isc_try_configure_rlp_dma(isc, rlp_dma_direct_dump);
1353         if (ret)
1354                 goto isc_try_fmt_err;
1355
1356         ret = isc_try_configure_pipeline(isc);
1357         if (ret)
1358                 goto isc_try_fmt_err;
1359
1360         /* Obtain frame sizes if possible to have crop requirements ready */
1361         isc_try_fse(isc, &pad_cfg);
1362
1363         v4l2_fill_mbus_format(&format.format, pixfmt, mbus_code);
1364         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, set_fmt,
1365                                &pad_cfg, &format);
1366         if (ret < 0)
1367                 goto isc_try_fmt_subdev_err;
1368
1369         v4l2_fill_pix_format(pixfmt, &format.format);
1370
1371         pixfmt->field = V4L2_FIELD_NONE;
1372         pixfmt->bytesperline = (pixfmt->width * isc->try_config.bpp) >> 3;
1373         pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
1374
1375         if (code)
1376                 *code = mbus_code;
1377
1378         return 0;
1379
1380 isc_try_fmt_err:
1381         v4l2_err(&isc->v4l2_dev, "Could not find any possible format for a working pipeline\n");
1382 isc_try_fmt_subdev_err:
1383         memset(&isc->try_config, 0, sizeof(isc->try_config));
1384
1385         return ret;
1386 }
1387
1388 static int isc_set_fmt(struct isc_device *isc, struct v4l2_format *f)
1389 {
1390         struct v4l2_subdev_format format = {
1391                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1392         };
1393         u32 mbus_code = 0;
1394         int ret;
1395
1396         ret = isc_try_fmt(isc, f, &mbus_code);
1397         if (ret)
1398                 return ret;
1399
1400         v4l2_fill_mbus_format(&format.format, &f->fmt.pix, mbus_code);
1401         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
1402                                set_fmt, NULL, &format);
1403         if (ret < 0)
1404                 return ret;
1405
1406         isc->fmt = *f;
1407
1408         if (isc->try_config.sd_format && isc->config.sd_format &&
1409             isc->try_config.sd_format != isc->config.sd_format) {
1410                 isc->ctrls.hist_stat = HIST_INIT;
1411                 isc_reset_awb_ctrls(isc);
1412                 isc_update_v4l2_ctrls(isc);
1413         }
1414         /* make the try configuration active */
1415         isc->config = isc->try_config;
1416
1417         v4l2_dbg(1, debug, &isc->v4l2_dev, "New ISC configuration in place\n");
1418
1419         return 0;
1420 }
1421
1422 static int isc_s_fmt_vid_cap(struct file *file, void *priv,
1423                               struct v4l2_format *f)
1424 {
1425         struct isc_device *isc = video_drvdata(file);
1426
1427         if (vb2_is_streaming(&isc->vb2_vidq))
1428                 return -EBUSY;
1429
1430         return isc_set_fmt(isc, f);
1431 }
1432
1433 static int isc_try_fmt_vid_cap(struct file *file, void *priv,
1434                                 struct v4l2_format *f)
1435 {
1436         struct isc_device *isc = video_drvdata(file);
1437
1438         return isc_try_fmt(isc, f, NULL);
1439 }
1440
1441 static int isc_enum_input(struct file *file, void *priv,
1442                            struct v4l2_input *inp)
1443 {
1444         if (inp->index != 0)
1445                 return -EINVAL;
1446
1447         inp->type = V4L2_INPUT_TYPE_CAMERA;
1448         inp->std = 0;
1449         strscpy(inp->name, "Camera", sizeof(inp->name));
1450
1451         return 0;
1452 }
1453
1454 static int isc_g_input(struct file *file, void *priv, unsigned int *i)
1455 {
1456         *i = 0;
1457
1458         return 0;
1459 }
1460
1461 static int isc_s_input(struct file *file, void *priv, unsigned int i)
1462 {
1463         if (i > 0)
1464                 return -EINVAL;
1465
1466         return 0;
1467 }
1468
1469 static int isc_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
1470 {
1471         struct isc_device *isc = video_drvdata(file);
1472
1473         return v4l2_g_parm_cap(video_devdata(file), isc->current_subdev->sd, a);
1474 }
1475
1476 static int isc_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
1477 {
1478         struct isc_device *isc = video_drvdata(file);
1479
1480         return v4l2_s_parm_cap(video_devdata(file), isc->current_subdev->sd, a);
1481 }
1482
1483 static int isc_enum_framesizes(struct file *file, void *fh,
1484                                struct v4l2_frmsizeenum *fsize)
1485 {
1486         struct isc_device *isc = video_drvdata(file);
1487         struct v4l2_subdev_frame_size_enum fse = {
1488                 .code = isc->config.sd_format->mbus_code,
1489                 .index = fsize->index,
1490                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1491         };
1492         int ret = -EINVAL;
1493         int i;
1494
1495         for (i = 0; i < isc->num_user_formats; i++)
1496                 if (isc->user_formats[i]->fourcc == fsize->pixel_format)
1497                         ret = 0;
1498
1499         for (i = 0; i < ARRAY_SIZE(controller_formats); i++)
1500                 if (controller_formats[i].fourcc == fsize->pixel_format)
1501                         ret = 0;
1502
1503         if (ret)
1504                 return ret;
1505
1506         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, enum_frame_size,
1507                                NULL, &fse);
1508         if (ret)
1509                 return ret;
1510
1511         fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1512         fsize->discrete.width = fse.max_width;
1513         fsize->discrete.height = fse.max_height;
1514
1515         return 0;
1516 }
1517
1518 static int isc_enum_frameintervals(struct file *file, void *fh,
1519                                     struct v4l2_frmivalenum *fival)
1520 {
1521         struct isc_device *isc = video_drvdata(file);
1522         struct v4l2_subdev_frame_interval_enum fie = {
1523                 .code = isc->config.sd_format->mbus_code,
1524                 .index = fival->index,
1525                 .width = fival->width,
1526                 .height = fival->height,
1527                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1528         };
1529         int ret = -EINVAL;
1530         unsigned int i;
1531
1532         for (i = 0; i < isc->num_user_formats; i++)
1533                 if (isc->user_formats[i]->fourcc == fival->pixel_format)
1534                         ret = 0;
1535
1536         for (i = 0; i < ARRAY_SIZE(controller_formats); i++)
1537                 if (controller_formats[i].fourcc == fival->pixel_format)
1538                         ret = 0;
1539
1540         if (ret)
1541                 return ret;
1542
1543         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
1544                                enum_frame_interval, NULL, &fie);
1545         if (ret)
1546                 return ret;
1547
1548         fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1549         fival->discrete = fie.interval;
1550
1551         return 0;
1552 }
1553
1554 static const struct v4l2_ioctl_ops isc_ioctl_ops = {
1555         .vidioc_querycap                = isc_querycap,
1556         .vidioc_enum_fmt_vid_cap        = isc_enum_fmt_vid_cap,
1557         .vidioc_g_fmt_vid_cap           = isc_g_fmt_vid_cap,
1558         .vidioc_s_fmt_vid_cap           = isc_s_fmt_vid_cap,
1559         .vidioc_try_fmt_vid_cap         = isc_try_fmt_vid_cap,
1560
1561         .vidioc_enum_input              = isc_enum_input,
1562         .vidioc_g_input                 = isc_g_input,
1563         .vidioc_s_input                 = isc_s_input,
1564
1565         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1566         .vidioc_querybuf                = vb2_ioctl_querybuf,
1567         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1568         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1569         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1570         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1571         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1572         .vidioc_streamon                = vb2_ioctl_streamon,
1573         .vidioc_streamoff               = vb2_ioctl_streamoff,
1574
1575         .vidioc_g_parm                  = isc_g_parm,
1576         .vidioc_s_parm                  = isc_s_parm,
1577         .vidioc_enum_framesizes         = isc_enum_framesizes,
1578         .vidioc_enum_frameintervals     = isc_enum_frameintervals,
1579
1580         .vidioc_log_status              = v4l2_ctrl_log_status,
1581         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1582         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1583 };
1584
1585 static int isc_open(struct file *file)
1586 {
1587         struct isc_device *isc = video_drvdata(file);
1588         struct v4l2_subdev *sd = isc->current_subdev->sd;
1589         int ret;
1590
1591         if (mutex_lock_interruptible(&isc->lock))
1592                 return -ERESTARTSYS;
1593
1594         ret = v4l2_fh_open(file);
1595         if (ret < 0)
1596                 goto unlock;
1597
1598         if (!v4l2_fh_is_singular_file(file))
1599                 goto unlock;
1600
1601         ret = v4l2_subdev_call(sd, core, s_power, 1);
1602         if (ret < 0 && ret != -ENOIOCTLCMD) {
1603                 v4l2_fh_release(file);
1604                 goto unlock;
1605         }
1606
1607         ret = isc_set_fmt(isc, &isc->fmt);
1608         if (ret) {
1609                 v4l2_subdev_call(sd, core, s_power, 0);
1610                 v4l2_fh_release(file);
1611         }
1612
1613 unlock:
1614         mutex_unlock(&isc->lock);
1615         return ret;
1616 }
1617
1618 static int isc_release(struct file *file)
1619 {
1620         struct isc_device *isc = video_drvdata(file);
1621         struct v4l2_subdev *sd = isc->current_subdev->sd;
1622         bool fh_singular;
1623         int ret;
1624
1625         mutex_lock(&isc->lock);
1626
1627         fh_singular = v4l2_fh_is_singular_file(file);
1628
1629         ret = _vb2_fop_release(file, NULL);
1630
1631         if (fh_singular)
1632                 v4l2_subdev_call(sd, core, s_power, 0);
1633
1634         mutex_unlock(&isc->lock);
1635
1636         return ret;
1637 }
1638
1639 static const struct v4l2_file_operations isc_fops = {
1640         .owner          = THIS_MODULE,
1641         .open           = isc_open,
1642         .release        = isc_release,
1643         .unlocked_ioctl = video_ioctl2,
1644         .read           = vb2_fop_read,
1645         .mmap           = vb2_fop_mmap,
1646         .poll           = vb2_fop_poll,
1647 };
1648
1649 irqreturn_t isc_interrupt(int irq, void *dev_id)
1650 {
1651         struct isc_device *isc = (struct isc_device *)dev_id;
1652         struct regmap *regmap = isc->regmap;
1653         u32 isc_intsr, isc_intmask, pending;
1654         irqreturn_t ret = IRQ_NONE;
1655
1656         regmap_read(regmap, ISC_INTSR, &isc_intsr);
1657         regmap_read(regmap, ISC_INTMASK, &isc_intmask);
1658
1659         pending = isc_intsr & isc_intmask;
1660
1661         if (likely(pending & ISC_INT_DDONE)) {
1662                 spin_lock(&isc->dma_queue_lock);
1663                 if (isc->cur_frm) {
1664                         struct vb2_v4l2_buffer *vbuf = &isc->cur_frm->vb;
1665                         struct vb2_buffer *vb = &vbuf->vb2_buf;
1666
1667                         vb->timestamp = ktime_get_ns();
1668                         vbuf->sequence = isc->sequence++;
1669                         vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
1670                         isc->cur_frm = NULL;
1671                 }
1672
1673                 if (!list_empty(&isc->dma_queue) && !isc->stop) {
1674                         isc->cur_frm = list_first_entry(&isc->dma_queue,
1675                                                      struct isc_buffer, list);
1676                         list_del(&isc->cur_frm->list);
1677
1678                         isc_start_dma(isc);
1679                 }
1680
1681                 if (isc->stop)
1682                         complete(&isc->comp);
1683
1684                 ret = IRQ_HANDLED;
1685                 spin_unlock(&isc->dma_queue_lock);
1686         }
1687
1688         if (pending & ISC_INT_HISDONE) {
1689                 schedule_work(&isc->awb_work);
1690                 ret = IRQ_HANDLED;
1691         }
1692
1693         return ret;
1694 }
1695
1696 static void isc_hist_count(struct isc_device *isc, u32 *min, u32 *max)
1697 {
1698         struct regmap *regmap = isc->regmap;
1699         struct isc_ctrls *ctrls = &isc->ctrls;
1700         u32 *hist_count = &ctrls->hist_count[ctrls->hist_id];
1701         u32 *hist_entry = &ctrls->hist_entry[0];
1702         u32 i;
1703
1704         *min = 0;
1705         *max = HIST_ENTRIES;
1706
1707         regmap_bulk_read(regmap, ISC_HIS_ENTRY, hist_entry, HIST_ENTRIES);
1708
1709         *hist_count = 0;
1710         /*
1711          * we deliberately ignore the end of the histogram,
1712          * the most white pixels
1713          */
1714         for (i = 1; i < HIST_ENTRIES; i++) {
1715                 if (*hist_entry && !*min)
1716                         *min = i;
1717                 if (*hist_entry)
1718                         *max = i;
1719                 *hist_count += i * (*hist_entry++);
1720         }
1721
1722         if (!*min)
1723                 *min = 1;
1724 }
1725
1726 static void isc_wb_update(struct isc_ctrls *ctrls)
1727 {
1728         u32 *hist_count = &ctrls->hist_count[0];
1729         u32 c, offset[4];
1730         u64 avg = 0;
1731         /* We compute two gains, stretch gain and grey world gain */
1732         u32 s_gain[4], gw_gain[4];
1733
1734         /*
1735          * According to Grey World, we need to set gains for R/B to normalize
1736          * them towards the green channel.
1737          * Thus we want to keep Green as fixed and adjust only Red/Blue
1738          * Compute the average of the both green channels first
1739          */
1740         avg = (u64)hist_count[ISC_HIS_CFG_MODE_GR] +
1741                 (u64)hist_count[ISC_HIS_CFG_MODE_GB];
1742         avg >>= 1;
1743
1744         /* Green histogram is null, nothing to do */
1745         if (!avg)
1746                 return;
1747
1748         for (c = ISC_HIS_CFG_MODE_GR; c <= ISC_HIS_CFG_MODE_B; c++) {
1749                 /*
1750                  * the color offset is the minimum value of the histogram.
1751                  * we stretch this color to the full range by substracting
1752                  * this value from the color component.
1753                  */
1754                 offset[c] = ctrls->hist_minmax[c][HIST_MIN_INDEX];
1755                 /*
1756                  * The offset is always at least 1. If the offset is 1, we do
1757                  * not need to adjust it, so our result must be zero.
1758                  * the offset is computed in a histogram on 9 bits (0..512)
1759                  * but the offset in register is based on
1760                  * 12 bits pipeline (0..4096).
1761                  * we need to shift with the 3 bits that the histogram is
1762                  * ignoring
1763                  */
1764                 ctrls->offset[c] = (offset[c] - 1) << 3;
1765
1766                 /*
1767                  * the offset is then taken and converted to 2's complements,
1768                  * and must be negative, as we subtract this value from the
1769                  * color components
1770                  */
1771                 ctrls->offset[c] = -ctrls->offset[c];
1772
1773                 /*
1774                  * the stretch gain is the total number of histogram bins
1775                  * divided by the actual range of color component (Max - Min)
1776                  * If we compute gain like this, the actual color component
1777                  * will be stretched to the full histogram.
1778                  * We need to shift 9 bits for precision, we have 9 bits for
1779                  * decimals
1780                  */
1781                 s_gain[c] = (HIST_ENTRIES << 9) /
1782                         (ctrls->hist_minmax[c][HIST_MAX_INDEX] -
1783                         ctrls->hist_minmax[c][HIST_MIN_INDEX] + 1);
1784
1785                 /*
1786                  * Now we have to compute the gain w.r.t. the average.
1787                  * Add/lose gain to the component towards the average.
1788                  * If it happens that the component is zero, use the
1789                  * fixed point value : 1.0 gain.
1790                  */
1791                 if (hist_count[c])
1792                         gw_gain[c] = div_u64(avg << 9, hist_count[c]);
1793                 else
1794                         gw_gain[c] = 1 << 9;
1795
1796                 /* multiply both gains and adjust for decimals */
1797                 ctrls->gain[c] = s_gain[c] * gw_gain[c];
1798                 ctrls->gain[c] >>= 9;
1799         }
1800 }
1801
1802 static void isc_awb_work(struct work_struct *w)
1803 {
1804         struct isc_device *isc =
1805                 container_of(w, struct isc_device, awb_work);
1806         struct regmap *regmap = isc->regmap;
1807         struct isc_ctrls *ctrls = &isc->ctrls;
1808         u32 hist_id = ctrls->hist_id;
1809         u32 baysel;
1810         unsigned long flags;
1811         u32 min, max;
1812
1813         /* streaming is not active anymore */
1814         if (isc->stop)
1815                 return;
1816
1817         if (ctrls->hist_stat != HIST_ENABLED)
1818                 return;
1819
1820         isc_hist_count(isc, &min, &max);
1821         ctrls->hist_minmax[hist_id][HIST_MIN_INDEX] = min;
1822         ctrls->hist_minmax[hist_id][HIST_MAX_INDEX] = max;
1823
1824         if (hist_id != ISC_HIS_CFG_MODE_B) {
1825                 hist_id++;
1826         } else {
1827                 isc_wb_update(ctrls);
1828                 hist_id = ISC_HIS_CFG_MODE_GR;
1829         }
1830
1831         ctrls->hist_id = hist_id;
1832         baysel = isc->config.sd_format->cfa_baycfg << ISC_HIS_CFG_BAYSEL_SHIFT;
1833
1834         pm_runtime_get_sync(isc->dev);
1835
1836         /*
1837          * only update if we have all the required histograms and controls
1838          * if awb has been disabled, we need to reset registers as well.
1839          */
1840         if (hist_id == ISC_HIS_CFG_MODE_GR || ctrls->awb == ISC_WB_NONE) {
1841                 /*
1842                  * It may happen that DMA Done IRQ will trigger while we are
1843                  * updating white balance registers here.
1844                  * In that case, only parts of the controls have been updated.
1845                  * We can avoid that by locking the section.
1846                  */
1847                 spin_lock_irqsave(&isc->awb_lock, flags);
1848                 isc_update_awb_ctrls(isc);
1849                 spin_unlock_irqrestore(&isc->awb_lock, flags);
1850
1851                 /*
1852                  * if we are doing just the one time white balance adjustment,
1853                  * we are basically done.
1854                  */
1855                 if (ctrls->awb == ISC_WB_ONETIME) {
1856                         v4l2_info(&isc->v4l2_dev,
1857                                   "Completed one time white-balance adjustment.\n");
1858                         /* update the v4l2 controls values */
1859                         isc_update_v4l2_ctrls(isc);
1860                         ctrls->awb = ISC_WB_NONE;
1861                 }
1862         }
1863         regmap_write(regmap, ISC_HIS_CFG, hist_id | baysel | ISC_HIS_CFG_RAR);
1864         isc_update_profile(isc);
1865         /* if awb has been disabled, we don't need to start another histogram */
1866         if (ctrls->awb)
1867                 regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_HISREQ);
1868
1869         pm_runtime_put_sync(isc->dev);
1870 }
1871
1872 static int isc_s_ctrl(struct v4l2_ctrl *ctrl)
1873 {
1874         struct isc_device *isc = container_of(ctrl->handler,
1875                                              struct isc_device, ctrls.handler);
1876         struct isc_ctrls *ctrls = &isc->ctrls;
1877
1878         if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
1879                 return 0;
1880
1881         switch (ctrl->id) {
1882         case V4L2_CID_BRIGHTNESS:
1883                 ctrls->brightness = ctrl->val & ISC_CBC_BRIGHT_MASK;
1884                 break;
1885         case V4L2_CID_CONTRAST:
1886                 ctrls->contrast = ctrl->val & ISC_CBC_CONTRAST_MASK;
1887                 break;
1888         case V4L2_CID_GAMMA:
1889                 ctrls->gamma_index = ctrl->val;
1890                 break;
1891         default:
1892                 return -EINVAL;
1893         }
1894
1895         return 0;
1896 }
1897
1898 static const struct v4l2_ctrl_ops isc_ctrl_ops = {
1899         .s_ctrl = isc_s_ctrl,
1900 };
1901
1902 static int isc_s_awb_ctrl(struct v4l2_ctrl *ctrl)
1903 {
1904         struct isc_device *isc = container_of(ctrl->handler,
1905                                              struct isc_device, ctrls.handler);
1906         struct isc_ctrls *ctrls = &isc->ctrls;
1907
1908         if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
1909                 return 0;
1910
1911         switch (ctrl->id) {
1912         case V4L2_CID_AUTO_WHITE_BALANCE:
1913                 if (ctrl->val == 1)
1914                         ctrls->awb = ISC_WB_AUTO;
1915                 else
1916                         ctrls->awb = ISC_WB_NONE;
1917
1918                 /* we did not configure ISC yet */
1919                 if (!isc->config.sd_format)
1920                         break;
1921
1922                 /* configure the controls with new values from v4l2 */
1923                 if (ctrl->cluster[ISC_CTRL_R_GAIN]->is_new)
1924                         ctrls->gain[ISC_HIS_CFG_MODE_R] = isc->r_gain_ctrl->val;
1925                 if (ctrl->cluster[ISC_CTRL_B_GAIN]->is_new)
1926                         ctrls->gain[ISC_HIS_CFG_MODE_B] = isc->b_gain_ctrl->val;
1927                 if (ctrl->cluster[ISC_CTRL_GR_GAIN]->is_new)
1928                         ctrls->gain[ISC_HIS_CFG_MODE_GR] = isc->gr_gain_ctrl->val;
1929                 if (ctrl->cluster[ISC_CTRL_GB_GAIN]->is_new)
1930                         ctrls->gain[ISC_HIS_CFG_MODE_GB] = isc->gb_gain_ctrl->val;
1931
1932                 if (ctrl->cluster[ISC_CTRL_R_OFF]->is_new)
1933                         ctrls->offset[ISC_HIS_CFG_MODE_R] = isc->r_off_ctrl->val;
1934                 if (ctrl->cluster[ISC_CTRL_B_OFF]->is_new)
1935                         ctrls->offset[ISC_HIS_CFG_MODE_B] = isc->b_off_ctrl->val;
1936                 if (ctrl->cluster[ISC_CTRL_GR_OFF]->is_new)
1937                         ctrls->offset[ISC_HIS_CFG_MODE_GR] = isc->gr_off_ctrl->val;
1938                 if (ctrl->cluster[ISC_CTRL_GB_OFF]->is_new)
1939                         ctrls->offset[ISC_HIS_CFG_MODE_GB] = isc->gb_off_ctrl->val;
1940
1941                 isc_update_awb_ctrls(isc);
1942
1943                 if (vb2_is_streaming(&isc->vb2_vidq)) {
1944                         /*
1945                          * If we are streaming, we can update profile to
1946                          * have the new settings in place.
1947                          */
1948                         isc_update_profile(isc);
1949                 } else {
1950                         /*
1951                          * The auto cluster will activate automatically this
1952                          * control. This has to be deactivated when not
1953                          * streaming.
1954                          */
1955                         v4l2_ctrl_activate(isc->do_wb_ctrl, false);
1956                 }
1957
1958                 /* if we have autowhitebalance on, start histogram procedure */
1959                 if (ctrls->awb == ISC_WB_AUTO &&
1960                     vb2_is_streaming(&isc->vb2_vidq) &&
1961                     ISC_IS_FORMAT_RAW(isc->config.sd_format->mbus_code))
1962                         isc_set_histogram(isc, true);
1963
1964                 /*
1965                  * for one time whitebalance adjustment, check the button,
1966                  * if it's pressed, perform the one time operation.
1967                  */
1968                 if (ctrls->awb == ISC_WB_NONE &&
1969                     ctrl->cluster[ISC_CTRL_DO_WB]->is_new &&
1970                     !(ctrl->cluster[ISC_CTRL_DO_WB]->flags &
1971                     V4L2_CTRL_FLAG_INACTIVE)) {
1972                         ctrls->awb = ISC_WB_ONETIME;
1973                         isc_set_histogram(isc, true);
1974                         v4l2_dbg(1, debug, &isc->v4l2_dev,
1975                                  "One time white-balance started.\n");
1976                 }
1977                 return 0;
1978         }
1979         return 0;
1980 }
1981
1982 static int isc_g_volatile_awb_ctrl(struct v4l2_ctrl *ctrl)
1983 {
1984         struct isc_device *isc = container_of(ctrl->handler,
1985                                              struct isc_device, ctrls.handler);
1986         struct isc_ctrls *ctrls = &isc->ctrls;
1987
1988         switch (ctrl->id) {
1989         /* being a cluster, this id will be called for every control */
1990         case V4L2_CID_AUTO_WHITE_BALANCE:
1991                 ctrl->cluster[ISC_CTRL_R_GAIN]->val =
1992                                         ctrls->gain[ISC_HIS_CFG_MODE_R];
1993                 ctrl->cluster[ISC_CTRL_B_GAIN]->val =
1994                                         ctrls->gain[ISC_HIS_CFG_MODE_B];
1995                 ctrl->cluster[ISC_CTRL_GR_GAIN]->val =
1996                                         ctrls->gain[ISC_HIS_CFG_MODE_GR];
1997                 ctrl->cluster[ISC_CTRL_GB_GAIN]->val =
1998                                         ctrls->gain[ISC_HIS_CFG_MODE_GB];
1999
2000                 ctrl->cluster[ISC_CTRL_R_OFF]->val =
2001                         ctrls->offset[ISC_HIS_CFG_MODE_R];
2002                 ctrl->cluster[ISC_CTRL_B_OFF]->val =
2003                         ctrls->offset[ISC_HIS_CFG_MODE_B];
2004                 ctrl->cluster[ISC_CTRL_GR_OFF]->val =
2005                         ctrls->offset[ISC_HIS_CFG_MODE_GR];
2006                 ctrl->cluster[ISC_CTRL_GB_OFF]->val =
2007                         ctrls->offset[ISC_HIS_CFG_MODE_GB];
2008                 break;
2009         }
2010         return 0;
2011 }
2012
2013 static const struct v4l2_ctrl_ops isc_awb_ops = {
2014         .s_ctrl = isc_s_awb_ctrl,
2015         .g_volatile_ctrl = isc_g_volatile_awb_ctrl,
2016 };
2017
2018 #define ISC_CTRL_OFF(_name, _id, _name_str) \
2019         static const struct v4l2_ctrl_config _name = { \
2020                 .ops = &isc_awb_ops, \
2021                 .id = _id, \
2022                 .name = _name_str, \
2023                 .type = V4L2_CTRL_TYPE_INTEGER, \
2024                 .flags = V4L2_CTRL_FLAG_SLIDER, \
2025                 .min = -4095, \
2026                 .max = 4095, \
2027                 .step = 1, \
2028                 .def = 0, \
2029         }
2030
2031 ISC_CTRL_OFF(isc_r_off_ctrl, ISC_CID_R_OFFSET, "Red Component Offset");
2032 ISC_CTRL_OFF(isc_b_off_ctrl, ISC_CID_B_OFFSET, "Blue Component Offset");
2033 ISC_CTRL_OFF(isc_gr_off_ctrl, ISC_CID_GR_OFFSET, "Green Red Component Offset");
2034 ISC_CTRL_OFF(isc_gb_off_ctrl, ISC_CID_GB_OFFSET, "Green Blue Component Offset");
2035
2036 #define ISC_CTRL_GAIN(_name, _id, _name_str) \
2037         static const struct v4l2_ctrl_config _name = { \
2038                 .ops = &isc_awb_ops, \
2039                 .id = _id, \
2040                 .name = _name_str, \
2041                 .type = V4L2_CTRL_TYPE_INTEGER, \
2042                 .flags = V4L2_CTRL_FLAG_SLIDER, \
2043                 .min = 0, \
2044                 .max = 8191, \
2045                 .step = 1, \
2046                 .def = 512, \
2047         }
2048
2049 ISC_CTRL_GAIN(isc_r_gain_ctrl, ISC_CID_R_GAIN, "Red Component Gain");
2050 ISC_CTRL_GAIN(isc_b_gain_ctrl, ISC_CID_B_GAIN, "Blue Component Gain");
2051 ISC_CTRL_GAIN(isc_gr_gain_ctrl, ISC_CID_GR_GAIN, "Green Red Component Gain");
2052 ISC_CTRL_GAIN(isc_gb_gain_ctrl, ISC_CID_GB_GAIN, "Green Blue Component Gain");
2053
2054 static int isc_ctrl_init(struct isc_device *isc)
2055 {
2056         const struct v4l2_ctrl_ops *ops = &isc_ctrl_ops;
2057         struct isc_ctrls *ctrls = &isc->ctrls;
2058         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
2059         int ret;
2060
2061         ctrls->hist_stat = HIST_INIT;
2062         isc_reset_awb_ctrls(isc);
2063
2064         ret = v4l2_ctrl_handler_init(hdl, 13);
2065         if (ret < 0)
2066                 return ret;
2067
2068         ctrls->brightness = 0;
2069         ctrls->contrast = 256;
2070
2071         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -1024, 1023, 1, 0);
2072         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -2048, 2047, 1, 256);
2073         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAMMA, 0, GAMMA_MAX, 1, 2);
2074         isc->awb_ctrl = v4l2_ctrl_new_std(hdl, &isc_awb_ops,
2075                                           V4L2_CID_AUTO_WHITE_BALANCE,
2076                                           0, 1, 1, 1);
2077
2078         /* do_white_balance is a button, so min,max,step,default are ignored */
2079         isc->do_wb_ctrl = v4l2_ctrl_new_std(hdl, &isc_awb_ops,
2080                                             V4L2_CID_DO_WHITE_BALANCE,
2081                                             0, 0, 0, 0);
2082
2083         if (!isc->do_wb_ctrl) {
2084                 ret = hdl->error;
2085                 v4l2_ctrl_handler_free(hdl);
2086                 return ret;
2087         }
2088
2089         v4l2_ctrl_activate(isc->do_wb_ctrl, false);
2090
2091         isc->r_gain_ctrl = v4l2_ctrl_new_custom(hdl, &isc_r_gain_ctrl, NULL);
2092         isc->b_gain_ctrl = v4l2_ctrl_new_custom(hdl, &isc_b_gain_ctrl, NULL);
2093         isc->gr_gain_ctrl = v4l2_ctrl_new_custom(hdl, &isc_gr_gain_ctrl, NULL);
2094         isc->gb_gain_ctrl = v4l2_ctrl_new_custom(hdl, &isc_gb_gain_ctrl, NULL);
2095         isc->r_off_ctrl = v4l2_ctrl_new_custom(hdl, &isc_r_off_ctrl, NULL);
2096         isc->b_off_ctrl = v4l2_ctrl_new_custom(hdl, &isc_b_off_ctrl, NULL);
2097         isc->gr_off_ctrl = v4l2_ctrl_new_custom(hdl, &isc_gr_off_ctrl, NULL);
2098         isc->gb_off_ctrl = v4l2_ctrl_new_custom(hdl, &isc_gb_off_ctrl, NULL);
2099
2100         /*
2101          * The cluster is in auto mode with autowhitebalance enabled
2102          * and manual mode otherwise.
2103          */
2104         v4l2_ctrl_auto_cluster(10, &isc->awb_ctrl, 0, true);
2105
2106         v4l2_ctrl_handler_setup(hdl);
2107
2108         return 0;
2109 }
2110
2111 static int isc_async_bound(struct v4l2_async_notifier *notifier,
2112                             struct v4l2_subdev *subdev,
2113                             struct v4l2_async_subdev *asd)
2114 {
2115         struct isc_device *isc = container_of(notifier->v4l2_dev,
2116                                               struct isc_device, v4l2_dev);
2117         struct isc_subdev_entity *subdev_entity =
2118                 container_of(notifier, struct isc_subdev_entity, notifier);
2119
2120         if (video_is_registered(&isc->video_dev)) {
2121                 v4l2_err(&isc->v4l2_dev, "only supports one sub-device.\n");
2122                 return -EBUSY;
2123         }
2124
2125         subdev_entity->sd = subdev;
2126
2127         return 0;
2128 }
2129
2130 static void isc_async_unbind(struct v4l2_async_notifier *notifier,
2131                               struct v4l2_subdev *subdev,
2132                               struct v4l2_async_subdev *asd)
2133 {
2134         struct isc_device *isc = container_of(notifier->v4l2_dev,
2135                                               struct isc_device, v4l2_dev);
2136         cancel_work_sync(&isc->awb_work);
2137         video_unregister_device(&isc->video_dev);
2138         v4l2_ctrl_handler_free(&isc->ctrls.handler);
2139 }
2140
2141 static struct isc_format *find_format_by_code(unsigned int code, int *index)
2142 {
2143         struct isc_format *fmt = &formats_list[0];
2144         unsigned int i;
2145
2146         for (i = 0; i < ARRAY_SIZE(formats_list); i++) {
2147                 if (fmt->mbus_code == code) {
2148                         *index = i;
2149                         return fmt;
2150                 }
2151
2152                 fmt++;
2153         }
2154
2155         return NULL;
2156 }
2157
2158 static int isc_formats_init(struct isc_device *isc)
2159 {
2160         struct isc_format *fmt;
2161         struct v4l2_subdev *subdev = isc->current_subdev->sd;
2162         unsigned int num_fmts, i, j;
2163         u32 list_size = ARRAY_SIZE(formats_list);
2164         struct v4l2_subdev_mbus_code_enum mbus_code = {
2165                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
2166         };
2167
2168         num_fmts = 0;
2169         while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
2170                NULL, &mbus_code)) {
2171                 mbus_code.index++;
2172
2173                 fmt = find_format_by_code(mbus_code.code, &i);
2174                 if (!fmt) {
2175                         v4l2_warn(&isc->v4l2_dev, "Mbus code %x not supported\n",
2176                                   mbus_code.code);
2177                         continue;
2178                 }
2179
2180                 fmt->sd_support = true;
2181                 num_fmts++;
2182         }
2183
2184         if (!num_fmts)
2185                 return -ENXIO;
2186
2187         isc->num_user_formats = num_fmts;
2188         isc->user_formats = devm_kcalloc(isc->dev,
2189                                          num_fmts, sizeof(*isc->user_formats),
2190                                          GFP_KERNEL);
2191         if (!isc->user_formats)
2192                 return -ENOMEM;
2193
2194         fmt = &formats_list[0];
2195         for (i = 0, j = 0; i < list_size; i++) {
2196                 if (fmt->sd_support)
2197                         isc->user_formats[j++] = fmt;
2198                 fmt++;
2199         }
2200
2201         return 0;
2202 }
2203
2204 static int isc_set_default_fmt(struct isc_device *isc)
2205 {
2206         struct v4l2_format f = {
2207                 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
2208                 .fmt.pix = {
2209                         .width          = VGA_WIDTH,
2210                         .height         = VGA_HEIGHT,
2211                         .field          = V4L2_FIELD_NONE,
2212                         .pixelformat    = isc->user_formats[0]->fourcc,
2213                 },
2214         };
2215         int ret;
2216
2217         ret = isc_try_fmt(isc, &f, NULL);
2218         if (ret)
2219                 return ret;
2220
2221         isc->fmt = f;
2222         return 0;
2223 }
2224
2225 static int isc_async_complete(struct v4l2_async_notifier *notifier)
2226 {
2227         struct isc_device *isc = container_of(notifier->v4l2_dev,
2228                                               struct isc_device, v4l2_dev);
2229         struct video_device *vdev = &isc->video_dev;
2230         struct vb2_queue *q = &isc->vb2_vidq;
2231         int ret = 0;
2232
2233         INIT_WORK(&isc->awb_work, isc_awb_work);
2234
2235         ret = v4l2_device_register_subdev_nodes(&isc->v4l2_dev);
2236         if (ret < 0) {
2237                 v4l2_err(&isc->v4l2_dev, "Failed to register subdev nodes\n");
2238                 return ret;
2239         }
2240
2241         isc->current_subdev = container_of(notifier,
2242                                            struct isc_subdev_entity, notifier);
2243         mutex_init(&isc->lock);
2244         init_completion(&isc->comp);
2245
2246         /* Initialize videobuf2 queue */
2247         q->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2248         q->io_modes             = VB2_MMAP | VB2_DMABUF | VB2_READ;
2249         q->drv_priv             = isc;
2250         q->buf_struct_size      = sizeof(struct isc_buffer);
2251         q->ops                  = &isc_vb2_ops;
2252         q->mem_ops              = &vb2_dma_contig_memops;
2253         q->timestamp_flags      = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
2254         q->lock                 = &isc->lock;
2255         q->min_buffers_needed   = 1;
2256         q->dev                  = isc->dev;
2257
2258         ret = vb2_queue_init(q);
2259         if (ret < 0) {
2260                 v4l2_err(&isc->v4l2_dev,
2261                          "vb2_queue_init() failed: %d\n", ret);
2262                 goto isc_async_complete_err;
2263         }
2264
2265         /* Init video dma queues */
2266         INIT_LIST_HEAD(&isc->dma_queue);
2267         spin_lock_init(&isc->dma_queue_lock);
2268         spin_lock_init(&isc->awb_lock);
2269
2270         ret = isc_formats_init(isc);
2271         if (ret < 0) {
2272                 v4l2_err(&isc->v4l2_dev,
2273                          "Init format failed: %d\n", ret);
2274                 goto isc_async_complete_err;
2275         }
2276
2277         ret = isc_set_default_fmt(isc);
2278         if (ret) {
2279                 v4l2_err(&isc->v4l2_dev, "Could not set default format\n");
2280                 goto isc_async_complete_err;
2281         }
2282
2283         ret = isc_ctrl_init(isc);
2284         if (ret) {
2285                 v4l2_err(&isc->v4l2_dev, "Init isc ctrols failed: %d\n", ret);
2286                 goto isc_async_complete_err;
2287         }
2288
2289         /* Register video device */
2290         strscpy(vdev->name, ATMEL_ISC_NAME, sizeof(vdev->name));
2291         vdev->release           = video_device_release_empty;
2292         vdev->fops              = &isc_fops;
2293         vdev->ioctl_ops         = &isc_ioctl_ops;
2294         vdev->v4l2_dev          = &isc->v4l2_dev;
2295         vdev->vfl_dir           = VFL_DIR_RX;
2296         vdev->queue             = q;
2297         vdev->lock              = &isc->lock;
2298         vdev->ctrl_handler      = &isc->ctrls.handler;
2299         vdev->device_caps       = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
2300         video_set_drvdata(vdev, isc);
2301
2302         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
2303         if (ret < 0) {
2304                 v4l2_err(&isc->v4l2_dev,
2305                          "video_register_device failed: %d\n", ret);
2306                 goto isc_async_complete_err;
2307         }
2308
2309         return 0;
2310
2311 isc_async_complete_err:
2312         mutex_destroy(&isc->lock);
2313         return ret;
2314 }
2315
2316 const struct v4l2_async_notifier_operations isc_async_ops = {
2317         .bound = isc_async_bound,
2318         .unbind = isc_async_unbind,
2319         .complete = isc_async_complete,
2320 };
2321
2322 void isc_subdev_cleanup(struct isc_device *isc)
2323 {
2324         struct isc_subdev_entity *subdev_entity;
2325
2326         list_for_each_entry(subdev_entity, &isc->subdev_entities, list) {
2327                 v4l2_async_notifier_unregister(&subdev_entity->notifier);
2328                 v4l2_async_notifier_cleanup(&subdev_entity->notifier);
2329         }
2330
2331         INIT_LIST_HEAD(&isc->subdev_entities);
2332 }
2333
2334 int isc_pipeline_init(struct isc_device *isc)
2335 {
2336         struct device *dev = isc->dev;
2337         struct regmap *regmap = isc->regmap;
2338         struct regmap_field *regs;
2339         unsigned int i;
2340
2341         /* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
2342         const struct reg_field regfields[ISC_PIPE_LINE_NODE_NUM] = {
2343                 REG_FIELD(ISC_WB_CTRL, 0, 0),
2344                 REG_FIELD(ISC_CFA_CTRL, 0, 0),
2345                 REG_FIELD(ISC_CC_CTRL, 0, 0),
2346                 REG_FIELD(ISC_GAM_CTRL, 0, 0),
2347                 REG_FIELD(ISC_GAM_CTRL, 1, 1),
2348                 REG_FIELD(ISC_GAM_CTRL, 2, 2),
2349                 REG_FIELD(ISC_GAM_CTRL, 3, 3),
2350                 REG_FIELD(ISC_CSC_CTRL, 0, 0),
2351                 REG_FIELD(ISC_CBC_CTRL, 0, 0),
2352                 REG_FIELD(ISC_SUB422_CTRL, 0, 0),
2353                 REG_FIELD(ISC_SUB420_CTRL, 0, 0),
2354         };
2355
2356         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
2357                 regs = devm_regmap_field_alloc(dev, regmap, regfields[i]);
2358                 if (IS_ERR(regs))
2359                         return PTR_ERR(regs);
2360
2361                 isc->pipeline[i] =  regs;
2362         }
2363
2364         return 0;
2365 }
2366
2367 /* regmap configuration */
2368 #define ATMEL_ISC_REG_MAX    0xbfc
2369 const struct regmap_config isc_regmap_config = {
2370         .reg_bits       = 32,
2371         .reg_stride     = 4,
2372         .val_bits       = 32,
2373         .max_register   = ATMEL_ISC_REG_MAX,
2374 };
2375