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

   1 /*
   2  * Atmel Image Sensor Controller (ISC) driver
   3  *
   4  * Copyright (C) 2016 Atmel
   5  *
   6  * Author: Songjun Wu <songjun.wu@microchip.com>
   7  *
   8  * This program is free software; you may redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; version 2 of the License.
  11  *
  12  * Sensor-->PFE-->WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB-->RLP-->DMA
  13  *
  14  * ISC video pipeline integrates the following submodules:
  15  * PFE: Parallel Front End to sample the camera sensor input stream
  16  *  WB: Programmable white balance in the Bayer domain
  17  * CFA: Color filter array interpolation module
  18  *  CC: Programmable color correction
  19  * GAM: Gamma correction
  20  * CSC: Programmable color space conversion
  21  * CBC: Contrast and Brightness control
  22  * SUB: This module performs YCbCr444 to YCbCr420 chrominance subsampling
  23  * RLP: This module performs rounding, range limiting
  24  *      and packing of the incoming data
  25  */
  26
  27 #include <linux/clk.h>
  28 #include <linux/clkdev.h>
  29 #include <linux/clk-provider.h>
  30 #include <linux/delay.h>
  31 #include <linux/interrupt.h>
  32 #include <linux/math64.h>
  33 #include <linux/module.h>
  34 #include <linux/of.h>
  35 #include <linux/of_graph.h>
  36 #include <linux/platform_device.h>
  37 #include <linux/pm_runtime.h>
  38 #include <linux/regmap.h>
  39 #include <linux/videodev2.h>
  40
  41 #include <media/v4l2-ctrls.h>
  42 #include <media/v4l2-device.h>
  43 #include <media/v4l2-event.h>
  44 #include <media/v4l2-image-sizes.h>
  45 #include <media/v4l2-ioctl.h>
  46 #include <media/v4l2-fwnode.h>
  47 #include <media/v4l2-subdev.h>
  48 #include <media/videobuf2-dma-contig.h>
  49
  50 #include "atmel-isc-regs.h"
  51
  52 #define ATMEL_ISC_NAME          "atmel_isc"
  53
  54 #define ISC_MAX_SUPPORT_WIDTH   2592
  55 #define ISC_MAX_SUPPORT_HEIGHT  1944
  56
  57 #define ISC_CLK_MAX_DIV         255
  58
  59 enum isc_clk_id {
  60         ISC_ISPCK = 0,
  61         ISC_MCK = 1,
  62 };
  63
  64 struct isc_clk {
  65         struct clk_hw   hw;
  66         struct clk      *clk;
  67         struct regmap   *regmap;
  68         u8              id;
  69         u8              parent_id;
  70         u32             div;
  71         struct device   *dev;
  72 };
  73
  74 #define to_isc_clk(hw) container_of(hw, struct isc_clk, hw)
  75
  76 struct isc_buffer {
  77         struct vb2_v4l2_buffer  vb;
  78         struct list_head        list;
  79 };
  80
  81 struct isc_subdev_entity {
  82         struct v4l2_subdev              *sd;
  83         struct v4l2_async_subdev        *asd;
  84         struct v4l2_async_notifier      notifier;
  85         struct v4l2_subdev_pad_config   *config;
  86
  87         u32 pfe_cfg0;
  88
  89         struct list_head list;
  90 };
  91
  92 /*
  93  * struct isc_format - ISC media bus format information
  94  * @fourcc:             Fourcc code for this format
  95  * @mbus_code:          V4L2 media bus format code.
  96  * @bpp:                Bits per pixel (when stored in memory)
  97  * @reg_bps:            reg value for bits per sample
  98  *                      (when transferred over a bus)
  99  * @pipeline:           pipeline switch
 100  * @sd_support:         Subdev supports this format
 101  * @isc_support:        ISC can convert raw format to this format
 102  */
 103 struct isc_format {
 104         u32     fourcc;
 105         u32     mbus_code;
 106         u8      bpp;
 107
 108         u32     reg_bps;
 109         u32     reg_bay_cfg;
 110         u32     reg_rlp_mode;
 111         u32     reg_dcfg_imode;
 112         u32     reg_dctrl_dview;
 113
 114         u32     pipeline;
 115
 116         bool    sd_support;
 117         bool    isc_support;
 118 };
 119
 120
 121 #define HIST_ENTRIES            512
 122 #define HIST_BAYER              (ISC_HIS_CFG_MODE_B + 1)
 123
 124 enum{
 125         HIST_INIT = 0,
 126         HIST_ENABLED,
 127         HIST_DISABLED,
 128 };
 129
 130 struct isc_ctrls {
 131         struct v4l2_ctrl_handler handler;
 132
 133         u32 brightness;
 134         u32 contrast;
 135         u8 gamma_index;
 136         u8 awb;
 137
 138         u32 r_gain;
 139         u32 b_gain;
 140
 141         u32 hist_entry[HIST_ENTRIES];
 142         u32 hist_count[HIST_BAYER];
 143         u8 hist_id;
 144         u8 hist_stat;
 145 };
 146
 147 #define ISC_PIPE_LINE_NODE_NUM  11
 148
 149 struct isc_device {
 150         struct regmap           *regmap;
 151         struct clk              *hclock;
 152         struct clk              *ispck;
 153         struct isc_clk          isc_clks[2];
 154
 155         struct device           *dev;
 156         struct v4l2_device      v4l2_dev;
 157         struct video_device     video_dev;
 158
 159         struct vb2_queue        vb2_vidq;
 160         spinlock_t              dma_queue_lock;
 161         struct list_head        dma_queue;
 162         struct isc_buffer       *cur_frm;
 163         unsigned int            sequence;
 164         bool                    stop;
 165         struct completion       comp;
 166
 167         struct v4l2_format      fmt;
 168         struct isc_format       **user_formats;
 169         unsigned int            num_user_formats;
 170         const struct isc_format *current_fmt;
 171         const struct isc_format *raw_fmt;
 172
 173         struct isc_ctrls        ctrls;
 174         struct work_struct      awb_work;
 175
 176         struct mutex            lock;
 177
 178         struct regmap_field     *pipeline[ISC_PIPE_LINE_NODE_NUM];
 179
 180         struct isc_subdev_entity        *current_subdev;
 181         struct list_head                subdev_entities;
 182 };
 183
 184 #define RAW_FMT_IND_START    0
 185 #define RAW_FMT_IND_END      11
 186 #define ISC_FMT_IND_START    12
 187 #define ISC_FMT_IND_END      14
 188
 189 static struct isc_format isc_formats[] = {
 190         { V4L2_PIX_FMT_SBGGR8, MEDIA_BUS_FMT_SBGGR8_1X8, 8,
 191           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT8,
 192           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
 193           false, false },
 194         { V4L2_PIX_FMT_SGBRG8, MEDIA_BUS_FMT_SGBRG8_1X8, 8,
 195           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_GBGB, ISC_RLP_CFG_MODE_DAT8,
 196           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
 197           false, false },
 198         { V4L2_PIX_FMT_SGRBG8, MEDIA_BUS_FMT_SGRBG8_1X8, 8,
 199           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_GRGR, ISC_RLP_CFG_MODE_DAT8,
 200           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
 201           false, false },
 202         { V4L2_PIX_FMT_SRGGB8, MEDIA_BUS_FMT_SRGGB8_1X8, 8,
 203           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_RGRG, ISC_RLP_CFG_MODE_DAT8,
 204           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
 205           false, false },
 206
 207         { V4L2_PIX_FMT_SBGGR10, MEDIA_BUS_FMT_SBGGR10_1X10, 16,
 208           ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT10,
 209           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 210           false, false },
 211         { V4L2_PIX_FMT_SGBRG10, MEDIA_BUS_FMT_SGBRG10_1X10, 16,
 212           ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_GBGB, ISC_RLP_CFG_MODE_DAT10,
 213           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 214           false, false },
 215         { V4L2_PIX_FMT_SGRBG10, MEDIA_BUS_FMT_SGRBG10_1X10, 16,
 216           ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_GRGR, ISC_RLP_CFG_MODE_DAT10,
 217           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 218           false, false },
 219         { V4L2_PIX_FMT_SRGGB10, MEDIA_BUS_FMT_SRGGB10_1X10, 16,
 220           ISC_PFG_CFG0_BPS_TEN, ISC_BAY_CFG_RGRG, ISC_RLP_CFG_MODE_DAT10,
 221           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 222           false, false },
 223
 224         { V4L2_PIX_FMT_SBGGR12, MEDIA_BUS_FMT_SBGGR12_1X12, 16,
 225           ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT12,
 226           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 227           false, false },
 228         { V4L2_PIX_FMT_SGBRG12, MEDIA_BUS_FMT_SGBRG12_1X12, 16,
 229           ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_GBGB, ISC_RLP_CFG_MODE_DAT12,
 230           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 231           false, false },
 232         { V4L2_PIX_FMT_SGRBG12, MEDIA_BUS_FMT_SGRBG12_1X12, 16,
 233           ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_GRGR, ISC_RLP_CFG_MODE_DAT12,
 234           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 235           false, false },
 236         { V4L2_PIX_FMT_SRGGB12, MEDIA_BUS_FMT_SRGGB12_1X12, 16,
 237           ISC_PFG_CFG0_BPS_TWELVE, ISC_BAY_CFG_RGRG, ISC_RLP_CFG_MODE_DAT12,
 238           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x0,
 239           false, false },
 240
 241         { V4L2_PIX_FMT_YUV420, 0x0, 12,
 242           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_YYCC,
 243           ISC_DCFG_IMODE_YC420P, ISC_DCTRL_DVIEW_PLANAR, 0x7fb,
 244           false, false },
 245         { V4L2_PIX_FMT_YUV422P, 0x0, 16,
 246           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_YYCC,
 247           ISC_DCFG_IMODE_YC422P, ISC_DCTRL_DVIEW_PLANAR, 0x3fb,
 248           false, false },
 249         { V4L2_PIX_FMT_RGB565, MEDIA_BUS_FMT_RGB565_2X8_LE, 16,
 250           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_RGB565,
 251           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, 0x7b,
 252           false, false },
 253
 254         { V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_YUYV8_2X8, 16,
 255           ISC_PFE_CFG0_BPS_EIGHT, ISC_BAY_CFG_BGBG, ISC_RLP_CFG_MODE_DAT8,
 256           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, 0x0,
 257           false, false },
 258 };
 259
 260 #define GAMMA_MAX       2
 261 #define GAMMA_ENTRIES   64
 262
 263 /* Gamma table with gamma 1/2.2 */
 264 static const u32 isc_gamma_table[GAMMA_MAX + 1][GAMMA_ENTRIES] = {
 265         /* 0 --> gamma 1/1.8 */
 266         {      0x65,  0x66002F,  0x950025,  0xBB0020,  0xDB001D,  0xF8001A,
 267           0x1130018, 0x12B0017, 0x1420016, 0x1580014, 0x16D0013, 0x1810012,
 268           0x1940012, 0x1A60012, 0x1B80011, 0x1C90010, 0x1DA0010, 0x1EA000F,
 269           0x1FA000F, 0x209000F, 0x218000F, 0x227000E, 0x235000E, 0x243000E,
 270           0x251000E, 0x25F000D, 0x26C000D, 0x279000D, 0x286000D, 0x293000C,
 271           0x2A0000C, 0x2AC000C, 0x2B8000C, 0x2C4000C, 0x2D0000B, 0x2DC000B,
 272           0x2E7000B, 0x2F3000B, 0x2FE000B, 0x309000B, 0x314000B, 0x31F000A,
 273           0x32A000A, 0x334000B, 0x33F000A, 0x349000A, 0x354000A, 0x35E000A,
 274           0x368000A, 0x372000A, 0x37C000A, 0x386000A, 0x3900009, 0x399000A,
 275           0x3A30009, 0x3AD0009, 0x3B60009, 0x3BF000A, 0x3C90009, 0x3D20009,
 276           0x3DB0009, 0x3E40009, 0x3ED0009, 0x3F60009 },
 277
 278         /* 1 --> gamma 1/2 */
 279         {      0x7F,  0x800034,  0xB50028,  0xDE0021, 0x100001E, 0x11E001B,
 280           0x1390019, 0x1520017, 0x16A0015, 0x1800014, 0x1940014, 0x1A80013,
 281           0x1BB0012, 0x1CD0011, 0x1DF0010, 0x1EF0010, 0x200000F, 0x20F000F,
 282           0x21F000E, 0x22D000F, 0x23C000E, 0x24A000E, 0x258000D, 0x265000D,
 283           0x273000C, 0x27F000D, 0x28C000C, 0x299000C, 0x2A5000C, 0x2B1000B,
 284           0x2BC000C, 0x2C8000B, 0x2D3000C, 0x2DF000B, 0x2EA000A, 0x2F5000A,
 285           0x2FF000B, 0x30A000A, 0x314000B, 0x31F000A, 0x329000A, 0x333000A,
 286           0x33D0009, 0x3470009, 0x350000A, 0x35A0009, 0x363000A, 0x36D0009,
 287           0x3760009, 0x37F0009, 0x3880009, 0x3910009, 0x39A0009, 0x3A30009,
 288           0x3AC0008, 0x3B40009, 0x3BD0008, 0x3C60008, 0x3CE0008, 0x3D60009,
 289           0x3DF0008, 0x3E70008, 0x3EF0008, 0x3F70008 },
 290
 291         /* 2 --> gamma 1/2.2 */
 292         {      0x99,  0x9B0038,  0xD4002A,  0xFF0023, 0x122001F, 0x141001B,
 293           0x15D0019, 0x1760017, 0x18E0015, 0x1A30015, 0x1B80013, 0x1CC0012,
 294           0x1DE0011, 0x1F00010, 0x2010010, 0x2110010, 0x221000F, 0x230000F,
 295           0x23F000E, 0x24D000E, 0x25B000D, 0x269000C, 0x276000C, 0x283000C,
 296           0x28F000C, 0x29B000C, 0x2A7000C, 0x2B3000B, 0x2BF000B, 0x2CA000B,
 297           0x2D5000B, 0x2E0000A, 0x2EB000A, 0x2F5000A, 0x2FF000A, 0x30A000A,
 298           0x3140009, 0x31E0009, 0x327000A, 0x3310009, 0x33A0009, 0x3440009,
 299           0x34D0009, 0x3560009, 0x35F0009, 0x3680008, 0x3710008, 0x3790009,
 300           0x3820008, 0x38A0008, 0x3930008, 0x39B0008, 0x3A30008, 0x3AB0008,
 301           0x3B30008, 0x3BB0008, 0x3C30008, 0x3CB0007, 0x3D20008, 0x3DA0007,
 302           0x3E20007, 0x3E90007, 0x3F00008, 0x3F80007 },
 303 };
 304
 305 static unsigned int sensor_preferred = 1;
 306 module_param(sensor_preferred, uint, 0644);
 307 MODULE_PARM_DESC(sensor_preferred,
 308                  "Sensor is preferred to output the specified format (1-on 0-off), default 1");
 309
 310 static int isc_clk_enable(struct clk_hw *hw)
 311 {
 312         struct isc_clk *isc_clk = to_isc_clk(hw);
 313         u32 id = isc_clk->id;
 314         struct regmap *regmap = isc_clk->regmap;
 315
 316         dev_dbg(isc_clk->dev, "ISC CLK: %s, div = %d, parent id = %d\n",
 317                 __func__, isc_clk->div, isc_clk->parent_id);
 318
 319         regmap_update_bits(regmap, ISC_CLKCFG,
 320                            ISC_CLKCFG_DIV_MASK(id) | ISC_CLKCFG_SEL_MASK(id),
 321                            (isc_clk->div << ISC_CLKCFG_DIV_SHIFT(id)) |
 322                            (isc_clk->parent_id << ISC_CLKCFG_SEL_SHIFT(id)));
 323
 324         regmap_write(regmap, ISC_CLKEN, ISC_CLK(id));
 325
 326         return 0;
 327 }
 328
 329 static void isc_clk_disable(struct clk_hw *hw)
 330 {
 331         struct isc_clk *isc_clk = to_isc_clk(hw);
 332         u32 id = isc_clk->id;
 333
 334         regmap_write(isc_clk->regmap, ISC_CLKDIS, ISC_CLK(id));
 335 }
 336
 337 static int isc_clk_is_enabled(struct clk_hw *hw)
 338 {
 339         struct isc_clk *isc_clk = to_isc_clk(hw);
 340         u32 status;
 341
 342         regmap_read(isc_clk->regmap, ISC_CLKSR, &status);
 343
 344         return status & ISC_CLK(isc_clk->id) ? 1 : 0;
 345 }
 346
 347 static unsigned long
 348 isc_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 349 {
 350         struct isc_clk *isc_clk = to_isc_clk(hw);
 351
 352         return DIV_ROUND_CLOSEST(parent_rate, isc_clk->div + 1);
 353 }
 354
 355 static int isc_clk_determine_rate(struct clk_hw *hw,
 356                                    struct clk_rate_request *req)
 357 {
 358         struct isc_clk *isc_clk = to_isc_clk(hw);
 359         long best_rate = -EINVAL;
 360         int best_diff = -1;
 361         unsigned int i, div;
 362
 363         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 364                 struct clk_hw *parent;
 365                 unsigned long parent_rate;
 366
 367                 parent = clk_hw_get_parent_by_index(hw, i);
 368                 if (!parent)
 369                         continue;
 370
 371                 parent_rate = clk_hw_get_rate(parent);
 372                 if (!parent_rate)
 373                         continue;
 374
 375                 for (div = 1; div < ISC_CLK_MAX_DIV + 2; div++) {
 376                         unsigned long rate;
 377                         int diff;
 378
 379                         rate = DIV_ROUND_CLOSEST(parent_rate, div);
 380                         diff = abs(req->rate - rate);
 381
 382                         if (best_diff < 0 || best_diff > diff) {
 383                                 best_rate = rate;
 384                                 best_diff = diff;
 385                                 req->best_parent_rate = parent_rate;
 386                                 req->best_parent_hw = parent;
 387                         }
 388
 389                         if (!best_diff || rate < req->rate)
 390                                 break;
 391                 }
 392
 393                 if (!best_diff)
 394                         break;
 395         }
 396
 397         dev_dbg(isc_clk->dev,
 398                 "ISC CLK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
 399                 __func__, best_rate,
 400                 __clk_get_name((req->best_parent_hw)->clk),
 401                 req->best_parent_rate);
 402
 403         if (best_rate < 0)
 404                 return best_rate;
 405
 406         req->rate = best_rate;
 407
 408         return 0;
 409 }
 410
 411 static int isc_clk_set_parent(struct clk_hw *hw, u8 index)
 412 {
 413         struct isc_clk *isc_clk = to_isc_clk(hw);
 414
 415         if (index >= clk_hw_get_num_parents(hw))
 416                 return -EINVAL;
 417
 418         isc_clk->parent_id = index;
 419
 420         return 0;
 421 }
 422
 423 static u8 isc_clk_get_parent(struct clk_hw *hw)
 424 {
 425         struct isc_clk *isc_clk = to_isc_clk(hw);
 426
 427         return isc_clk->parent_id;
 428 }
 429
 430 static int isc_clk_set_rate(struct clk_hw *hw,
 431                              unsigned long rate,
 432                              unsigned long parent_rate)
 433 {
 434         struct isc_clk *isc_clk = to_isc_clk(hw);
 435         u32 div;
 436
 437         if (!rate)
 438                 return -EINVAL;
 439
 440         div = DIV_ROUND_CLOSEST(parent_rate, rate);
 441         if (div > (ISC_CLK_MAX_DIV + 1) || !div)
 442                 return -EINVAL;
 443
 444         isc_clk->div = div - 1;
 445
 446         return 0;
 447 }
 448
 449 static const struct clk_ops isc_clk_ops = {
 450         .enable         = isc_clk_enable,
 451         .disable        = isc_clk_disable,
 452         .is_enabled     = isc_clk_is_enabled,
 453         .recalc_rate    = isc_clk_recalc_rate,
 454         .determine_rate = isc_clk_determine_rate,
 455         .set_parent     = isc_clk_set_parent,
 456         .get_parent     = isc_clk_get_parent,
 457         .set_rate       = isc_clk_set_rate,
 458 };
 459
 460 static int isc_clk_register(struct isc_device *isc, unsigned int id)
 461 {
 462         struct regmap *regmap = isc->regmap;
 463         struct device_node *np = isc->dev->of_node;
 464         struct isc_clk *isc_clk;
 465         struct clk_init_data init;
 466         const char *clk_name = np->name;
 467         const char *parent_names[3];
 468         int num_parents;
 469
 470         num_parents = of_clk_get_parent_count(np);
 471         if (num_parents < 1 || num_parents > 3)
 472                 return -EINVAL;
 473
 474         if (num_parents > 2 && id == ISC_ISPCK)
 475                 num_parents = 2;
 476
 477         of_clk_parent_fill(np, parent_names, num_parents);
 478
 479         if (id == ISC_MCK)
 480                 of_property_read_string(np, "clock-output-names", &clk_name);
 481         else
 482                 clk_name = "isc-ispck";
 483
 484         init.parent_names       = parent_names;
 485         init.num_parents        = num_parents;
 486         init.name               = clk_name;
 487         init.ops                = &isc_clk_ops;
 488         init.flags              = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
 489
 490         isc_clk = &isc->isc_clks[id];
 491         isc_clk->hw.init        = &init;
 492         isc_clk->regmap         = regmap;
 493         isc_clk->id             = id;
 494         isc_clk->dev            = isc->dev;
 495
 496         isc_clk->clk = clk_register(isc->dev, &isc_clk->hw);
 497         if (IS_ERR(isc_clk->clk)) {
 498                 dev_err(isc->dev, "%s: clock register fail\n", clk_name);
 499                 return PTR_ERR(isc_clk->clk);
 500         } else if (id == ISC_MCK)
 501                 of_clk_add_provider(np, of_clk_src_simple_get, isc_clk->clk);
 502
 503         return 0;
 504 }
 505
 506 static int isc_clk_init(struct isc_device *isc)
 507 {
 508         unsigned int i;
 509         int ret;
 510
 511         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++)
 512                 isc->isc_clks[i].clk = ERR_PTR(-EINVAL);
 513
 514         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
 515                 ret = isc_clk_register(isc, i);
 516                 if (ret)
 517                         return ret;
 518         }
 519
 520         return 0;
 521 }
 522
 523 static void isc_clk_cleanup(struct isc_device *isc)
 524 {
 525         unsigned int i;
 526
 527         of_clk_del_provider(isc->dev->of_node);
 528
 529         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
 530                 struct isc_clk *isc_clk = &isc->isc_clks[i];
 531
 532                 if (!IS_ERR(isc_clk->clk))
 533                         clk_unregister(isc_clk->clk);
 534         }
 535 }
 536
 537 static int isc_queue_setup(struct vb2_queue *vq,
 538                             unsigned int *nbuffers, unsigned int *nplanes,
 539                             unsigned int sizes[], struct device *alloc_devs[])
 540 {
 541         struct isc_device *isc = vb2_get_drv_priv(vq);
 542         unsigned int size = isc->fmt.fmt.pix.sizeimage;
 543
 544         if (*nplanes)
 545                 return sizes[0] < size ? -EINVAL : 0;
 546
 547         *nplanes = 1;
 548         sizes[0] = size;
 549
 550         return 0;
 551 }
 552
 553 static int isc_buffer_prepare(struct vb2_buffer *vb)
 554 {
 555         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 556         struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
 557         unsigned long size = isc->fmt.fmt.pix.sizeimage;
 558
 559         if (vb2_plane_size(vb, 0) < size) {
 560                 v4l2_err(&isc->v4l2_dev, "buffer too small (%lu < %lu)\n",
 561                          vb2_plane_size(vb, 0), size);
 562                 return -EINVAL;
 563         }
 564
 565         vb2_set_plane_payload(vb, 0, size);
 566
 567         vbuf->field = isc->fmt.fmt.pix.field;
 568
 569         return 0;
 570 }
 571
 572 static inline bool sensor_is_preferred(const struct isc_format *isc_fmt)
 573 {
 574         return (sensor_preferred && isc_fmt->sd_support) ||
 575                 !isc_fmt->isc_support;
 576 }
 577
 578 static void isc_start_dma(struct isc_device *isc)
 579 {
 580         struct regmap *regmap = isc->regmap;
 581         struct v4l2_pix_format *pixfmt = &isc->fmt.fmt.pix;
 582         u32 sizeimage = pixfmt->sizeimage;
 583         u32 dctrl_dview;
 584         dma_addr_t addr0;
 585
 586         addr0 = vb2_dma_contig_plane_dma_addr(&isc->cur_frm->vb.vb2_buf, 0);
 587         regmap_write(regmap, ISC_DAD0, addr0);
 588
 589         switch (pixfmt->pixelformat) {
 590         case V4L2_PIX_FMT_YUV420:
 591                 regmap_write(regmap, ISC_DAD1, addr0 + (sizeimage * 2) / 3);
 592                 regmap_write(regmap, ISC_DAD2, addr0 + (sizeimage * 5) / 6);
 593                 break;
 594         case V4L2_PIX_FMT_YUV422P:
 595                 regmap_write(regmap, ISC_DAD1, addr0 + sizeimage / 2);
 596                 regmap_write(regmap, ISC_DAD2, addr0 + (sizeimage * 3) / 4);
 597                 break;
 598         default:
 599                 break;
 600         }
 601
 602         if (sensor_is_preferred(isc->current_fmt))
 603                 dctrl_dview = ISC_DCTRL_DVIEW_PACKED;
 604         else
 605                 dctrl_dview = isc->current_fmt->reg_dctrl_dview;
 606
 607         regmap_write(regmap, ISC_DCTRL, dctrl_dview | ISC_DCTRL_IE_IS);
 608         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_CAPTURE);
 609 }
 610
 611 static void isc_set_pipeline(struct isc_device *isc, u32 pipeline)
 612 {
 613         struct regmap *regmap = isc->regmap;
 614         struct isc_ctrls *ctrls = &isc->ctrls;
 615         u32 val, bay_cfg;
 616         const u32 *gamma;
 617         unsigned int i;
 618
 619         /* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
 620         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
 621                 val = pipeline & BIT(i) ? 1 : 0;
 622                 regmap_field_write(isc->pipeline[i], val);
 623         }
 624
 625         if (!pipeline)
 626                 return;
 627
 628         bay_cfg = isc->raw_fmt->reg_bay_cfg;
 629
 630         regmap_write(regmap, ISC_WB_CFG, bay_cfg);
 631         regmap_write(regmap, ISC_WB_O_RGR, 0x0);
 632         regmap_write(regmap, ISC_WB_O_BGR, 0x0);
 633         regmap_write(regmap, ISC_WB_G_RGR, ctrls->r_gain | (0x1 << 25));
 634         regmap_write(regmap, ISC_WB_G_BGR, ctrls->b_gain | (0x1 << 25));
 635
 636         regmap_write(regmap, ISC_CFA_CFG, bay_cfg | ISC_CFA_CFG_EITPOL);
 637
 638         gamma = &isc_gamma_table[ctrls->gamma_index][0];
 639         regmap_bulk_write(regmap, ISC_GAM_BENTRY, gamma, GAMMA_ENTRIES);
 640         regmap_bulk_write(regmap, ISC_GAM_GENTRY, gamma, GAMMA_ENTRIES);
 641         regmap_bulk_write(regmap, ISC_GAM_RENTRY, gamma, GAMMA_ENTRIES);
 642
 643         /* Convert RGB to YUV */
 644         regmap_write(regmap, ISC_CSC_YR_YG, 0x42 | (0x81 << 16));
 645         regmap_write(regmap, ISC_CSC_YB_OY, 0x19 | (0x10 << 16));
 646         regmap_write(regmap, ISC_CSC_CBR_CBG, 0xFDA | (0xFB6 << 16));
 647         regmap_write(regmap, ISC_CSC_CBB_OCB, 0x70 | (0x80 << 16));
 648         regmap_write(regmap, ISC_CSC_CRR_CRG, 0x70 | (0xFA2 << 16));
 649         regmap_write(regmap, ISC_CSC_CRB_OCR, 0xFEE | (0x80 << 16));
 650
 651         regmap_write(regmap, ISC_CBC_BRIGHT, ctrls->brightness);
 652         regmap_write(regmap, ISC_CBC_CONTRAST, ctrls->contrast);
 653 }
 654
 655 static int isc_update_profile(struct isc_device *isc)
 656 {
 657         struct regmap *regmap = isc->regmap;
 658         u32 sr;
 659         int counter = 100;
 660
 661         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_UPPRO);
 662
 663         regmap_read(regmap, ISC_CTRLSR, &sr);
 664         while ((sr & ISC_CTRL_UPPRO) && counter--) {
 665                 usleep_range(1000, 2000);
 666                 regmap_read(regmap, ISC_CTRLSR, &sr);
 667         }
 668
 669         if (counter < 0) {
 670                 v4l2_warn(&isc->v4l2_dev, "Time out to update profie\n");
 671                 return -ETIMEDOUT;
 672         }
 673
 674         return 0;
 675 }
 676
 677 static void isc_set_histogram(struct isc_device *isc)
 678 {
 679         struct regmap *regmap = isc->regmap;
 680         struct isc_ctrls *ctrls = &isc->ctrls;
 681
 682         if (ctrls->awb && (ctrls->hist_stat != HIST_ENABLED)) {
 683                 regmap_write(regmap, ISC_HIS_CFG, ISC_HIS_CFG_MODE_R |
 684                       (isc->raw_fmt->reg_bay_cfg << ISC_HIS_CFG_BAYSEL_SHIFT) |
 685                       ISC_HIS_CFG_RAR);
 686                 regmap_write(regmap, ISC_HIS_CTRL, ISC_HIS_CTRL_EN);
 687                 regmap_write(regmap, ISC_INTEN, ISC_INT_HISDONE);
 688                 ctrls->hist_id = ISC_HIS_CFG_MODE_R;
 689                 isc_update_profile(isc);
 690                 regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_HISREQ);
 691
 692                 ctrls->hist_stat = HIST_ENABLED;
 693         } else if (!ctrls->awb && (ctrls->hist_stat != HIST_DISABLED)) {
 694                 regmap_write(regmap, ISC_INTDIS, ISC_INT_HISDONE);
 695                 regmap_write(regmap, ISC_HIS_CTRL, ISC_HIS_CTRL_DIS);
 696
 697                 ctrls->hist_stat = HIST_DISABLED;
 698         }
 699 }
 700
 701 static inline void isc_get_param(const struct isc_format *fmt,
 702                                   u32 *rlp_mode, u32 *dcfg)
 703 {
 704         *dcfg = ISC_DCFG_YMBSIZE_BEATS8;
 705
 706         switch (fmt->fourcc) {
 707         case V4L2_PIX_FMT_SBGGR10:
 708         case V4L2_PIX_FMT_SGBRG10:
 709         case V4L2_PIX_FMT_SGRBG10:
 710         case V4L2_PIX_FMT_SRGGB10:
 711         case V4L2_PIX_FMT_SBGGR12:
 712         case V4L2_PIX_FMT_SGBRG12:
 713         case V4L2_PIX_FMT_SGRBG12:
 714         case V4L2_PIX_FMT_SRGGB12:
 715                 *rlp_mode = fmt->reg_rlp_mode;
 716                 *dcfg |= fmt->reg_dcfg_imode;
 717                 break;
 718         default:
 719                 *rlp_mode = ISC_RLP_CFG_MODE_DAT8;
 720                 *dcfg |= ISC_DCFG_IMODE_PACKED8;
 721                 break;
 722         }
 723 }
 724
 725 static int isc_configure(struct isc_device *isc)
 726 {
 727         struct regmap *regmap = isc->regmap;
 728         const struct isc_format *current_fmt = isc->current_fmt;
 729         struct isc_subdev_entity *subdev = isc->current_subdev;
 730         u32 pfe_cfg0, rlp_mode, dcfg, mask, pipeline;
 731
 732         if (sensor_is_preferred(current_fmt)) {
 733                 pfe_cfg0 = current_fmt->reg_bps;
 734                 pipeline = 0x0;
 735                 isc_get_param(current_fmt, &rlp_mode, &dcfg);
 736                 isc->ctrls.hist_stat = HIST_INIT;
 737         } else {
 738                 pfe_cfg0  = isc->raw_fmt->reg_bps;
 739                 pipeline = current_fmt->pipeline;
 740                 rlp_mode = current_fmt->reg_rlp_mode;
 741                 dcfg = current_fmt->reg_dcfg_imode | ISC_DCFG_YMBSIZE_BEATS8 |
 742                        ISC_DCFG_CMBSIZE_BEATS8;
 743         }
 744
 745         pfe_cfg0  |= subdev->pfe_cfg0 | ISC_PFE_CFG0_MODE_PROGRESSIVE;
 746         mask = ISC_PFE_CFG0_BPS_MASK | ISC_PFE_CFG0_HPOL_LOW |
 747                ISC_PFE_CFG0_VPOL_LOW | ISC_PFE_CFG0_PPOL_LOW |
 748                ISC_PFE_CFG0_MODE_MASK;
 749
 750         regmap_update_bits(regmap, ISC_PFE_CFG0, mask, pfe_cfg0);
 751
 752         regmap_update_bits(regmap, ISC_RLP_CFG, ISC_RLP_CFG_MODE_MASK,
 753                            rlp_mode);
 754
 755         regmap_write(regmap, ISC_DCFG, dcfg);
 756
 757         /* Set the pipeline */
 758         isc_set_pipeline(isc, pipeline);
 759
 760         if (pipeline)
 761                 isc_set_histogram(isc);
 762
 763         /* Update profile */
 764         return isc_update_profile(isc);
 765 }
 766
 767 static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
 768 {
 769         struct isc_device *isc = vb2_get_drv_priv(vq);
 770         struct regmap *regmap = isc->regmap;
 771         struct isc_buffer *buf;
 772         unsigned long flags;
 773         int ret;
 774
 775         /* Enable stream on the sub device */
 776         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 1);
 777         if (ret && ret != -ENOIOCTLCMD) {
 778                 v4l2_err(&isc->v4l2_dev, "stream on failed in subdev\n");
 779                 goto err_start_stream;
 780         }
 781
 782         pm_runtime_get_sync(isc->dev);
 783
 784         ret = isc_configure(isc);
 785         if (unlikely(ret))
 786                 goto err_configure;
 787
 788         /* Enable DMA interrupt */
 789         regmap_write(regmap, ISC_INTEN, ISC_INT_DDONE);
 790
 791         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 792
 793         isc->sequence = 0;
 794         isc->stop = false;
 795         reinit_completion(&isc->comp);
 796
 797         isc->cur_frm = list_first_entry(&isc->dma_queue,
 798                                         struct isc_buffer, list);
 799         list_del(&isc->cur_frm->list);
 800
 801         isc_start_dma(isc);
 802
 803         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 804
 805         return 0;
 806
 807 err_configure:
 808         pm_runtime_put_sync(isc->dev);
 809
 810         v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
 811
 812 err_start_stream:
 813         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 814         list_for_each_entry(buf, &isc->dma_queue, list)
 815                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 816         INIT_LIST_HEAD(&isc->dma_queue);
 817         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 818
 819         return ret;
 820 }
 821
 822 static void isc_stop_streaming(struct vb2_queue *vq)
 823 {
 824         struct isc_device *isc = vb2_get_drv_priv(vq);
 825         unsigned long flags;
 826         struct isc_buffer *buf;
 827         int ret;
 828
 829         isc->stop = true;
 830
 831         /* Wait until the end of the current frame */
 832         if (isc->cur_frm && !wait_for_completion_timeout(&isc->comp, 5 * HZ))
 833                 v4l2_err(&isc->v4l2_dev,
 834                          "Timeout waiting for end of the capture\n");
 835
 836         /* Disable DMA interrupt */
 837         regmap_write(isc->regmap, ISC_INTDIS, ISC_INT_DDONE);
 838
 839         pm_runtime_put_sync(isc->dev);
 840
 841         /* Disable stream on the sub device */
 842         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
 843         if (ret && ret != -ENOIOCTLCMD)
 844                 v4l2_err(&isc->v4l2_dev, "stream off failed in subdev\n");
 845
 846         /* Release all active buffers */
 847         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 848         if (unlikely(isc->cur_frm)) {
 849                 vb2_buffer_done(&isc->cur_frm->vb.vb2_buf,
 850                                 VB2_BUF_STATE_ERROR);
 851                 isc->cur_frm = NULL;
 852         }
 853         list_for_each_entry(buf, &isc->dma_queue, list)
 854                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 855         INIT_LIST_HEAD(&isc->dma_queue);
 856         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 857 }
 858
 859 static void isc_buffer_queue(struct vb2_buffer *vb)
 860 {
 861         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 862         struct isc_buffer *buf = container_of(vbuf, struct isc_buffer, vb);
 863         struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
 864         unsigned long flags;
 865
 866         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 867         if (!isc->cur_frm && list_empty(&isc->dma_queue) &&
 868                 vb2_is_streaming(vb->vb2_queue)) {
 869                 isc->cur_frm = buf;
 870                 isc_start_dma(isc);
 871         } else
 872                 list_add_tail(&buf->list, &isc->dma_queue);
 873         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 874 }
 875
 876 static const struct vb2_ops isc_vb2_ops = {
 877         .queue_setup            = isc_queue_setup,
 878         .wait_prepare           = vb2_ops_wait_prepare,
 879         .wait_finish            = vb2_ops_wait_finish,
 880         .buf_prepare            = isc_buffer_prepare,
 881         .start_streaming        = isc_start_streaming,
 882         .stop_streaming         = isc_stop_streaming,
 883         .buf_queue              = isc_buffer_queue,
 884 };
 885
 886 static int isc_querycap(struct file *file, void *priv,
 887                          struct v4l2_capability *cap)
 888 {
 889         struct isc_device *isc = video_drvdata(file);
 890
 891         strcpy(cap->driver, ATMEL_ISC_NAME);
 892         strcpy(cap->card, "Atmel Image Sensor Controller");
 893         snprintf(cap->bus_info, sizeof(cap->bus_info),
 894                  "platform:%s", isc->v4l2_dev.name);
 895
 896         return 0;
 897 }
 898
 899 static int isc_enum_fmt_vid_cap(struct file *file, void *priv,
 900                                  struct v4l2_fmtdesc *f)
 901 {
 902         struct isc_device *isc = video_drvdata(file);
 903         u32 index = f->index;
 904
 905         if (index >= isc->num_user_formats)
 906                 return -EINVAL;
 907
 908         f->pixelformat = isc->user_formats[index]->fourcc;
 909
 910         return 0;
 911 }
 912
 913 static int isc_g_fmt_vid_cap(struct file *file, void *priv,
 914                               struct v4l2_format *fmt)
 915 {
 916         struct isc_device *isc = video_drvdata(file);
 917
 918         *fmt = isc->fmt;
 919
 920         return 0;
 921 }
 922
 923 static struct isc_format *find_format_by_fourcc(struct isc_device *isc,
 924                                                  unsigned int fourcc)
 925 {
 926         unsigned int num_formats = isc->num_user_formats;
 927         struct isc_format *fmt;
 928         unsigned int i;
 929
 930         for (i = 0; i < num_formats; i++) {
 931                 fmt = isc->user_formats[i];
 932                 if (fmt->fourcc == fourcc)
 933                         return fmt;
 934         }
 935
 936         return NULL;
 937 }
 938
 939 static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
 940                         struct isc_format **current_fmt, u32 *code)
 941 {
 942         struct isc_format *isc_fmt;
 943         struct v4l2_pix_format *pixfmt = &f->fmt.pix;
 944         struct v4l2_subdev_format format = {
 945                 .which = V4L2_SUBDEV_FORMAT_TRY,
 946         };
 947         u32 mbus_code;
 948         int ret;
 949
 950         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 951                 return -EINVAL;
 952
 953         isc_fmt = find_format_by_fourcc(isc, pixfmt->pixelformat);
 954         if (!isc_fmt) {
 955                 v4l2_warn(&isc->v4l2_dev, "Format 0x%x not found\n",
 956                           pixfmt->pixelformat);
 957                 isc_fmt = isc->user_formats[isc->num_user_formats - 1];
 958                 pixfmt->pixelformat = isc_fmt->fourcc;
 959         }
 960
 961         /* Limit to Atmel ISC hardware capabilities */
 962         if (pixfmt->width > ISC_MAX_SUPPORT_WIDTH)
 963                 pixfmt->width = ISC_MAX_SUPPORT_WIDTH;
 964         if (pixfmt->height > ISC_MAX_SUPPORT_HEIGHT)
 965                 pixfmt->height = ISC_MAX_SUPPORT_HEIGHT;
 966
 967         if (sensor_is_preferred(isc_fmt))
 968                 mbus_code = isc_fmt->mbus_code;
 969         else
 970                 mbus_code = isc->raw_fmt->mbus_code;
 971
 972         v4l2_fill_mbus_format(&format.format, pixfmt, mbus_code);
 973         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, set_fmt,
 974                                isc->current_subdev->config, &format);
 975         if (ret < 0)
 976                 return ret;
 977
 978         v4l2_fill_pix_format(pixfmt, &format.format);
 979
 980         pixfmt->field = V4L2_FIELD_NONE;
 981         pixfmt->bytesperline = (pixfmt->width * isc_fmt->bpp) >> 3;
 982         pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
 983
 984         if (current_fmt)
 985                 *current_fmt = isc_fmt;
 986
 987         if (code)
 988                 *code = mbus_code;
 989
 990         return 0;
 991 }
 992
 993 static int isc_set_fmt(struct isc_device *isc, struct v4l2_format *f)
 994 {
 995         struct v4l2_subdev_format format = {
 996                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 997         };
 998         struct isc_format *current_fmt;
 999         u32 mbus_code;
1000         int ret;
1001
1002         ret = isc_try_fmt(isc, f, &current_fmt, &mbus_code);
1003         if (ret)
1004                 return ret;
1005
1006         v4l2_fill_mbus_format(&format.format, &f->fmt.pix, mbus_code);
1007         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
1008                                set_fmt, NULL, &format);
1009         if (ret < 0)
1010                 return ret;
1011
1012         isc->fmt = *f;
1013         isc->current_fmt = current_fmt;
1014
1015         return 0;
1016 }
1017
1018 static int isc_s_fmt_vid_cap(struct file *file, void *priv,
1019                               struct v4l2_format *f)
1020 {
1021         struct isc_device *isc = video_drvdata(file);
1022
1023         if (vb2_is_streaming(&isc->vb2_vidq))
1024                 return -EBUSY;
1025
1026         return isc_set_fmt(isc, f);
1027 }
1028
1029 static int isc_try_fmt_vid_cap(struct file *file, void *priv,
1030                                 struct v4l2_format *f)
1031 {
1032         struct isc_device *isc = video_drvdata(file);
1033
1034         return isc_try_fmt(isc, f, NULL, NULL);
1035 }
1036
1037 static int isc_enum_input(struct file *file, void *priv,
1038                            struct v4l2_input *inp)
1039 {
1040         if (inp->index != 0)
1041                 return -EINVAL;
1042
1043         inp->type = V4L2_INPUT_TYPE_CAMERA;
1044         inp->std = 0;
1045         strcpy(inp->name, "Camera");
1046
1047         return 0;
1048 }
1049
1050 static int isc_g_input(struct file *file, void *priv, unsigned int *i)
1051 {
1052         *i = 0;
1053
1054         return 0;
1055 }
1056
1057 static int isc_s_input(struct file *file, void *priv, unsigned int i)
1058 {
1059         if (i > 0)
1060                 return -EINVAL;
1061
1062         return 0;
1063 }
1064
1065 static int isc_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
1066 {
1067         struct isc_device *isc = video_drvdata(file);
1068
1069         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1070                 return -EINVAL;
1071
1072         return v4l2_subdev_call(isc->current_subdev->sd, video, g_parm, a);
1073 }
1074
1075 static int isc_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
1076 {
1077         struct isc_device *isc = video_drvdata(file);
1078
1079         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1080                 return -EINVAL;
1081
1082         return v4l2_subdev_call(isc->current_subdev->sd, video, s_parm, a);
1083 }
1084
1085 static int isc_enum_framesizes(struct file *file, void *fh,
1086                                struct v4l2_frmsizeenum *fsize)
1087 {
1088         struct isc_device *isc = video_drvdata(file);
1089         const struct isc_format *isc_fmt;
1090         struct v4l2_subdev_frame_size_enum fse = {
1091                 .index = fsize->index,
1092                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1093         };
1094         int ret;
1095
1096         isc_fmt = find_format_by_fourcc(isc, fsize->pixel_format);
1097         if (!isc_fmt)
1098                 return -EINVAL;
1099
1100         if (sensor_is_preferred(isc_fmt))
1101                 fse.code = isc_fmt->mbus_code;
1102         else
1103                 fse.code = isc->raw_fmt->mbus_code;
1104
1105         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, enum_frame_size,
1106                                NULL, &fse);
1107         if (ret)
1108                 return ret;
1109
1110         fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1111         fsize->discrete.width = fse.max_width;
1112         fsize->discrete.height = fse.max_height;
1113
1114         return 0;
1115 }
1116
1117 static int isc_enum_frameintervals(struct file *file, void *fh,
1118                                     struct v4l2_frmivalenum *fival)
1119 {
1120         struct isc_device *isc = video_drvdata(file);
1121         const struct isc_format *isc_fmt;
1122         struct v4l2_subdev_frame_interval_enum fie = {
1123                 .index = fival->index,
1124                 .width = fival->width,
1125                 .height = fival->height,
1126                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1127         };
1128         int ret;
1129
1130         isc_fmt = find_format_by_fourcc(isc, fival->pixel_format);
1131         if (!isc_fmt)
1132                 return -EINVAL;
1133
1134         if (sensor_is_preferred(isc_fmt))
1135                 fie.code = isc_fmt->mbus_code;
1136         else
1137                 fie.code = isc->raw_fmt->mbus_code;
1138
1139         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
1140                                enum_frame_interval, NULL, &fie);
1141         if (ret)
1142                 return ret;
1143
1144         fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1145         fival->discrete = fie.interval;
1146
1147         return 0;
1148 }
1149
1150 static const struct v4l2_ioctl_ops isc_ioctl_ops = {
1151         .vidioc_querycap                = isc_querycap,
1152         .vidioc_enum_fmt_vid_cap        = isc_enum_fmt_vid_cap,
1153         .vidioc_g_fmt_vid_cap           = isc_g_fmt_vid_cap,
1154         .vidioc_s_fmt_vid_cap           = isc_s_fmt_vid_cap,
1155         .vidioc_try_fmt_vid_cap         = isc_try_fmt_vid_cap,
1156
1157         .vidioc_enum_input              = isc_enum_input,
1158         .vidioc_g_input                 = isc_g_input,
1159         .vidioc_s_input                 = isc_s_input,
1160
1161         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1162         .vidioc_querybuf                = vb2_ioctl_querybuf,
1163         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1164         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1165         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1166         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1167         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1168         .vidioc_streamon                = vb2_ioctl_streamon,
1169         .vidioc_streamoff               = vb2_ioctl_streamoff,
1170
1171         .vidioc_g_parm                  = isc_g_parm,
1172         .vidioc_s_parm                  = isc_s_parm,
1173         .vidioc_enum_framesizes         = isc_enum_framesizes,
1174         .vidioc_enum_frameintervals     = isc_enum_frameintervals,
1175
1176         .vidioc_log_status              = v4l2_ctrl_log_status,
1177         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
1178         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1179 };
1180
1181 static int isc_open(struct file *file)
1182 {
1183         struct isc_device *isc = video_drvdata(file);
1184         struct v4l2_subdev *sd = isc->current_subdev->sd;
1185         int ret;
1186
1187         if (mutex_lock_interruptible(&isc->lock))
1188                 return -ERESTARTSYS;
1189
1190         ret = v4l2_fh_open(file);
1191         if (ret < 0)
1192                 goto unlock;
1193
1194         if (!v4l2_fh_is_singular_file(file))
1195                 goto unlock;
1196
1197         ret = v4l2_subdev_call(sd, core, s_power, 1);
1198         if (ret < 0 && ret != -ENOIOCTLCMD) {
1199                 v4l2_fh_release(file);
1200                 goto unlock;
1201         }
1202
1203         ret = isc_set_fmt(isc, &isc->fmt);
1204         if (ret) {
1205                 v4l2_subdev_call(sd, core, s_power, 0);
1206                 v4l2_fh_release(file);
1207         }
1208
1209 unlock:
1210         mutex_unlock(&isc->lock);
1211         return ret;
1212 }
1213
1214 static int isc_release(struct file *file)
1215 {
1216         struct isc_device *isc = video_drvdata(file);
1217         struct v4l2_subdev *sd = isc->current_subdev->sd;
1218         bool fh_singular;
1219         int ret;
1220
1221         mutex_lock(&isc->lock);
1222
1223         fh_singular = v4l2_fh_is_singular_file(file);
1224
1225         ret = _vb2_fop_release(file, NULL);
1226
1227         if (fh_singular)
1228                 v4l2_subdev_call(sd, core, s_power, 0);
1229
1230         mutex_unlock(&isc->lock);
1231
1232         return ret;
1233 }
1234
1235 static const struct v4l2_file_operations isc_fops = {
1236         .owner          = THIS_MODULE,
1237         .open           = isc_open,
1238         .release        = isc_release,
1239         .unlocked_ioctl = video_ioctl2,
1240         .read           = vb2_fop_read,
1241         .mmap           = vb2_fop_mmap,
1242         .poll           = vb2_fop_poll,
1243 };
1244
1245 static irqreturn_t isc_interrupt(int irq, void *dev_id)
1246 {
1247         struct isc_device *isc = (struct isc_device *)dev_id;
1248         struct regmap *regmap = isc->regmap;
1249         u32 isc_intsr, isc_intmask, pending;
1250         irqreturn_t ret = IRQ_NONE;
1251
1252         regmap_read(regmap, ISC_INTSR, &isc_intsr);
1253         regmap_read(regmap, ISC_INTMASK, &isc_intmask);
1254
1255         pending = isc_intsr & isc_intmask;
1256
1257         if (likely(pending & ISC_INT_DDONE)) {
1258                 spin_lock(&isc->dma_queue_lock);
1259                 if (isc->cur_frm) {
1260                         struct vb2_v4l2_buffer *vbuf = &isc->cur_frm->vb;
1261                         struct vb2_buffer *vb = &vbuf->vb2_buf;
1262
1263                         vb->timestamp = ktime_get_ns();
1264                         vbuf->sequence = isc->sequence++;
1265                         vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
1266                         isc->cur_frm = NULL;
1267                 }
1268
1269                 if (!list_empty(&isc->dma_queue) && !isc->stop) {
1270                         isc->cur_frm = list_first_entry(&isc->dma_queue,
1271                                                      struct isc_buffer, list);
1272                         list_del(&isc->cur_frm->list);
1273
1274                         isc_start_dma(isc);
1275                 }
1276
1277                 if (isc->stop)
1278                         complete(&isc->comp);
1279
1280                 ret = IRQ_HANDLED;
1281                 spin_unlock(&isc->dma_queue_lock);
1282         }
1283
1284         if (pending & ISC_INT_HISDONE) {
1285                 schedule_work(&isc->awb_work);
1286                 ret = IRQ_HANDLED;
1287         }
1288
1289         return ret;
1290 }
1291
1292 static void isc_hist_count(struct isc_device *isc)
1293 {
1294         struct regmap *regmap = isc->regmap;
1295         struct isc_ctrls *ctrls = &isc->ctrls;
1296         u32 *hist_count = &ctrls->hist_count[ctrls->hist_id];
1297         u32 *hist_entry = &ctrls->hist_entry[0];
1298         u32 i;
1299
1300         regmap_bulk_read(regmap, ISC_HIS_ENTRY, hist_entry, HIST_ENTRIES);
1301
1302         *hist_count = 0;
1303         for (i = 0; i < HIST_ENTRIES; i++)
1304                 *hist_count += i * (*hist_entry++);
1305 }
1306
1307 static void isc_wb_update(struct isc_ctrls *ctrls)
1308 {
1309         u32 *hist_count = &ctrls->hist_count[0];
1310         u64 g_count = (u64)hist_count[ISC_HIS_CFG_MODE_GB] << 9;
1311         u32 hist_r = hist_count[ISC_HIS_CFG_MODE_R];
1312         u32 hist_b = hist_count[ISC_HIS_CFG_MODE_B];
1313
1314         if (hist_r)
1315                 ctrls->r_gain = div_u64(g_count, hist_r);
1316
1317         if (hist_b)
1318                 ctrls->b_gain = div_u64(g_count, hist_b);
1319 }
1320
1321 static void isc_awb_work(struct work_struct *w)
1322 {
1323         struct isc_device *isc =
1324                 container_of(w, struct isc_device, awb_work);
1325         struct regmap *regmap = isc->regmap;
1326         struct isc_ctrls *ctrls = &isc->ctrls;
1327         u32 hist_id = ctrls->hist_id;
1328         u32 baysel;
1329
1330         if (ctrls->hist_stat != HIST_ENABLED)
1331                 return;
1332
1333         isc_hist_count(isc);
1334
1335         if (hist_id != ISC_HIS_CFG_MODE_B) {
1336                 hist_id++;
1337         } else {
1338                 isc_wb_update(ctrls);
1339                 hist_id = ISC_HIS_CFG_MODE_R;
1340         }
1341
1342         ctrls->hist_id = hist_id;
1343         baysel = isc->raw_fmt->reg_bay_cfg << ISC_HIS_CFG_BAYSEL_SHIFT;
1344
1345         pm_runtime_get_sync(isc->dev);
1346
1347         regmap_write(regmap, ISC_HIS_CFG, hist_id | baysel | ISC_HIS_CFG_RAR);
1348         isc_update_profile(isc);
1349         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_HISREQ);
1350
1351         pm_runtime_put_sync(isc->dev);
1352 }
1353
1354 static int isc_s_ctrl(struct v4l2_ctrl *ctrl)
1355 {
1356         struct isc_device *isc = container_of(ctrl->handler,
1357                                              struct isc_device, ctrls.handler);
1358         struct isc_ctrls *ctrls = &isc->ctrls;
1359
1360         switch (ctrl->id) {
1361         case V4L2_CID_BRIGHTNESS:
1362                 ctrls->brightness = ctrl->val & ISC_CBC_BRIGHT_MASK;
1363                 break;
1364         case V4L2_CID_CONTRAST:
1365                 ctrls->contrast = ctrl->val & ISC_CBC_CONTRAST_MASK;
1366                 break;
1367         case V4L2_CID_GAMMA:
1368                 ctrls->gamma_index = ctrl->val;
1369                 break;
1370         case V4L2_CID_AUTO_WHITE_BALANCE:
1371                 ctrls->awb = ctrl->val;
1372                 if (ctrls->hist_stat != HIST_ENABLED) {
1373                         ctrls->r_gain = 0x1 << 9;
1374                         ctrls->b_gain = 0x1 << 9;
1375                 }
1376                 break;
1377         default:
1378                 return -EINVAL;
1379         }
1380
1381         return 0;
1382 }
1383
1384 static const struct v4l2_ctrl_ops isc_ctrl_ops = {
1385         .s_ctrl = isc_s_ctrl,
1386 };
1387
1388 static int isc_ctrl_init(struct isc_device *isc)
1389 {
1390         const struct v4l2_ctrl_ops *ops = &isc_ctrl_ops;
1391         struct isc_ctrls *ctrls = &isc->ctrls;
1392         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1393         int ret;
1394
1395         ctrls->hist_stat = HIST_INIT;
1396
1397         ret = v4l2_ctrl_handler_init(hdl, 4);
1398         if (ret < 0)
1399                 return ret;
1400
1401         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS, -1024, 1023, 1, 0);
1402         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST, -2048, 2047, 1, 256);
1403         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAMMA, 0, GAMMA_MAX, 1, 2);
1404         v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
1405
1406         v4l2_ctrl_handler_setup(hdl);
1407
1408         return 0;
1409 }
1410
1411
1412 static int isc_async_bound(struct v4l2_async_notifier *notifier,
1413                             struct v4l2_subdev *subdev,
1414                             struct v4l2_async_subdev *asd)
1415 {
1416         struct isc_device *isc = container_of(notifier->v4l2_dev,
1417                                               struct isc_device, v4l2_dev);
1418         struct isc_subdev_entity *subdev_entity =
1419                 container_of(notifier, struct isc_subdev_entity, notifier);
1420
1421         if (video_is_registered(&isc->video_dev)) {
1422                 v4l2_err(&isc->v4l2_dev, "only supports one sub-device.\n");
1423                 return -EBUSY;
1424         }
1425
1426         subdev_entity->sd = subdev;
1427
1428         return 0;
1429 }
1430
1431 static void isc_async_unbind(struct v4l2_async_notifier *notifier,
1432                               struct v4l2_subdev *subdev,
1433                               struct v4l2_async_subdev *asd)
1434 {
1435         struct isc_device *isc = container_of(notifier->v4l2_dev,
1436                                               struct isc_device, v4l2_dev);
1437         cancel_work_sync(&isc->awb_work);
1438         video_unregister_device(&isc->video_dev);
1439         if (isc->current_subdev->config)
1440                 v4l2_subdev_free_pad_config(isc->current_subdev->config);
1441         v4l2_ctrl_handler_free(&isc->ctrls.handler);
1442 }
1443
1444 static struct isc_format *find_format_by_code(unsigned int code, int *index)
1445 {
1446         struct isc_format *fmt = &isc_formats[0];
1447         unsigned int i;
1448
1449         for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
1450                 if (fmt->mbus_code == code) {
1451                         *index = i;
1452                         return fmt;
1453                 }
1454
1455                 fmt++;
1456         }
1457
1458         return NULL;
1459 }
1460
1461 static int isc_formats_init(struct isc_device *isc)
1462 {
1463         struct isc_format *fmt;
1464         struct v4l2_subdev *subdev = isc->current_subdev->sd;
1465         unsigned int num_fmts, i, j;
1466         struct v4l2_subdev_mbus_code_enum mbus_code = {
1467                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1468         };
1469
1470         fmt = &isc_formats[0];
1471         for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
1472                 fmt->isc_support = false;
1473                 fmt->sd_support = false;
1474
1475                 fmt++;
1476         }
1477
1478         while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
1479                NULL, &mbus_code)) {
1480                 mbus_code.index++;
1481                 fmt = find_format_by_code(mbus_code.code, &i);
1482                 if (!fmt)
1483                         continue;
1484
1485                 fmt->sd_support = true;
1486
1487                 if (i <= RAW_FMT_IND_END) {
1488                         for (j = ISC_FMT_IND_START; j <= ISC_FMT_IND_END; j++)
1489                                 isc_formats[j].isc_support = true;
1490
1491                         isc->raw_fmt = fmt;
1492                 }
1493         }
1494
1495         fmt = &isc_formats[0];
1496         for (i = 0, num_fmts = 0; i < ARRAY_SIZE(isc_formats); i++) {
1497                 if (fmt->isc_support || fmt->sd_support)
1498                         num_fmts++;
1499
1500                 fmt++;
1501         }
1502
1503         if (!num_fmts)
1504                 return -ENXIO;
1505
1506         isc->num_user_formats = num_fmts;
1507         isc->user_formats = devm_kcalloc(isc->dev,
1508                                          num_fmts, sizeof(struct isc_format *),
1509                                          GFP_KERNEL);
1510         if (!isc->user_formats) {
1511                 v4l2_err(&isc->v4l2_dev, "could not allocate memory\n");
1512                 return -ENOMEM;
1513         }
1514
1515         fmt = &isc_formats[0];
1516         for (i = 0, j = 0; i < ARRAY_SIZE(isc_formats); i++) {
1517                 if (fmt->isc_support || fmt->sd_support)
1518                         isc->user_formats[j++] = fmt;
1519
1520                 fmt++;
1521         }
1522
1523         return 0;
1524 }
1525
1526 static int isc_set_default_fmt(struct isc_device *isc)
1527 {
1528         struct v4l2_format f = {
1529                 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
1530                 .fmt.pix = {
1531                         .width          = VGA_WIDTH,
1532                         .height         = VGA_HEIGHT,
1533                         .field          = V4L2_FIELD_NONE,
1534                         .pixelformat    = isc->user_formats[0]->fourcc,
1535                 },
1536         };
1537         int ret;
1538
1539         ret = isc_try_fmt(isc, &f, NULL, NULL);
1540         if (ret)
1541                 return ret;
1542
1543         isc->current_fmt = isc->user_formats[0];
1544         isc->fmt = f;
1545
1546         return 0;
1547 }
1548
1549 static int isc_async_complete(struct v4l2_async_notifier *notifier)
1550 {
1551         struct isc_device *isc = container_of(notifier->v4l2_dev,
1552                                               struct isc_device, v4l2_dev);
1553         struct isc_subdev_entity *sd_entity;
1554         struct video_device *vdev = &isc->video_dev;
1555         struct vb2_queue *q = &isc->vb2_vidq;
1556         int ret;
1557
1558         INIT_WORK(&isc->awb_work, isc_awb_work);
1559
1560         ret = v4l2_device_register_subdev_nodes(&isc->v4l2_dev);
1561         if (ret < 0) {
1562                 v4l2_err(&isc->v4l2_dev, "Failed to register subdev nodes\n");
1563                 return ret;
1564         }
1565
1566         isc->current_subdev = container_of(notifier,
1567                                            struct isc_subdev_entity, notifier);
1568         sd_entity = isc->current_subdev;
1569
1570         mutex_init(&isc->lock);
1571         init_completion(&isc->comp);
1572
1573         /* Initialize videobuf2 queue */
1574         q->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1575         q->io_modes             = VB2_MMAP | VB2_DMABUF | VB2_READ;
1576         q->drv_priv             = isc;
1577         q->buf_struct_size      = sizeof(struct isc_buffer);
1578         q->ops                  = &isc_vb2_ops;
1579         q->mem_ops              = &vb2_dma_contig_memops;
1580         q->timestamp_flags      = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1581         q->lock                 = &isc->lock;
1582         q->min_buffers_needed   = 1;
1583         q->dev                  = isc->dev;
1584
1585         ret = vb2_queue_init(q);
1586         if (ret < 0) {
1587                 v4l2_err(&isc->v4l2_dev,
1588                          "vb2_queue_init() failed: %d\n", ret);
1589                 return ret;
1590         }
1591
1592         /* Init video dma queues */
1593         INIT_LIST_HEAD(&isc->dma_queue);
1594         spin_lock_init(&isc->dma_queue_lock);
1595
1596         sd_entity->config = v4l2_subdev_alloc_pad_config(sd_entity->sd);
1597         if (sd_entity->config == NULL)
1598                 return -ENOMEM;
1599
1600         ret = isc_formats_init(isc);
1601         if (ret < 0) {
1602                 v4l2_err(&isc->v4l2_dev,
1603                          "Init format failed: %d\n", ret);
1604                 return ret;
1605         }
1606
1607         ret = isc_set_default_fmt(isc);
1608         if (ret) {
1609                 v4l2_err(&isc->v4l2_dev, "Could not set default format\n");
1610                 return ret;
1611         }
1612
1613         ret = isc_ctrl_init(isc);
1614         if (ret) {
1615                 v4l2_err(&isc->v4l2_dev, "Init isc ctrols failed: %d\n", ret);
1616                 return ret;
1617         }
1618
1619         /* Register video device */
1620         strlcpy(vdev->name, ATMEL_ISC_NAME, sizeof(vdev->name));
1621         vdev->release           = video_device_release_empty;
1622         vdev->fops              = &isc_fops;
1623         vdev->ioctl_ops         = &isc_ioctl_ops;
1624         vdev->v4l2_dev          = &isc->v4l2_dev;
1625         vdev->vfl_dir           = VFL_DIR_RX;
1626         vdev->queue             = q;
1627         vdev->lock              = &isc->lock;
1628         vdev->ctrl_handler      = &isc->ctrls.handler;
1629         vdev->device_caps       = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
1630         video_set_drvdata(vdev, isc);
1631
1632         ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1633         if (ret < 0) {
1634                 v4l2_err(&isc->v4l2_dev,
1635                          "video_register_device failed: %d\n", ret);
1636                 return ret;
1637         }
1638
1639         return 0;
1640 }
1641
1642 static void isc_subdev_cleanup(struct isc_device *isc)
1643 {
1644         struct isc_subdev_entity *subdev_entity;
1645
1646         list_for_each_entry(subdev_entity, &isc->subdev_entities, list)
1647                 v4l2_async_notifier_unregister(&subdev_entity->notifier);
1648
1649         INIT_LIST_HEAD(&isc->subdev_entities);
1650 }
1651
1652 static int isc_pipeline_init(struct isc_device *isc)
1653 {
1654         struct device *dev = isc->dev;
1655         struct regmap *regmap = isc->regmap;
1656         struct regmap_field *regs;
1657         unsigned int i;
1658
1659         /* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
1660         const struct reg_field regfields[ISC_PIPE_LINE_NODE_NUM] = {
1661                 REG_FIELD(ISC_WB_CTRL, 0, 0),
1662                 REG_FIELD(ISC_CFA_CTRL, 0, 0),
1663                 REG_FIELD(ISC_CC_CTRL, 0, 0),
1664                 REG_FIELD(ISC_GAM_CTRL, 0, 0),
1665                 REG_FIELD(ISC_GAM_CTRL, 1, 1),
1666                 REG_FIELD(ISC_GAM_CTRL, 2, 2),
1667                 REG_FIELD(ISC_GAM_CTRL, 3, 3),
1668                 REG_FIELD(ISC_CSC_CTRL, 0, 0),
1669                 REG_FIELD(ISC_CBC_CTRL, 0, 0),
1670                 REG_FIELD(ISC_SUB422_CTRL, 0, 0),
1671                 REG_FIELD(ISC_SUB420_CTRL, 0, 0),
1672         };
1673
1674         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
1675                 regs = devm_regmap_field_alloc(dev, regmap, regfields[i]);
1676                 if (IS_ERR(regs))
1677                         return PTR_ERR(regs);
1678
1679                 isc->pipeline[i] =  regs;
1680         }
1681
1682         return 0;
1683 }
1684
1685 static int isc_parse_dt(struct device *dev, struct isc_device *isc)
1686 {
1687         struct device_node *np = dev->of_node;
1688         struct device_node *epn = NULL, *rem;
1689         struct v4l2_fwnode_endpoint v4l2_epn;
1690         struct isc_subdev_entity *subdev_entity;
1691         unsigned int flags;
1692         int ret;
1693
1694         INIT_LIST_HEAD(&isc->subdev_entities);
1695
1696         for (; ;) {
1697                 epn = of_graph_get_next_endpoint(np, epn);
1698                 if (!epn)
1699                         break;
1700
1701                 rem = of_graph_get_remote_port_parent(epn);
1702                 if (!rem) {
1703                         dev_notice(dev, "Remote device at %pOF not found\n",
1704                                    epn);
1705                         continue;
1706                 }
1707
1708                 ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(epn),
1709                                                  &v4l2_epn);
1710                 if (ret) {
1711                         of_node_put(rem);
1712                         ret = -EINVAL;
1713                         dev_err(dev, "Could not parse the endpoint\n");
1714                         break;
1715                 }
1716
1717                 subdev_entity = devm_kzalloc(dev,
1718                                           sizeof(*subdev_entity), GFP_KERNEL);
1719                 if (subdev_entity == NULL) {
1720                         of_node_put(rem);
1721                         ret = -ENOMEM;
1722                         break;
1723                 }
1724
1725                 /* asd will be freed by the subsystem once it's added to the
1726                  * notifier list
1727                  */
1728                 subdev_entity->asd = kzalloc(sizeof(*subdev_entity->asd),
1729                                              GFP_KERNEL);
1730                 if (subdev_entity->asd == NULL) {
1731                         of_node_put(rem);
1732                         ret = -ENOMEM;
1733                         break;
1734                 }
1735
1736                 flags = v4l2_epn.bus.parallel.flags;
1737
1738                 if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
1739                         subdev_entity->pfe_cfg0 = ISC_PFE_CFG0_HPOL_LOW;
1740
1741                 if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
1742                         subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_VPOL_LOW;
1743
1744                 if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
1745                         subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_PPOL_LOW;
1746
1747                 subdev_entity->asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
1748                 subdev_entity->asd->match.fwnode.fwnode =
1749                         of_fwnode_handle(rem);
1750                 list_add_tail(&subdev_entity->list, &isc->subdev_entities);
1751         }
1752
1753         of_node_put(epn);
1754         return ret;
1755 }
1756
1757 /* regmap configuration */
1758 #define ATMEL_ISC_REG_MAX    0xbfc
1759 static const struct regmap_config isc_regmap_config = {
1760         .reg_bits       = 32,
1761         .reg_stride     = 4,
1762         .val_bits       = 32,
1763         .max_register   = ATMEL_ISC_REG_MAX,
1764 };
1765
1766 static int atmel_isc_probe(struct platform_device *pdev)
1767 {
1768         struct device *dev = &pdev->dev;
1769         struct isc_device *isc;
1770         struct resource *res;
1771         void __iomem *io_base;
1772         struct isc_subdev_entity *subdev_entity;
1773         int irq;
1774         int ret;
1775
1776         isc = devm_kzalloc(dev, sizeof(*isc), GFP_KERNEL);
1777         if (!isc)
1778                 return -ENOMEM;
1779
1780         platform_set_drvdata(pdev, isc);
1781         isc->dev = dev;
1782
1783         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1784         io_base = devm_ioremap_resource(dev, res);
1785         if (IS_ERR(io_base))
1786                 return PTR_ERR(io_base);
1787
1788         isc->regmap = devm_regmap_init_mmio(dev, io_base, &isc_regmap_config);
1789         if (IS_ERR(isc->regmap)) {
1790                 ret = PTR_ERR(isc->regmap);
1791                 dev_err(dev, "failed to init register map: %d\n", ret);
1792                 return ret;
1793         }
1794
1795         irq = platform_get_irq(pdev, 0);
1796         if (irq < 0) {
1797                 ret = irq;
1798                 dev_err(dev, "failed to get irq: %d\n", ret);
1799                 return ret;
1800         }
1801
1802         ret = devm_request_irq(dev, irq, isc_interrupt, 0,
1803                                ATMEL_ISC_NAME, isc);
1804         if (ret < 0) {
1805                 dev_err(dev, "can't register ISR for IRQ %u (ret=%i)\n",
1806                         irq, ret);
1807                 return ret;
1808         }
1809
1810         ret = isc_pipeline_init(isc);
1811         if (ret)
1812                 return ret;
1813
1814         isc->hclock = devm_clk_get(dev, "hclock");
1815         if (IS_ERR(isc->hclock)) {
1816                 ret = PTR_ERR(isc->hclock);
1817                 dev_err(dev, "failed to get hclock: %d\n", ret);
1818                 return ret;
1819         }
1820
1821         ret = isc_clk_init(isc);
1822         if (ret) {
1823                 dev_err(dev, "failed to init isc clock: %d\n", ret);
1824                 goto clean_isc_clk;
1825         }
1826
1827         isc->ispck = isc->isc_clks[ISC_ISPCK].clk;
1828
1829         /* ispck should be greater or equal to hclock */
1830         ret = clk_set_rate(isc->ispck, clk_get_rate(isc->hclock));
1831         if (ret) {
1832                 dev_err(dev, "failed to set ispck rate: %d\n", ret);
1833                 goto clean_isc_clk;
1834         }
1835
1836         ret = v4l2_device_register(dev, &isc->v4l2_dev);
1837         if (ret) {
1838                 dev_err(dev, "unable to register v4l2 device.\n");
1839                 goto clean_isc_clk;
1840         }
1841
1842         ret = isc_parse_dt(dev, isc);
1843         if (ret) {
1844                 dev_err(dev, "fail to parse device tree\n");
1845                 goto unregister_v4l2_device;
1846         }
1847
1848         if (list_empty(&isc->subdev_entities)) {
1849                 dev_err(dev, "no subdev found\n");
1850                 ret = -ENODEV;
1851                 goto unregister_v4l2_device;
1852         }
1853
1854         list_for_each_entry(subdev_entity, &isc->subdev_entities, list) {
1855                 subdev_entity->notifier.subdevs = &subdev_entity->asd;
1856                 subdev_entity->notifier.num_subdevs = 1;
1857                 subdev_entity->notifier.bound = isc_async_bound;
1858                 subdev_entity->notifier.unbind = isc_async_unbind;
1859                 subdev_entity->notifier.complete = isc_async_complete;
1860
1861                 ret = v4l2_async_notifier_register(&isc->v4l2_dev,
1862                                                    &subdev_entity->notifier);
1863                 if (ret) {
1864                         dev_err(dev, "fail to register async notifier\n");
1865                         kfree(subdev_entity->asd);
1866                         goto cleanup_subdev;
1867                 }
1868
1869                 if (video_is_registered(&isc->video_dev))
1870                         break;
1871         }
1872
1873         pm_runtime_enable(dev);
1874
1875         return 0;
1876
1877 cleanup_subdev:
1878         isc_subdev_cleanup(isc);
1879
1880 unregister_v4l2_device:
1881         v4l2_device_unregister(&isc->v4l2_dev);
1882
1883 clean_isc_clk:
1884         isc_clk_cleanup(isc);
1885
1886         return ret;
1887 }
1888
1889 static int atmel_isc_remove(struct platform_device *pdev)
1890 {
1891         struct isc_device *isc = platform_get_drvdata(pdev);
1892
1893         pm_runtime_disable(&pdev->dev);
1894
1895         isc_subdev_cleanup(isc);
1896
1897         v4l2_device_unregister(&isc->v4l2_dev);
1898
1899         isc_clk_cleanup(isc);
1900
1901         return 0;
1902 }
1903
1904 static int __maybe_unused isc_runtime_suspend(struct device *dev)
1905 {
1906         struct isc_device *isc = dev_get_drvdata(dev);
1907
1908         clk_disable_unprepare(isc->ispck);
1909         clk_disable_unprepare(isc->hclock);
1910
1911         return 0;
1912 }
1913
1914 static int __maybe_unused isc_runtime_resume(struct device *dev)
1915 {
1916         struct isc_device *isc = dev_get_drvdata(dev);
1917         int ret;
1918
1919         ret = clk_prepare_enable(isc->hclock);
1920         if (ret)
1921                 return ret;
1922
1923         return clk_prepare_enable(isc->ispck);
1924 }
1925
1926 static const struct dev_pm_ops atmel_isc_dev_pm_ops = {
1927         SET_RUNTIME_PM_OPS(isc_runtime_suspend, isc_runtime_resume, NULL)
1928 };
1929
1930 static const struct of_device_id atmel_isc_of_match[] = {
1931         { .compatible = "atmel,sama5d2-isc" },
1932         { }
1933 };
1934 MODULE_DEVICE_TABLE(of, atmel_isc_of_match);
1935
1936 static struct platform_driver atmel_isc_driver = {
1937         .probe  = atmel_isc_probe,
1938         .remove = atmel_isc_remove,
1939         .driver = {
1940                 .name           = ATMEL_ISC_NAME,
1941                 .pm             = &atmel_isc_dev_pm_ops,
1942                 .of_match_table = of_match_ptr(atmel_isc_of_match),
1943         },
1944 };
1945
1946 module_platform_driver(atmel_isc_driver);
1947
1948 MODULE_AUTHOR("Songjun Wu <songjun.wu@microchip.com>");
1949 MODULE_DESCRIPTION("The V4L2 driver for Atmel-ISC");
1950 MODULE_LICENSE("GPL v2");
1951 MODULE_SUPPORTED_DEVICE("video");