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/module.h>
  33 #include <linux/of.h>
  34 #include <linux/platform_device.h>
  35 #include <linux/pm_runtime.h>
  36 #include <linux/regmap.h>
  37 #include <linux/videodev2.h>
  38
  39 #include <media/v4l2-device.h>
  40 #include <media/v4l2-image-sizes.h>
  41 #include <media/v4l2-ioctl.h>
  42 #include <media/v4l2-of.h>
  43 #include <media/v4l2-subdev.h>
  44 #include <media/videobuf2-dma-contig.h>
  45
  46 #include "atmel-isc-regs.h"
  47
  48 #define ATMEL_ISC_NAME          "atmel_isc"
  49
  50 #define ISC_MAX_SUPPORT_WIDTH   2592
  51 #define ISC_MAX_SUPPORT_HEIGHT  1944
  52
  53 #define ISC_CLK_MAX_DIV         255
  54
  55 enum isc_clk_id {
  56         ISC_ISPCK = 0,
  57         ISC_MCK = 1,
  58 };
  59
  60 struct isc_clk {
  61         struct clk_hw   hw;
  62         struct clk      *clk;
  63         struct regmap   *regmap;
  64         u8              id;
  65         u8              parent_id;
  66         u32             div;
  67         struct device   *dev;
  68 };
  69
  70 #define to_isc_clk(hw) container_of(hw, struct isc_clk, hw)
  71
  72 struct isc_buffer {
  73         struct vb2_v4l2_buffer  vb;
  74         struct list_head        list;
  75 };
  76
  77 struct isc_subdev_entity {
  78         struct v4l2_subdev              *sd;
  79         struct v4l2_async_subdev        *asd;
  80         struct v4l2_async_notifier      notifier;
  81         struct v4l2_subdev_pad_config   *config;
  82
  83         u32 pfe_cfg0;
  84
  85         struct list_head list;
  86 };
  87
  88 /*
  89  * struct isc_format - ISC media bus format information
  90  * @fourcc:             Fourcc code for this format
  91  * @mbus_code:          V4L2 media bus format code.
  92  * @bpp:                Bytes per pixel (when stored in memory)
  93  * @reg_bps:            reg value for bits per sample
  94  *                      (when transferred over a bus)
  95  * @support:            Indicates format supported by subdev
  96  */
  97 struct isc_format {
  98         u32     fourcc;
  99         u32     mbus_code;
 100         u8      bpp;
 101
 102         u32     reg_bps;
 103         u32     reg_rlp_mode;
 104         u32     reg_dcfg_imode;
 105         u32     reg_dctrl_dview;
 106
 107         bool    support;
 108 };
 109
 110 #define ISC_PIPE_LINE_NODE_NUM  11
 111
 112 struct isc_device {
 113         struct regmap           *regmap;
 114         struct clk              *hclock;
 115         struct clk              *ispck;
 116         struct isc_clk          isc_clks[2];
 117
 118         struct device           *dev;
 119         struct v4l2_device      v4l2_dev;
 120         struct video_device     video_dev;
 121
 122         struct vb2_queue        vb2_vidq;
 123         spinlock_t              dma_queue_lock;
 124         struct list_head        dma_queue;
 125         struct isc_buffer       *cur_frm;
 126         unsigned int            sequence;
 127         bool                    stop;
 128         struct completion       comp;
 129
 130         struct v4l2_format      fmt;
 131         struct isc_format       **user_formats;
 132         unsigned int            num_user_formats;
 133         const struct isc_format *current_fmt;
 134
 135         struct mutex            lock;
 136
 137         struct regmap_field     *pipeline[ISC_PIPE_LINE_NODE_NUM];
 138
 139         struct isc_subdev_entity        *current_subdev;
 140         struct list_head                subdev_entities;
 141 };
 142
 143 static struct isc_format isc_formats[] = {
 144         { V4L2_PIX_FMT_SBGGR8, MEDIA_BUS_FMT_SBGGR8_1X8,
 145           1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
 146           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
 147         { V4L2_PIX_FMT_SGBRG8, MEDIA_BUS_FMT_SGBRG8_1X8,
 148           1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
 149           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
 150         { V4L2_PIX_FMT_SGRBG8, MEDIA_BUS_FMT_SGRBG8_1X8,
 151           1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
 152           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
 153         { V4L2_PIX_FMT_SRGGB8, MEDIA_BUS_FMT_SRGGB8_1X8,
 154           1, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
 155           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
 156
 157         { V4L2_PIX_FMT_SBGGR10, MEDIA_BUS_FMT_SBGGR10_1X10,
 158           2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
 159           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 160         { V4L2_PIX_FMT_SGBRG10, MEDIA_BUS_FMT_SGBRG10_1X10,
 161           2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
 162           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 163         { V4L2_PIX_FMT_SGRBG10, MEDIA_BUS_FMT_SGRBG10_1X10,
 164           2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
 165           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 166         { V4L2_PIX_FMT_SRGGB10, MEDIA_BUS_FMT_SRGGB10_1X10,
 167           2, ISC_PFG_CFG0_BPS_TEN, ISC_RLP_CFG_MODE_DAT10,
 168           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 169
 170         { V4L2_PIX_FMT_SBGGR12, MEDIA_BUS_FMT_SBGGR12_1X12,
 171           2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
 172           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 173         { V4L2_PIX_FMT_SGBRG12, MEDIA_BUS_FMT_SGBRG12_1X12,
 174           2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
 175           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 176         { V4L2_PIX_FMT_SGRBG12, MEDIA_BUS_FMT_SGRBG12_1X12,
 177           2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
 178           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 179         { V4L2_PIX_FMT_SRGGB12, MEDIA_BUS_FMT_SRGGB12_1X12,
 180           2, ISC_PFG_CFG0_BPS_TWELVE, ISC_RLP_CFG_MODE_DAT12,
 181           ISC_DCFG_IMODE_PACKED16, ISC_DCTRL_DVIEW_PACKED, false },
 182
 183         { V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_YUYV8_2X8,
 184           2, ISC_PFE_CFG0_BPS_EIGHT, ISC_RLP_CFG_MODE_DAT8,
 185           ISC_DCFG_IMODE_PACKED8, ISC_DCTRL_DVIEW_PACKED, false },
 186 };
 187
 188 static int isc_clk_enable(struct clk_hw *hw)
 189 {
 190         struct isc_clk *isc_clk = to_isc_clk(hw);
 191         u32 id = isc_clk->id;
 192         struct regmap *regmap = isc_clk->regmap;
 193
 194         dev_dbg(isc_clk->dev, "ISC CLK: %s, div = %d, parent id = %d\n",
 195                 __func__, isc_clk->div, isc_clk->parent_id);
 196
 197         regmap_update_bits(regmap, ISC_CLKCFG,
 198                            ISC_CLKCFG_DIV_MASK(id) | ISC_CLKCFG_SEL_MASK(id),
 199                            (isc_clk->div << ISC_CLKCFG_DIV_SHIFT(id)) |
 200                            (isc_clk->parent_id << ISC_CLKCFG_SEL_SHIFT(id)));
 201
 202         regmap_write(regmap, ISC_CLKEN, ISC_CLK(id));
 203
 204         return 0;
 205 }
 206
 207 static void isc_clk_disable(struct clk_hw *hw)
 208 {
 209         struct isc_clk *isc_clk = to_isc_clk(hw);
 210         u32 id = isc_clk->id;
 211
 212         regmap_write(isc_clk->regmap, ISC_CLKDIS, ISC_CLK(id));
 213 }
 214
 215 static int isc_clk_is_enabled(struct clk_hw *hw)
 216 {
 217         struct isc_clk *isc_clk = to_isc_clk(hw);
 218         u32 status;
 219
 220         regmap_read(isc_clk->regmap, ISC_CLKSR, &status);
 221
 222         return status & ISC_CLK(isc_clk->id) ? 1 : 0;
 223 }
 224
 225 static unsigned long
 226 isc_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 227 {
 228         struct isc_clk *isc_clk = to_isc_clk(hw);
 229
 230         return DIV_ROUND_CLOSEST(parent_rate, isc_clk->div + 1);
 231 }
 232
 233 static int isc_clk_determine_rate(struct clk_hw *hw,
 234                                    struct clk_rate_request *req)
 235 {
 236         struct isc_clk *isc_clk = to_isc_clk(hw);
 237         long best_rate = -EINVAL;
 238         int best_diff = -1;
 239         unsigned int i, div;
 240
 241         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 242                 struct clk_hw *parent;
 243                 unsigned long parent_rate;
 244
 245                 parent = clk_hw_get_parent_by_index(hw, i);
 246                 if (!parent)
 247                         continue;
 248
 249                 parent_rate = clk_hw_get_rate(parent);
 250                 if (!parent_rate)
 251                         continue;
 252
 253                 for (div = 1; div < ISC_CLK_MAX_DIV + 2; div++) {
 254                         unsigned long rate;
 255                         int diff;
 256
 257                         rate = DIV_ROUND_CLOSEST(parent_rate, div);
 258                         diff = abs(req->rate - rate);
 259
 260                         if (best_diff < 0 || best_diff > diff) {
 261                                 best_rate = rate;
 262                                 best_diff = diff;
 263                                 req->best_parent_rate = parent_rate;
 264                                 req->best_parent_hw = parent;
 265                         }
 266
 267                         if (!best_diff || rate < req->rate)
 268                                 break;
 269                 }
 270
 271                 if (!best_diff)
 272                         break;
 273         }
 274
 275         dev_dbg(isc_clk->dev,
 276                 "ISC CLK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
 277                 __func__, best_rate,
 278                 __clk_get_name((req->best_parent_hw)->clk),
 279                 req->best_parent_rate);
 280
 281         if (best_rate < 0)
 282                 return best_rate;
 283
 284         req->rate = best_rate;
 285
 286         return 0;
 287 }
 288
 289 static int isc_clk_set_parent(struct clk_hw *hw, u8 index)
 290 {
 291         struct isc_clk *isc_clk = to_isc_clk(hw);
 292
 293         if (index >= clk_hw_get_num_parents(hw))
 294                 return -EINVAL;
 295
 296         isc_clk->parent_id = index;
 297
 298         return 0;
 299 }
 300
 301 static u8 isc_clk_get_parent(struct clk_hw *hw)
 302 {
 303         struct isc_clk *isc_clk = to_isc_clk(hw);
 304
 305         return isc_clk->parent_id;
 306 }
 307
 308 static int isc_clk_set_rate(struct clk_hw *hw,
 309                              unsigned long rate,
 310                              unsigned long parent_rate)
 311 {
 312         struct isc_clk *isc_clk = to_isc_clk(hw);
 313         u32 div;
 314
 315         if (!rate)
 316                 return -EINVAL;
 317
 318         div = DIV_ROUND_CLOSEST(parent_rate, rate);
 319         if (div > (ISC_CLK_MAX_DIV + 1) || !div)
 320                 return -EINVAL;
 321
 322         isc_clk->div = div - 1;
 323
 324         return 0;
 325 }
 326
 327 static const struct clk_ops isc_clk_ops = {
 328         .enable         = isc_clk_enable,
 329         .disable        = isc_clk_disable,
 330         .is_enabled     = isc_clk_is_enabled,
 331         .recalc_rate    = isc_clk_recalc_rate,
 332         .determine_rate = isc_clk_determine_rate,
 333         .set_parent     = isc_clk_set_parent,
 334         .get_parent     = isc_clk_get_parent,
 335         .set_rate       = isc_clk_set_rate,
 336 };
 337
 338 static int isc_clk_register(struct isc_device *isc, unsigned int id)
 339 {
 340         struct regmap *regmap = isc->regmap;
 341         struct device_node *np = isc->dev->of_node;
 342         struct isc_clk *isc_clk;
 343         struct clk_init_data init;
 344         const char *clk_name = np->name;
 345         const char *parent_names[3];
 346         int num_parents;
 347
 348         num_parents = of_clk_get_parent_count(np);
 349         if (num_parents < 1 || num_parents > 3)
 350                 return -EINVAL;
 351
 352         if (num_parents > 2 && id == ISC_ISPCK)
 353                 num_parents = 2;
 354
 355         of_clk_parent_fill(np, parent_names, num_parents);
 356
 357         if (id == ISC_MCK)
 358                 of_property_read_string(np, "clock-output-names", &clk_name);
 359         else
 360                 clk_name = "isc-ispck";
 361
 362         init.parent_names       = parent_names;
 363         init.num_parents        = num_parents;
 364         init.name               = clk_name;
 365         init.ops                = &isc_clk_ops;
 366         init.flags              = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
 367
 368         isc_clk = &isc->isc_clks[id];
 369         isc_clk->hw.init        = &init;
 370         isc_clk->regmap         = regmap;
 371         isc_clk->id             = id;
 372         isc_clk->dev            = isc->dev;
 373
 374         isc_clk->clk = clk_register(isc->dev, &isc_clk->hw);
 375         if (IS_ERR(isc_clk->clk)) {
 376                 dev_err(isc->dev, "%s: clock register fail\n", clk_name);
 377                 return PTR_ERR(isc_clk->clk);
 378         } else if (id == ISC_MCK)
 379                 of_clk_add_provider(np, of_clk_src_simple_get, isc_clk->clk);
 380
 381         return 0;
 382 }
 383
 384 static int isc_clk_init(struct isc_device *isc)
 385 {
 386         unsigned int i;
 387         int ret;
 388
 389         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++)
 390                 isc->isc_clks[i].clk = ERR_PTR(-EINVAL);
 391
 392         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
 393                 ret = isc_clk_register(isc, i);
 394                 if (ret)
 395                         return ret;
 396         }
 397
 398         return 0;
 399 }
 400
 401 static void isc_clk_cleanup(struct isc_device *isc)
 402 {
 403         unsigned int i;
 404
 405         of_clk_del_provider(isc->dev->of_node);
 406
 407         for (i = 0; i < ARRAY_SIZE(isc->isc_clks); i++) {
 408                 struct isc_clk *isc_clk = &isc->isc_clks[i];
 409
 410                 if (!IS_ERR(isc_clk->clk))
 411                         clk_unregister(isc_clk->clk);
 412         }
 413 }
 414
 415 static int isc_queue_setup(struct vb2_queue *vq,
 416                             unsigned int *nbuffers, unsigned int *nplanes,
 417                             unsigned int sizes[], struct device *alloc_devs[])
 418 {
 419         struct isc_device *isc = vb2_get_drv_priv(vq);
 420         unsigned int size = isc->fmt.fmt.pix.sizeimage;
 421
 422         if (*nplanes)
 423                 return sizes[0] < size ? -EINVAL : 0;
 424
 425         *nplanes = 1;
 426         sizes[0] = size;
 427
 428         return 0;
 429 }
 430
 431 static int isc_buffer_prepare(struct vb2_buffer *vb)
 432 {
 433         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 434         struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
 435         unsigned long size = isc->fmt.fmt.pix.sizeimage;
 436
 437         if (vb2_plane_size(vb, 0) < size) {
 438                 v4l2_err(&isc->v4l2_dev, "buffer too small (%lu < %lu)\n",
 439                          vb2_plane_size(vb, 0), size);
 440                 return -EINVAL;
 441         }
 442
 443         vb2_set_plane_payload(vb, 0, size);
 444
 445         vbuf->field = isc->fmt.fmt.pix.field;
 446
 447         return 0;
 448 }
 449
 450 static inline void isc_start_dma(struct regmap *regmap,
 451                                   struct isc_buffer *frm, u32 dview)
 452 {
 453         dma_addr_t addr;
 454
 455         addr = vb2_dma_contig_plane_dma_addr(&frm->vb.vb2_buf, 0);
 456
 457         regmap_write(regmap, ISC_DCTRL, dview | ISC_DCTRL_IE_IS);
 458         regmap_write(regmap, ISC_DAD0, addr);
 459         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_CAPTURE);
 460 }
 461
 462 static void isc_set_pipeline(struct isc_device *isc, u32 pipeline)
 463 {
 464         u32 val;
 465         unsigned int i;
 466
 467         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
 468                 val = pipeline & BIT(i) ? 1 : 0;
 469                 regmap_field_write(isc->pipeline[i], val);
 470         }
 471 }
 472
 473 static int isc_configure(struct isc_device *isc)
 474 {
 475         struct regmap *regmap = isc->regmap;
 476         const struct isc_format *current_fmt = isc->current_fmt;
 477         struct isc_subdev_entity *subdev = isc->current_subdev;
 478         u32 val, mask;
 479         int counter = 10;
 480
 481         val = current_fmt->reg_bps | subdev->pfe_cfg0 |
 482               ISC_PFE_CFG0_MODE_PROGRESSIVE;
 483         mask = ISC_PFE_CFG0_BPS_MASK | ISC_PFE_CFG0_HPOL_LOW |
 484                ISC_PFE_CFG0_VPOL_LOW | ISC_PFE_CFG0_PPOL_LOW |
 485                ISC_PFE_CFG0_MODE_MASK;
 486
 487         regmap_update_bits(regmap, ISC_PFE_CFG0, mask, val);
 488
 489         regmap_update_bits(regmap, ISC_RLP_CFG, ISC_RLP_CFG_MODE_MASK,
 490                            current_fmt->reg_rlp_mode);
 491
 492         regmap_update_bits(regmap, ISC_DCFG, ISC_DCFG_IMODE_MASK,
 493                            current_fmt->reg_dcfg_imode);
 494
 495         /* Disable the pipeline */
 496         isc_set_pipeline(isc, 0x0);
 497
 498         /* Update profile */
 499         regmap_write(regmap, ISC_CTRLEN, ISC_CTRL_UPPRO);
 500
 501         regmap_read(regmap, ISC_CTRLSR, &val);
 502         while ((val & ISC_CTRL_UPPRO) && counter--) {
 503                 usleep_range(1000, 2000);
 504                 regmap_read(regmap, ISC_CTRLSR, &val);
 505         }
 506
 507         if (counter < 0)
 508                 return -ETIMEDOUT;
 509
 510         return 0;
 511 }
 512
 513 static int isc_start_streaming(struct vb2_queue *vq, unsigned int count)
 514 {
 515         struct isc_device *isc = vb2_get_drv_priv(vq);
 516         struct regmap *regmap = isc->regmap;
 517         struct isc_buffer *buf;
 518         unsigned long flags;
 519         int ret;
 520         u32 val;
 521
 522         /* Enable stream on the sub device */
 523         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 1);
 524         if (ret && ret != -ENOIOCTLCMD) {
 525                 v4l2_err(&isc->v4l2_dev, "stream on failed in subdev\n");
 526                 goto err_start_stream;
 527         }
 528
 529         pm_runtime_get_sync(isc->dev);
 530
 531         /* Disable all the interrupts */
 532         regmap_write(isc->regmap, ISC_INTDIS, (u32)~0UL);
 533
 534         /* Clean the interrupt status register */
 535         regmap_read(regmap, ISC_INTSR, &val);
 536
 537         ret = isc_configure(isc);
 538         if (unlikely(ret))
 539                 goto err_configure;
 540
 541         /* Enable DMA interrupt */
 542         regmap_write(regmap, ISC_INTEN, ISC_INT_DDONE);
 543
 544         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 545
 546         isc->sequence = 0;
 547         isc->stop = false;
 548         reinit_completion(&isc->comp);
 549
 550         isc->cur_frm = list_first_entry(&isc->dma_queue,
 551                                         struct isc_buffer, list);
 552         list_del(&isc->cur_frm->list);
 553
 554         isc_start_dma(regmap, isc->cur_frm, isc->current_fmt->reg_dctrl_dview);
 555
 556         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 557
 558         return 0;
 559
 560 err_configure:
 561         pm_runtime_put_sync(isc->dev);
 562
 563         v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
 564
 565 err_start_stream:
 566         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 567         list_for_each_entry(buf, &isc->dma_queue, list)
 568                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 569         INIT_LIST_HEAD(&isc->dma_queue);
 570         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 571
 572         return ret;
 573 }
 574
 575 static void isc_stop_streaming(struct vb2_queue *vq)
 576 {
 577         struct isc_device *isc = vb2_get_drv_priv(vq);
 578         unsigned long flags;
 579         struct isc_buffer *buf;
 580         int ret;
 581
 582         isc->stop = true;
 583
 584         /* Wait until the end of the current frame */
 585         if (isc->cur_frm && !wait_for_completion_timeout(&isc->comp, 5 * HZ))
 586                 v4l2_err(&isc->v4l2_dev,
 587                          "Timeout waiting for end of the capture\n");
 588
 589         /* Disable DMA interrupt */
 590         regmap_write(isc->regmap, ISC_INTDIS, ISC_INT_DDONE);
 591
 592         pm_runtime_put_sync(isc->dev);
 593
 594         /* Disable stream on the sub device */
 595         ret = v4l2_subdev_call(isc->current_subdev->sd, video, s_stream, 0);
 596         if (ret && ret != -ENOIOCTLCMD)
 597                 v4l2_err(&isc->v4l2_dev, "stream off failed in subdev\n");
 598
 599         /* Release all active buffers */
 600         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 601         if (unlikely(isc->cur_frm)) {
 602                 vb2_buffer_done(&isc->cur_frm->vb.vb2_buf,
 603                                 VB2_BUF_STATE_ERROR);
 604                 isc->cur_frm = NULL;
 605         }
 606         list_for_each_entry(buf, &isc->dma_queue, list)
 607                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 608         INIT_LIST_HEAD(&isc->dma_queue);
 609         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 610 }
 611
 612 static void isc_buffer_queue(struct vb2_buffer *vb)
 613 {
 614         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 615         struct isc_buffer *buf = container_of(vbuf, struct isc_buffer, vb);
 616         struct isc_device *isc = vb2_get_drv_priv(vb->vb2_queue);
 617         unsigned long flags;
 618
 619         spin_lock_irqsave(&isc->dma_queue_lock, flags);
 620         list_add_tail(&buf->list, &isc->dma_queue);
 621         spin_unlock_irqrestore(&isc->dma_queue_lock, flags);
 622 }
 623
 624 static struct vb2_ops isc_vb2_ops = {
 625         .queue_setup            = isc_queue_setup,
 626         .wait_prepare           = vb2_ops_wait_prepare,
 627         .wait_finish            = vb2_ops_wait_finish,
 628         .buf_prepare            = isc_buffer_prepare,
 629         .start_streaming        = isc_start_streaming,
 630         .stop_streaming         = isc_stop_streaming,
 631         .buf_queue              = isc_buffer_queue,
 632 };
 633
 634 static int isc_querycap(struct file *file, void *priv,
 635                          struct v4l2_capability *cap)
 636 {
 637         struct isc_device *isc = video_drvdata(file);
 638
 639         strcpy(cap->driver, ATMEL_ISC_NAME);
 640         strcpy(cap->card, "Atmel Image Sensor Controller");
 641         snprintf(cap->bus_info, sizeof(cap->bus_info),
 642                  "platform:%s", isc->v4l2_dev.name);
 643
 644         return 0;
 645 }
 646
 647 static int isc_enum_fmt_vid_cap(struct file *file, void *priv,
 648                                  struct v4l2_fmtdesc *f)
 649 {
 650         struct isc_device *isc = video_drvdata(file);
 651         u32 index = f->index;
 652
 653         if (index >= isc->num_user_formats)
 654                 return -EINVAL;
 655
 656         f->pixelformat = isc->user_formats[index]->fourcc;
 657
 658         return 0;
 659 }
 660
 661 static int isc_g_fmt_vid_cap(struct file *file, void *priv,
 662                               struct v4l2_format *fmt)
 663 {
 664         struct isc_device *isc = video_drvdata(file);
 665
 666         *fmt = isc->fmt;
 667
 668         return 0;
 669 }
 670
 671 static struct isc_format *find_format_by_fourcc(struct isc_device *isc,
 672                                                  unsigned int fourcc)
 673 {
 674         unsigned int num_formats = isc->num_user_formats;
 675         struct isc_format *fmt;
 676         unsigned int i;
 677
 678         for (i = 0; i < num_formats; i++) {
 679                 fmt = isc->user_formats[i];
 680                 if (fmt->fourcc == fourcc)
 681                         return fmt;
 682         }
 683
 684         return NULL;
 685 }
 686
 687 static int isc_try_fmt(struct isc_device *isc, struct v4l2_format *f,
 688                         struct isc_format **current_fmt)
 689 {
 690         struct isc_format *isc_fmt;
 691         struct v4l2_pix_format *pixfmt = &f->fmt.pix;
 692         struct v4l2_subdev_format format = {
 693                 .which = V4L2_SUBDEV_FORMAT_TRY,
 694         };
 695         int ret;
 696
 697         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 698                 return -EINVAL;
 699
 700         isc_fmt = find_format_by_fourcc(isc, pixfmt->pixelformat);
 701         if (!isc_fmt) {
 702                 v4l2_warn(&isc->v4l2_dev, "Format 0x%x not found\n",
 703                           pixfmt->pixelformat);
 704                 isc_fmt = isc->user_formats[isc->num_user_formats - 1];
 705                 pixfmt->pixelformat = isc_fmt->fourcc;
 706         }
 707
 708         /* Limit to Atmel ISC hardware capabilities */
 709         if (pixfmt->width > ISC_MAX_SUPPORT_WIDTH)
 710                 pixfmt->width = ISC_MAX_SUPPORT_WIDTH;
 711         if (pixfmt->height > ISC_MAX_SUPPORT_HEIGHT)
 712                 pixfmt->height = ISC_MAX_SUPPORT_HEIGHT;
 713
 714         v4l2_fill_mbus_format(&format.format, pixfmt, isc_fmt->mbus_code);
 715         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, set_fmt,
 716                                isc->current_subdev->config, &format);
 717         if (ret < 0)
 718                 return ret;
 719
 720         v4l2_fill_pix_format(pixfmt, &format.format);
 721
 722         pixfmt->field = V4L2_FIELD_NONE;
 723         pixfmt->bytesperline = pixfmt->width * isc_fmt->bpp;
 724         pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
 725
 726         if (current_fmt)
 727                 *current_fmt = isc_fmt;
 728
 729         return 0;
 730 }
 731
 732 static int isc_set_fmt(struct isc_device *isc, struct v4l2_format *f)
 733 {
 734         struct v4l2_subdev_format format = {
 735                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 736         };
 737         struct isc_format *current_fmt;
 738         int ret;
 739
 740         ret = isc_try_fmt(isc, f, &current_fmt);
 741         if (ret)
 742                 return ret;
 743
 744         v4l2_fill_mbus_format(&format.format, &f->fmt.pix,
 745                               current_fmt->mbus_code);
 746         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
 747                                set_fmt, NULL, &format);
 748         if (ret < 0)
 749                 return ret;
 750
 751         isc->fmt = *f;
 752         isc->current_fmt = current_fmt;
 753
 754         return 0;
 755 }
 756
 757 static int isc_s_fmt_vid_cap(struct file *file, void *priv,
 758                               struct v4l2_format *f)
 759 {
 760         struct isc_device *isc = video_drvdata(file);
 761
 762         if (vb2_is_streaming(&isc->vb2_vidq))
 763                 return -EBUSY;
 764
 765         return isc_set_fmt(isc, f);
 766 }
 767
 768 static int isc_try_fmt_vid_cap(struct file *file, void *priv,
 769                                 struct v4l2_format *f)
 770 {
 771         struct isc_device *isc = video_drvdata(file);
 772
 773         return isc_try_fmt(isc, f, NULL);
 774 }
 775
 776 static int isc_enum_input(struct file *file, void *priv,
 777                            struct v4l2_input *inp)
 778 {
 779         if (inp->index != 0)
 780                 return -EINVAL;
 781
 782         inp->type = V4L2_INPUT_TYPE_CAMERA;
 783         inp->std = 0;
 784         strcpy(inp->name, "Camera");
 785
 786         return 0;
 787 }
 788
 789 static int isc_g_input(struct file *file, void *priv, unsigned int *i)
 790 {
 791         *i = 0;
 792
 793         return 0;
 794 }
 795
 796 static int isc_s_input(struct file *file, void *priv, unsigned int i)
 797 {
 798         if (i > 0)
 799                 return -EINVAL;
 800
 801         return 0;
 802 }
 803
 804 static int isc_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
 805 {
 806         struct isc_device *isc = video_drvdata(file);
 807
 808         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 809                 return -EINVAL;
 810
 811         return v4l2_subdev_call(isc->current_subdev->sd, video, g_parm, a);
 812 }
 813
 814 static int isc_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
 815 {
 816         struct isc_device *isc = video_drvdata(file);
 817
 818         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 819                 return -EINVAL;
 820
 821         return v4l2_subdev_call(isc->current_subdev->sd, video, s_parm, a);
 822 }
 823
 824 static int isc_enum_framesizes(struct file *file, void *fh,
 825                                struct v4l2_frmsizeenum *fsize)
 826 {
 827         struct isc_device *isc = video_drvdata(file);
 828         const struct isc_format *isc_fmt;
 829         struct v4l2_subdev_frame_size_enum fse = {
 830                 .index = fsize->index,
 831                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 832         };
 833         int ret;
 834
 835         isc_fmt = find_format_by_fourcc(isc, fsize->pixel_format);
 836         if (!isc_fmt)
 837                 return -EINVAL;
 838
 839         fse.code = isc_fmt->mbus_code;
 840
 841         ret = v4l2_subdev_call(isc->current_subdev->sd, pad, enum_frame_size,
 842                                NULL, &fse);
 843         if (ret)
 844                 return ret;
 845
 846         fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 847         fsize->discrete.width = fse.max_width;
 848         fsize->discrete.height = fse.max_height;
 849
 850         return 0;
 851 }
 852
 853 static int isc_enum_frameintervals(struct file *file, void *fh,
 854                                     struct v4l2_frmivalenum *fival)
 855 {
 856         struct isc_device *isc = video_drvdata(file);
 857         const struct isc_format *isc_fmt;
 858         struct v4l2_subdev_frame_interval_enum fie = {
 859                 .index = fival->index,
 860                 .width = fival->width,
 861                 .height = fival->height,
 862                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 863         };
 864         int ret;
 865
 866         isc_fmt = find_format_by_fourcc(isc, fival->pixel_format);
 867         if (!isc_fmt)
 868                 return -EINVAL;
 869
 870         fie.code = isc_fmt->mbus_code;
 871
 872         ret = v4l2_subdev_call(isc->current_subdev->sd, pad,
 873                                enum_frame_interval, NULL, &fie);
 874         if (ret)
 875                 return ret;
 876
 877         fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 878         fival->discrete = fie.interval;
 879
 880         return 0;
 881 }
 882
 883 static const struct v4l2_ioctl_ops isc_ioctl_ops = {
 884         .vidioc_querycap                = isc_querycap,
 885         .vidioc_enum_fmt_vid_cap        = isc_enum_fmt_vid_cap,
 886         .vidioc_g_fmt_vid_cap           = isc_g_fmt_vid_cap,
 887         .vidioc_s_fmt_vid_cap           = isc_s_fmt_vid_cap,
 888         .vidioc_try_fmt_vid_cap         = isc_try_fmt_vid_cap,
 889
 890         .vidioc_enum_input              = isc_enum_input,
 891         .vidioc_g_input                 = isc_g_input,
 892         .vidioc_s_input                 = isc_s_input,
 893
 894         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 895         .vidioc_querybuf                = vb2_ioctl_querybuf,
 896         .vidioc_qbuf                    = vb2_ioctl_qbuf,
 897         .vidioc_expbuf                  = vb2_ioctl_expbuf,
 898         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 899         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 900         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
 901         .vidioc_streamon                = vb2_ioctl_streamon,
 902         .vidioc_streamoff               = vb2_ioctl_streamoff,
 903
 904         .vidioc_g_parm                  = isc_g_parm,
 905         .vidioc_s_parm                  = isc_s_parm,
 906         .vidioc_enum_framesizes         = isc_enum_framesizes,
 907         .vidioc_enum_frameintervals     = isc_enum_frameintervals,
 908 };
 909
 910 static int isc_open(struct file *file)
 911 {
 912         struct isc_device *isc = video_drvdata(file);
 913         struct v4l2_subdev *sd = isc->current_subdev->sd;
 914         int ret;
 915
 916         if (mutex_lock_interruptible(&isc->lock))
 917                 return -ERESTARTSYS;
 918
 919         ret = v4l2_fh_open(file);
 920         if (ret < 0)
 921                 goto unlock;
 922
 923         if (!v4l2_fh_is_singular_file(file))
 924                 goto unlock;
 925
 926         ret = v4l2_subdev_call(sd, core, s_power, 1);
 927         if (ret < 0 && ret != -ENOIOCTLCMD) {
 928                 v4l2_fh_release(file);
 929                 goto unlock;
 930         }
 931
 932         ret = isc_set_fmt(isc, &isc->fmt);
 933         if (ret) {
 934                 v4l2_subdev_call(sd, core, s_power, 0);
 935                 v4l2_fh_release(file);
 936         }
 937
 938 unlock:
 939         mutex_unlock(&isc->lock);
 940         return ret;
 941 }
 942
 943 static int isc_release(struct file *file)
 944 {
 945         struct isc_device *isc = video_drvdata(file);
 946         struct v4l2_subdev *sd = isc->current_subdev->sd;
 947         bool fh_singular;
 948         int ret;
 949
 950         mutex_lock(&isc->lock);
 951
 952         fh_singular = v4l2_fh_is_singular_file(file);
 953
 954         ret = _vb2_fop_release(file, NULL);
 955
 956         if (fh_singular)
 957                 v4l2_subdev_call(sd, core, s_power, 0);
 958
 959         mutex_unlock(&isc->lock);
 960
 961         return ret;
 962 }
 963
 964 static const struct v4l2_file_operations isc_fops = {
 965         .owner          = THIS_MODULE,
 966         .open           = isc_open,
 967         .release        = isc_release,
 968         .unlocked_ioctl = video_ioctl2,
 969         .read           = vb2_fop_read,
 970         .mmap           = vb2_fop_mmap,
 971         .poll           = vb2_fop_poll,
 972 };
 973
 974 static irqreturn_t isc_interrupt(int irq, void *dev_id)
 975 {
 976         struct isc_device *isc = (struct isc_device *)dev_id;
 977         struct regmap *regmap = isc->regmap;
 978         u32 isc_intsr, isc_intmask, pending;
 979         irqreturn_t ret = IRQ_NONE;
 980
 981         spin_lock(&isc->dma_queue_lock);
 982
 983         regmap_read(regmap, ISC_INTSR, &isc_intsr);
 984         regmap_read(regmap, ISC_INTMASK, &isc_intmask);
 985
 986         pending = isc_intsr & isc_intmask;
 987
 988         if (likely(pending & ISC_INT_DDONE)) {
 989                 if (isc->cur_frm) {
 990                         struct vb2_v4l2_buffer *vbuf = &isc->cur_frm->vb;
 991                         struct vb2_buffer *vb = &vbuf->vb2_buf;
 992
 993                         vb->timestamp = ktime_get_ns();
 994                         vbuf->sequence = isc->sequence++;
 995                         vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 996                         isc->cur_frm = NULL;
 997                 }
 998
 999                 if (!list_empty(&isc->dma_queue) && !isc->stop) {
1000                         isc->cur_frm = list_first_entry(&isc->dma_queue,
1001                                                      struct isc_buffer, list);
1002                         list_del(&isc->cur_frm->list);
1003
1004                         isc_start_dma(regmap, isc->cur_frm,
1005                                       isc->current_fmt->reg_dctrl_dview);
1006                 }
1007
1008                 if (isc->stop)
1009                         complete(&isc->comp);
1010
1011                 ret = IRQ_HANDLED;
1012         }
1013
1014         spin_unlock(&isc->dma_queue_lock);
1015
1016         return ret;
1017 }
1018
1019 static int isc_async_bound(struct v4l2_async_notifier *notifier,
1020                             struct v4l2_subdev *subdev,
1021                             struct v4l2_async_subdev *asd)
1022 {
1023         struct isc_device *isc = container_of(notifier->v4l2_dev,
1024                                               struct isc_device, v4l2_dev);
1025         struct isc_subdev_entity *subdev_entity =
1026                 container_of(notifier, struct isc_subdev_entity, notifier);
1027
1028         if (video_is_registered(&isc->video_dev)) {
1029                 v4l2_err(&isc->v4l2_dev, "only supports one sub-device.\n");
1030                 return -EBUSY;
1031         }
1032
1033         subdev_entity->sd = subdev;
1034
1035         return 0;
1036 }
1037
1038 static void isc_async_unbind(struct v4l2_async_notifier *notifier,
1039                               struct v4l2_subdev *subdev,
1040                               struct v4l2_async_subdev *asd)
1041 {
1042         struct isc_device *isc = container_of(notifier->v4l2_dev,
1043                                               struct isc_device, v4l2_dev);
1044
1045         video_unregister_device(&isc->video_dev);
1046         if (isc->current_subdev->config)
1047                 v4l2_subdev_free_pad_config(isc->current_subdev->config);
1048 }
1049
1050 static struct isc_format *find_format_by_code(unsigned int code, int *index)
1051 {
1052         struct isc_format *fmt = &isc_formats[0];
1053         unsigned int i;
1054
1055         for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
1056                 if (fmt->mbus_code == code) {
1057                         *index = i;
1058                         return fmt;
1059                 }
1060
1061                 fmt++;
1062         }
1063
1064         return NULL;
1065 }
1066
1067 static int isc_formats_init(struct isc_device *isc)
1068 {
1069         struct isc_format *fmt;
1070         struct v4l2_subdev *subdev = isc->current_subdev->sd;
1071         int num_fmts = 0, i, j;
1072         struct v4l2_subdev_mbus_code_enum mbus_code = {
1073                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1074         };
1075
1076         fmt = &isc_formats[0];
1077         for (i = 0; i < ARRAY_SIZE(isc_formats); i++) {
1078                 fmt->support = false;
1079                 fmt++;
1080         }
1081
1082         while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
1083                NULL, &mbus_code)) {
1084                 mbus_code.index++;
1085                 fmt = find_format_by_code(mbus_code.code, &i);
1086                 if (!fmt)
1087                         continue;
1088
1089                 fmt->support = true;
1090                 num_fmts++;
1091         }
1092
1093         if (!num_fmts)
1094                 return -ENXIO;
1095
1096         isc->num_user_formats = num_fmts;
1097         isc->user_formats = devm_kcalloc(isc->dev,
1098                                          num_fmts, sizeof(struct isc_format *),
1099                                          GFP_KERNEL);
1100         if (!isc->user_formats) {
1101                 v4l2_err(&isc->v4l2_dev, "could not allocate memory\n");
1102                 return -ENOMEM;
1103         }
1104
1105         fmt = &isc_formats[0];
1106         for (i = 0, j = 0; i < ARRAY_SIZE(isc_formats); i++) {
1107                 if (fmt->support)
1108                         isc->user_formats[j++] = fmt;
1109
1110                 fmt++;
1111         }
1112
1113         return 0;
1114 }
1115
1116 static int isc_set_default_fmt(struct isc_device *isc)
1117 {
1118         struct v4l2_format f = {
1119                 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
1120                 .fmt.pix = {
1121                         .width          = VGA_WIDTH,
1122                         .height         = VGA_HEIGHT,
1123                         .field          = V4L2_FIELD_NONE,
1124                         .pixelformat    = isc->user_formats[0]->fourcc,
1125                 },
1126         };
1127         int ret;
1128
1129         ret = isc_try_fmt(isc, &f, NULL);
1130         if (ret)
1131                 return ret;
1132
1133         isc->current_fmt = isc->user_formats[0];
1134         isc->fmt = f;
1135
1136         return 0;
1137 }
1138
1139 static int isc_async_complete(struct v4l2_async_notifier *notifier)
1140 {
1141         struct isc_device *isc = container_of(notifier->v4l2_dev,
1142                                               struct isc_device, v4l2_dev);
1143         struct isc_subdev_entity *sd_entity;
1144         struct video_device *vdev = &isc->video_dev;
1145         struct vb2_queue *q = &isc->vb2_vidq;
1146         int ret;
1147
1148         isc->current_subdev = container_of(notifier,
1149                                            struct isc_subdev_entity, notifier);
1150         sd_entity = isc->current_subdev;
1151
1152         mutex_init(&isc->lock);
1153         init_completion(&isc->comp);
1154
1155         /* Initialize videobuf2 queue */
1156         q->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1157         q->io_modes             = VB2_MMAP | VB2_DMABUF | VB2_READ;
1158         q->drv_priv             = isc;
1159         q->buf_struct_size      = sizeof(struct isc_buffer);
1160         q->ops                  = &isc_vb2_ops;
1161         q->mem_ops              = &vb2_dma_contig_memops;
1162         q->timestamp_flags      = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1163         q->lock                 = &isc->lock;
1164         q->min_buffers_needed   = 1;
1165         q->dev                  = isc->dev;
1166
1167         ret = vb2_queue_init(q);
1168         if (ret < 0) {
1169                 v4l2_err(&isc->v4l2_dev,
1170                          "vb2_queue_init() failed: %d\n", ret);
1171                 return ret;
1172         }
1173
1174         /* Init video dma queues */
1175         INIT_LIST_HEAD(&isc->dma_queue);
1176         spin_lock_init(&isc->dma_queue_lock);
1177
1178         sd_entity->config = v4l2_subdev_alloc_pad_config(sd_entity->sd);
1179         if (sd_entity->config == NULL)
1180                 return -ENOMEM;
1181
1182         ret = isc_formats_init(isc);
1183         if (ret < 0) {
1184                 v4l2_err(&isc->v4l2_dev,
1185                          "Init format failed: %d\n", ret);
1186                 return ret;
1187         }
1188
1189         ret = isc_set_default_fmt(isc);
1190         if (ret) {
1191                 v4l2_err(&isc->v4l2_dev, "Could not set default format\n");
1192                 return ret;
1193         }
1194
1195         /* Register video device */
1196         strlcpy(vdev->name, ATMEL_ISC_NAME, sizeof(vdev->name));
1197         vdev->release           = video_device_release_empty;
1198         vdev->fops              = &isc_fops;
1199         vdev->ioctl_ops         = &isc_ioctl_ops;
1200         vdev->v4l2_dev          = &isc->v4l2_dev;
1201         vdev->vfl_dir           = VFL_DIR_RX;
1202         vdev->queue             = q;
1203         vdev->lock              = &isc->lock;
1204         vdev->ctrl_handler      = isc->current_subdev->sd->ctrl_handler;
1205         vdev->device_caps       = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
1206         video_set_drvdata(vdev, isc);
1207
1208         ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1209         if (ret < 0) {
1210                 v4l2_err(&isc->v4l2_dev,
1211                          "video_register_device failed: %d\n", ret);
1212                 return ret;
1213         }
1214
1215         return 0;
1216 }
1217
1218 static void isc_subdev_cleanup(struct isc_device *isc)
1219 {
1220         struct isc_subdev_entity *subdev_entity;
1221
1222         list_for_each_entry(subdev_entity, &isc->subdev_entities, list)
1223                 v4l2_async_notifier_unregister(&subdev_entity->notifier);
1224
1225         INIT_LIST_HEAD(&isc->subdev_entities);
1226 }
1227
1228 static int isc_pipeline_init(struct isc_device *isc)
1229 {
1230         struct device *dev = isc->dev;
1231         struct regmap *regmap = isc->regmap;
1232         struct regmap_field *regs;
1233         unsigned int i;
1234
1235         /* WB-->CFA-->CC-->GAM-->CSC-->CBC-->SUB422-->SUB420 */
1236         const struct reg_field regfields[ISC_PIPE_LINE_NODE_NUM] = {
1237                 REG_FIELD(ISC_WB_CTRL, 0, 0),
1238                 REG_FIELD(ISC_CFA_CTRL, 0, 0),
1239                 REG_FIELD(ISC_CC_CTRL, 0, 0),
1240                 REG_FIELD(ISC_GAM_CTRL, 0, 0),
1241                 REG_FIELD(ISC_GAM_CTRL, 1, 1),
1242                 REG_FIELD(ISC_GAM_CTRL, 2, 2),
1243                 REG_FIELD(ISC_GAM_CTRL, 3, 3),
1244                 REG_FIELD(ISC_CSC_CTRL, 0, 0),
1245                 REG_FIELD(ISC_CBC_CTRL, 0, 0),
1246                 REG_FIELD(ISC_SUB422_CTRL, 0, 0),
1247                 REG_FIELD(ISC_SUB420_CTRL, 0, 0),
1248         };
1249
1250         for (i = 0; i < ISC_PIPE_LINE_NODE_NUM; i++) {
1251                 regs = devm_regmap_field_alloc(dev, regmap, regfields[i]);
1252                 if (IS_ERR(regs))
1253                         return PTR_ERR(regs);
1254
1255                 isc->pipeline[i] =  regs;
1256         }
1257
1258         return 0;
1259 }
1260
1261 static int isc_parse_dt(struct device *dev, struct isc_device *isc)
1262 {
1263         struct device_node *np = dev->of_node;
1264         struct device_node *epn = NULL, *rem;
1265         struct v4l2_of_endpoint v4l2_epn;
1266         struct isc_subdev_entity *subdev_entity;
1267         unsigned int flags;
1268         int ret;
1269
1270         INIT_LIST_HEAD(&isc->subdev_entities);
1271
1272         for (; ;) {
1273                 epn = of_graph_get_next_endpoint(np, epn);
1274                 if (!epn)
1275                         break;
1276
1277                 rem = of_graph_get_remote_port_parent(epn);
1278                 if (!rem) {
1279                         dev_notice(dev, "Remote device at %s not found\n",
1280                                    of_node_full_name(epn));
1281                         continue;
1282                 }
1283
1284                 ret = v4l2_of_parse_endpoint(epn, &v4l2_epn);
1285                 if (ret) {
1286                         of_node_put(rem);
1287                         ret = -EINVAL;
1288                         dev_err(dev, "Could not parse the endpoint\n");
1289                         break;
1290                 }
1291
1292                 subdev_entity = devm_kzalloc(dev,
1293                                           sizeof(*subdev_entity), GFP_KERNEL);
1294                 if (subdev_entity == NULL) {
1295                         of_node_put(rem);
1296                         ret = -ENOMEM;
1297                         break;
1298                 }
1299
1300                 subdev_entity->asd = devm_kzalloc(dev,
1301                                      sizeof(*subdev_entity->asd), GFP_KERNEL);
1302                 if (subdev_entity->asd == NULL) {
1303                         of_node_put(rem);
1304                         ret = -ENOMEM;
1305                         break;
1306                 }
1307
1308                 flags = v4l2_epn.bus.parallel.flags;
1309
1310                 if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
1311                         subdev_entity->pfe_cfg0 = ISC_PFE_CFG0_HPOL_LOW;
1312
1313                 if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
1314                         subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_VPOL_LOW;
1315
1316                 if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
1317                         subdev_entity->pfe_cfg0 |= ISC_PFE_CFG0_PPOL_LOW;
1318
1319                 subdev_entity->asd->match_type = V4L2_ASYNC_MATCH_OF;
1320                 subdev_entity->asd->match.of.node = rem;
1321                 list_add_tail(&subdev_entity->list, &isc->subdev_entities);
1322         }
1323
1324         of_node_put(epn);
1325         return ret;
1326 }
1327
1328 /* regmap configuration */
1329 #define ATMEL_ISC_REG_MAX    0xbfc
1330 static const struct regmap_config isc_regmap_config = {
1331         .reg_bits       = 32,
1332         .reg_stride     = 4,
1333         .val_bits       = 32,
1334         .max_register   = ATMEL_ISC_REG_MAX,
1335 };
1336
1337 static int atmel_isc_probe(struct platform_device *pdev)
1338 {
1339         struct device *dev = &pdev->dev;
1340         struct isc_device *isc;
1341         struct resource *res;
1342         void __iomem *io_base;
1343         struct isc_subdev_entity *subdev_entity;
1344         int irq;
1345         int ret;
1346
1347         isc = devm_kzalloc(dev, sizeof(*isc), GFP_KERNEL);
1348         if (!isc)
1349                 return -ENOMEM;
1350
1351         platform_set_drvdata(pdev, isc);
1352         isc->dev = dev;
1353
1354         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1355         io_base = devm_ioremap_resource(dev, res);
1356         if (IS_ERR(io_base))
1357                 return PTR_ERR(io_base);
1358
1359         isc->regmap = devm_regmap_init_mmio(dev, io_base, &isc_regmap_config);
1360         if (IS_ERR(isc->regmap)) {
1361                 ret = PTR_ERR(isc->regmap);
1362                 dev_err(dev, "failed to init register map: %d\n", ret);
1363                 return ret;
1364         }
1365
1366         irq = platform_get_irq(pdev, 0);
1367         if (irq < 0) {
1368                 ret = irq;
1369                 dev_err(dev, "failed to get irq: %d\n", ret);
1370                 return ret;
1371         }
1372
1373         ret = devm_request_irq(dev, irq, isc_interrupt, 0,
1374                                ATMEL_ISC_NAME, isc);
1375         if (ret < 0) {
1376                 dev_err(dev, "can't register ISR for IRQ %u (ret=%i)\n",
1377                         irq, ret);
1378                 return ret;
1379         }
1380
1381         ret = isc_pipeline_init(isc);
1382         if (ret)
1383                 return ret;
1384
1385         isc->hclock = devm_clk_get(dev, "hclock");
1386         if (IS_ERR(isc->hclock)) {
1387                 ret = PTR_ERR(isc->hclock);
1388                 dev_err(dev, "failed to get hclock: %d\n", ret);
1389                 return ret;
1390         }
1391
1392         ret = isc_clk_init(isc);
1393         if (ret) {
1394                 dev_err(dev, "failed to init isc clock: %d\n", ret);
1395                 goto clean_isc_clk;
1396         }
1397
1398         isc->ispck = isc->isc_clks[ISC_ISPCK].clk;
1399
1400         /* ispck should be greater or equal to hclock */
1401         ret = clk_set_rate(isc->ispck, clk_get_rate(isc->hclock));
1402         if (ret) {
1403                 dev_err(dev, "failed to set ispck rate: %d\n", ret);
1404                 goto clean_isc_clk;
1405         }
1406
1407         ret = v4l2_device_register(dev, &isc->v4l2_dev);
1408         if (ret) {
1409                 dev_err(dev, "unable to register v4l2 device.\n");
1410                 goto clean_isc_clk;
1411         }
1412
1413         ret = isc_parse_dt(dev, isc);
1414         if (ret) {
1415                 dev_err(dev, "fail to parse device tree\n");
1416                 goto unregister_v4l2_device;
1417         }
1418
1419         if (list_empty(&isc->subdev_entities)) {
1420                 dev_err(dev, "no subdev found\n");
1421                 goto unregister_v4l2_device;
1422         }
1423
1424         list_for_each_entry(subdev_entity, &isc->subdev_entities, list) {
1425                 subdev_entity->notifier.subdevs = &subdev_entity->asd;
1426                 subdev_entity->notifier.num_subdevs = 1;
1427                 subdev_entity->notifier.bound = isc_async_bound;
1428                 subdev_entity->notifier.unbind = isc_async_unbind;
1429                 subdev_entity->notifier.complete = isc_async_complete;
1430
1431                 ret = v4l2_async_notifier_register(&isc->v4l2_dev,
1432                                                    &subdev_entity->notifier);
1433                 if (ret) {
1434                         dev_err(dev, "fail to register async notifier\n");
1435                         goto cleanup_subdev;
1436                 }
1437
1438                 if (video_is_registered(&isc->video_dev))
1439                         break;
1440         }
1441
1442         pm_runtime_enable(dev);
1443
1444         return 0;
1445
1446 cleanup_subdev:
1447         isc_subdev_cleanup(isc);
1448
1449 unregister_v4l2_device:
1450         v4l2_device_unregister(&isc->v4l2_dev);
1451
1452 clean_isc_clk:
1453         isc_clk_cleanup(isc);
1454
1455         return ret;
1456 }
1457
1458 static int atmel_isc_remove(struct platform_device *pdev)
1459 {
1460         struct isc_device *isc = platform_get_drvdata(pdev);
1461
1462         pm_runtime_disable(&pdev->dev);
1463
1464         isc_subdev_cleanup(isc);
1465
1466         v4l2_device_unregister(&isc->v4l2_dev);
1467
1468         isc_clk_cleanup(isc);
1469
1470         return 0;
1471 }
1472
1473 static int __maybe_unused isc_runtime_suspend(struct device *dev)
1474 {
1475         struct isc_device *isc = dev_get_drvdata(dev);
1476
1477         clk_disable_unprepare(isc->ispck);
1478         clk_disable_unprepare(isc->hclock);
1479
1480         return 0;
1481 }
1482
1483 static int __maybe_unused isc_runtime_resume(struct device *dev)
1484 {
1485         struct isc_device *isc = dev_get_drvdata(dev);
1486         int ret;
1487
1488         ret = clk_prepare_enable(isc->hclock);
1489         if (ret)
1490                 return ret;
1491
1492         return clk_prepare_enable(isc->ispck);
1493 }
1494
1495 static const struct dev_pm_ops atmel_isc_dev_pm_ops = {
1496         SET_RUNTIME_PM_OPS(isc_runtime_suspend, isc_runtime_resume, NULL)
1497 };
1498
1499 static const struct of_device_id atmel_isc_of_match[] = {
1500         { .compatible = "atmel,sama5d2-isc" },
1501         { }
1502 };
1503 MODULE_DEVICE_TABLE(of, atmel_isc_of_match);
1504
1505 static struct platform_driver atmel_isc_driver = {
1506         .probe  = atmel_isc_probe,
1507         .remove = atmel_isc_remove,
1508         .driver = {
1509                 .name           = ATMEL_ISC_NAME,
1510                 .pm             = &atmel_isc_dev_pm_ops,
1511                 .of_match_table = of_match_ptr(atmel_isc_of_match),
1512         },
1513 };
1514
1515 module_platform_driver(atmel_isc_driver);
1516
1517 MODULE_AUTHOR("Songjun Wu <songjun.wu@microchip.com>");
1518 MODULE_DESCRIPTION("The V4L2 driver for Atmel-ISC");
1519 MODULE_LICENSE("GPL v2");
1520 MODULE_SUPPORTED_DEVICE("video");