drivers/media/platform/pxa_camera.c

   1 /*
   2  * V4L2 Driver for PXA camera host
   3  *
   4  * Copyright (C) 2006, Sascha Hauer, Pengutronix
   5  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
   6  * Copyright (C) 2016, Robert Jarzmik <robert.jarzmik@free.fr>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; either version 2 of the License, or
  11  * (at your option) any later version.
  12  */
  13
  14 #include <linux/init.h>
  15 #include <linux/module.h>
  16 #include <linux/io.h>
  17 #include <linux/delay.h>
  18 #include <linux/device.h>
  19 #include <linux/dma-mapping.h>
  20 #include <linux/err.h>
  21 #include <linux/errno.h>
  22 #include <linux/fs.h>
  23 #include <linux/interrupt.h>
  24 #include <linux/kernel.h>
  25 #include <linux/mm.h>
  26 #include <linux/moduleparam.h>
  27 #include <linux/of.h>
  28 #include <linux/time.h>
  29 #include <linux/device.h>
  30 #include <linux/platform_device.h>
  31 #include <linux/clk.h>
  32 #include <linux/sched.h>
  33 #include <linux/slab.h>
  34 #include <linux/dmaengine.h>
  35 #include <linux/dma-mapping.h>
  36 #include <linux/dma/pxa-dma.h>
  37
  38 #include <media/v4l2-async.h>
  39 #include <media/v4l2-clk.h>
  40 #include <media/v4l2-common.h>
  41 #include <media/v4l2-device.h>
  42 #include <media/v4l2-ioctl.h>
  43 #include <media/v4l2-of.h>
  44
  45 #include <media/drv-intf/soc_mediabus.h>
  46 #include <media/videobuf2-dma-sg.h>
  47
  48 #include <linux/videodev2.h>
  49
  50 #include <linux/platform_data/media/camera-pxa.h>
  51
  52 #define PXA_CAM_VERSION "0.0.6"
  53 #define PXA_CAM_DRV_NAME "pxa27x-camera"
  54
  55 #define DEFAULT_WIDTH   640
  56 #define DEFAULT_HEIGHT  480
  57
  58 /* Camera Interface */
  59 #define CICR0           0x0000
  60 #define CICR1           0x0004
  61 #define CICR2           0x0008
  62 #define CICR3           0x000C
  63 #define CICR4           0x0010
  64 #define CISR            0x0014
  65 #define CIFR            0x0018
  66 #define CITOR           0x001C
  67 #define CIBR0           0x0028
  68 #define CIBR1           0x0030
  69 #define CIBR2           0x0038
  70
  71 #define CICR0_DMAEN     (1 << 31)       /* DMA request enable */
  72 #define CICR0_PAR_EN    (1 << 30)       /* Parity enable */
  73 #define CICR0_SL_CAP_EN (1 << 29)       /* Capture enable for slave mode */
  74 #define CICR0_ENB       (1 << 28)       /* Camera interface enable */
  75 #define CICR0_DIS       (1 << 27)       /* Camera interface disable */
  76 #define CICR0_SIM       (0x7 << 24)     /* Sensor interface mode mask */
  77 #define CICR0_TOM       (1 << 9)        /* Time-out mask */
  78 #define CICR0_RDAVM     (1 << 8)        /* Receive-data-available mask */
  79 #define CICR0_FEM       (1 << 7)        /* FIFO-empty mask */
  80 #define CICR0_EOLM      (1 << 6)        /* End-of-line mask */
  81 #define CICR0_PERRM     (1 << 5)        /* Parity-error mask */
  82 #define CICR0_QDM       (1 << 4)        /* Quick-disable mask */
  83 #define CICR0_CDM       (1 << 3)        /* Disable-done mask */
  84 #define CICR0_SOFM      (1 << 2)        /* Start-of-frame mask */
  85 #define CICR0_EOFM      (1 << 1)        /* End-of-frame mask */
  86 #define CICR0_FOM       (1 << 0)        /* FIFO-overrun mask */
  87
  88 #define CICR1_TBIT      (1 << 31)       /* Transparency bit */
  89 #define CICR1_RGBT_CONV (0x3 << 29)     /* RGBT conversion mask */
  90 #define CICR1_PPL       (0x7ff << 15)   /* Pixels per line mask */
  91 #define CICR1_RGB_CONV  (0x7 << 12)     /* RGB conversion mask */
  92 #define CICR1_RGB_F     (1 << 11)       /* RGB format */
  93 #define CICR1_YCBCR_F   (1 << 10)       /* YCbCr format */
  94 #define CICR1_RGB_BPP   (0x7 << 7)      /* RGB bis per pixel mask */
  95 #define CICR1_RAW_BPP   (0x3 << 5)      /* Raw bis per pixel mask */
  96 #define CICR1_COLOR_SP  (0x3 << 3)      /* Color space mask */
  97 #define CICR1_DW        (0x7 << 0)      /* Data width mask */
  98
  99 #define CICR2_BLW       (0xff << 24)    /* Beginning-of-line pixel clock
 100                                            wait count mask */
 101 #define CICR2_ELW       (0xff << 16)    /* End-of-line pixel clock
 102                                            wait count mask */
 103 #define CICR2_HSW       (0x3f << 10)    /* Horizontal sync pulse width mask */
 104 #define CICR2_BFPW      (0x3f << 3)     /* Beginning-of-frame pixel clock
 105                                            wait count mask */
 106 #define CICR2_FSW       (0x7 << 0)      /* Frame stabilization
 107                                            wait count mask */
 108
 109 #define CICR3_BFW       (0xff << 24)    /* Beginning-of-frame line clock
 110                                            wait count mask */
 111 #define CICR3_EFW       (0xff << 16)    /* End-of-frame line clock
 112                                            wait count mask */
 113 #define CICR3_VSW       (0x3f << 10)    /* Vertical sync pulse width mask */
 114 #define CICR3_BFPW      (0x3f << 3)     /* Beginning-of-frame pixel clock
 115                                            wait count mask */
 116 #define CICR3_LPF       (0x7ff << 0)    /* Lines per frame mask */
 117
 118 #define CICR4_MCLK_DLY  (0x3 << 24)     /* MCLK Data Capture Delay mask */
 119 #define CICR4_PCLK_EN   (1 << 23)       /* Pixel clock enable */
 120 #define CICR4_PCP       (1 << 22)       /* Pixel clock polarity */
 121 #define CICR4_HSP       (1 << 21)       /* Horizontal sync polarity */
 122 #define CICR4_VSP       (1 << 20)       /* Vertical sync polarity */
 123 #define CICR4_MCLK_EN   (1 << 19)       /* MCLK enable */
 124 #define CICR4_FR_RATE   (0x7 << 8)      /* Frame rate mask */
 125 #define CICR4_DIV       (0xff << 0)     /* Clock divisor mask */
 126
 127 #define CISR_FTO        (1 << 15)       /* FIFO time-out */
 128 #define CISR_RDAV_2     (1 << 14)       /* Channel 2 receive data available */
 129 #define CISR_RDAV_1     (1 << 13)       /* Channel 1 receive data available */
 130 #define CISR_RDAV_0     (1 << 12)       /* Channel 0 receive data available */
 131 #define CISR_FEMPTY_2   (1 << 11)       /* Channel 2 FIFO empty */
 132 #define CISR_FEMPTY_1   (1 << 10)       /* Channel 1 FIFO empty */
 133 #define CISR_FEMPTY_0   (1 << 9)        /* Channel 0 FIFO empty */
 134 #define CISR_EOL        (1 << 8)        /* End of line */
 135 #define CISR_PAR_ERR    (1 << 7)        /* Parity error */
 136 #define CISR_CQD        (1 << 6)        /* Camera interface quick disable */
 137 #define CISR_CDD        (1 << 5)        /* Camera interface disable done */
 138 #define CISR_SOF        (1 << 4)        /* Start of frame */
 139 #define CISR_EOF        (1 << 3)        /* End of frame */
 140 #define CISR_IFO_2      (1 << 2)        /* FIFO overrun for Channel 2 */
 141 #define CISR_IFO_1      (1 << 1)        /* FIFO overrun for Channel 1 */
 142 #define CISR_IFO_0      (1 << 0)        /* FIFO overrun for Channel 0 */
 143
 144 #define CIFR_FLVL2      (0x7f << 23)    /* FIFO 2 level mask */
 145 #define CIFR_FLVL1      (0x7f << 16)    /* FIFO 1 level mask */
 146 #define CIFR_FLVL0      (0xff << 8)     /* FIFO 0 level mask */
 147 #define CIFR_THL_0      (0x3 << 4)      /* Threshold Level for Channel 0 FIFO */
 148 #define CIFR_RESET_F    (1 << 3)        /* Reset input FIFOs */
 149 #define CIFR_FEN2       (1 << 2)        /* FIFO enable for channel 2 */
 150 #define CIFR_FEN1       (1 << 1)        /* FIFO enable for channel 1 */
 151 #define CIFR_FEN0       (1 << 0)        /* FIFO enable for channel 0 */
 152
 153 #define CICR0_SIM_MP    (0 << 24)
 154 #define CICR0_SIM_SP    (1 << 24)
 155 #define CICR0_SIM_MS    (2 << 24)
 156 #define CICR0_SIM_EP    (3 << 24)
 157 #define CICR0_SIM_ES    (4 << 24)
 158
 159 #define CICR1_DW_VAL(x)   ((x) & CICR1_DW)          /* Data bus width */
 160 #define CICR1_PPL_VAL(x)  (((x) << 15) & CICR1_PPL) /* Pixels per line */
 161 #define CICR1_COLOR_SP_VAL(x)   (((x) << 3) & CICR1_COLOR_SP)   /* color space */
 162 #define CICR1_RGB_BPP_VAL(x)    (((x) << 7) & CICR1_RGB_BPP)    /* bpp for rgb */
 163 #define CICR1_RGBT_CONV_VAL(x)  (((x) << 29) & CICR1_RGBT_CONV) /* rgbt conv */
 164
 165 #define CICR2_BLW_VAL(x)  (((x) << 24) & CICR2_BLW) /* Beginning-of-line pixel clock wait count */
 166 #define CICR2_ELW_VAL(x)  (((x) << 16) & CICR2_ELW) /* End-of-line pixel clock wait count */
 167 #define CICR2_HSW_VAL(x)  (((x) << 10) & CICR2_HSW) /* Horizontal sync pulse width */
 168 #define CICR2_BFPW_VAL(x) (((x) << 3) & CICR2_BFPW) /* Beginning-of-frame pixel clock wait count */
 169 #define CICR2_FSW_VAL(x)  (((x) << 0) & CICR2_FSW)  /* Frame stabilization wait count */
 170
 171 #define CICR3_BFW_VAL(x)  (((x) << 24) & CICR3_BFW) /* Beginning-of-frame line clock wait count  */
 172 #define CICR3_EFW_VAL(x)  (((x) << 16) & CICR3_EFW) /* End-of-frame line clock wait count */
 173 #define CICR3_VSW_VAL(x)  (((x) << 11) & CICR3_VSW) /* Vertical sync pulse width */
 174 #define CICR3_LPF_VAL(x)  (((x) << 0) & CICR3_LPF)  /* Lines per frame */
 175
 176 #define CICR0_IRQ_MASK (CICR0_TOM | CICR0_RDAVM | CICR0_FEM | CICR0_EOLM | \
 177                         CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
 178                         CICR0_EOFM | CICR0_FOM)
 179
 180 #define sensor_call(cam, o, f, args...) \
 181         v4l2_subdev_call(cam->sensor, o, f, ##args)
 182
 183 /*
 184  * Format handling
 185  */
 186 /**
 187  * struct soc_camera_format_xlate - match between host and sensor formats
 188  * @code: code of a sensor provided format
 189  * @host_fmt: host format after host translation from code
 190  *
 191  * Host and sensor translation structure. Used in table of host and sensor
 192  * formats matchings in soc_camera_device. A host can override the generic list
 193  * generation by implementing get_formats(), and use it for format checks and
 194  * format setup.
 195  */
 196 struct soc_camera_format_xlate {
 197         u32 code;
 198         const struct soc_mbus_pixelfmt *host_fmt;
 199 };
 200
 201 /*
 202  * Structures
 203  */
 204 enum pxa_camera_active_dma {
 205         DMA_Y = 0x1,
 206         DMA_U = 0x2,
 207         DMA_V = 0x4,
 208 };
 209
 210 /* buffer for one video frame */
 211 struct pxa_buffer {
 212         /* common v4l buffer stuff -- must be first */
 213         struct vb2_v4l2_buffer          vbuf;
 214         struct list_head                queue;
 215         u32     code;
 216         int                             nb_planes;
 217         /* our descriptor lists for Y, U and V channels */
 218         struct dma_async_tx_descriptor  *descs[3];
 219         dma_cookie_t                    cookie[3];
 220         struct scatterlist              *sg[3];
 221         int                             sg_len[3];
 222         size_t                          plane_sizes[3];
 223         int                             inwork;
 224         enum pxa_camera_active_dma      active_dma;
 225 };
 226
 227 struct pxa_camera_dev {
 228         struct v4l2_device      v4l2_dev;
 229         struct video_device     vdev;
 230         struct v4l2_async_notifier notifier;
 231         struct vb2_queue        vb2_vq;
 232         struct v4l2_subdev      *sensor;
 233         struct soc_camera_format_xlate *user_formats;
 234         const struct soc_camera_format_xlate *current_fmt;
 235         struct v4l2_pix_format  current_pix;
 236
 237         struct v4l2_async_subdev asd;
 238         struct v4l2_async_subdev *asds[1];
 239
 240         /*
 241          * PXA27x is only supposed to handle one camera on its Quick Capture
 242          * interface. If anyone ever builds hardware to enable more than
 243          * one camera, they will have to modify this driver too
 244          */
 245         struct clk              *clk;
 246
 247         unsigned int            irq;
 248         void __iomem            *base;
 249
 250         int                     channels;
 251         struct dma_chan         *dma_chans[3];
 252
 253         struct pxacamera_platform_data *pdata;
 254         struct resource         *res;
 255         unsigned long           platform_flags;
 256         unsigned long           ciclk;
 257         unsigned long           mclk;
 258         u32                     mclk_divisor;
 259         struct v4l2_clk         *mclk_clk;
 260         u16                     width_flags;    /* max 10 bits */
 261
 262         struct list_head        capture;
 263
 264         spinlock_t              lock;
 265         struct mutex            mlock;
 266         unsigned int            buf_sequence;
 267
 268         struct pxa_buffer       *active;
 269         struct tasklet_struct   task_eof;
 270
 271         u32                     save_cicr[5];
 272 };
 273
 274 struct pxa_cam {
 275         unsigned long flags;
 276 };
 277
 278 static const char *pxa_cam_driver_description = "PXA_Camera";
 279
 280 /*
 281  * Format translation functions
 282  */
 283 static const struct soc_camera_format_xlate
 284 *soc_mbus_xlate_by_fourcc(struct soc_camera_format_xlate *user_formats,
 285                           unsigned int fourcc)
 286 {
 287         unsigned int i;
 288
 289         for (i = 0; user_formats[i].code; i++)
 290                 if (user_formats[i].host_fmt->fourcc == fourcc)
 291                         return user_formats + i;
 292         return NULL;
 293 }
 294
 295 static struct soc_camera_format_xlate *soc_mbus_build_fmts_xlate(
 296         struct v4l2_device *v4l2_dev, struct v4l2_subdev *subdev,
 297         int (*get_formats)(struct v4l2_device *, unsigned int,
 298                            struct soc_camera_format_xlate *xlate))
 299 {
 300         unsigned int i, fmts = 0, raw_fmts = 0;
 301         int ret;
 302         struct v4l2_subdev_mbus_code_enum code = {
 303                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 304         };
 305         struct soc_camera_format_xlate *user_formats;
 306
 307         while (!v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code)) {
 308                 raw_fmts++;
 309                 code.index++;
 310         }
 311
 312         /*
 313          * First pass - only count formats this host-sensor
 314          * configuration can provide
 315          */
 316         for (i = 0; i < raw_fmts; i++) {
 317                 ret = get_formats(v4l2_dev, i, NULL);
 318                 if (ret < 0)
 319                         return ERR_PTR(ret);
 320                 fmts += ret;
 321         }
 322
 323         if (!fmts)
 324                 return ERR_PTR(-ENXIO);
 325
 326         user_formats = kcalloc(fmts + 1, sizeof(*user_formats), GFP_KERNEL);
 327         if (!user_formats)
 328                 return ERR_PTR(-ENOMEM);
 329
 330         /* Second pass - actually fill data formats */
 331         fmts = 0;
 332         for (i = 0; i < raw_fmts; i++) {
 333                 ret = get_formats(v4l2_dev, i, user_formats + fmts);
 334                 if (ret < 0)
 335                         goto egfmt;
 336                 fmts += ret;
 337         }
 338         user_formats[fmts].code = 0;
 339
 340         return user_formats;
 341 egfmt:
 342         kfree(user_formats);
 343         return ERR_PTR(ret);
 344 }
 345
 346 /*
 347  *  Videobuf operations
 348  */
 349 static struct pxa_buffer *vb2_to_pxa_buffer(struct vb2_buffer *vb)
 350 {
 351         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 352
 353         return container_of(vbuf, struct pxa_buffer, vbuf);
 354 }
 355
 356 static struct device *pcdev_to_dev(struct pxa_camera_dev *pcdev)
 357 {
 358         return pcdev->v4l2_dev.dev;
 359 }
 360
 361 static struct pxa_camera_dev *v4l2_dev_to_pcdev(struct v4l2_device *v4l2_dev)
 362 {
 363         return container_of(v4l2_dev, struct pxa_camera_dev, v4l2_dev);
 364 }
 365
 366 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 367                                enum pxa_camera_active_dma act_dma);
 368
 369 static void pxa_camera_dma_irq_y(void *data)
 370 {
 371         struct pxa_camera_dev *pcdev = data;
 372
 373         pxa_camera_dma_irq(pcdev, DMA_Y);
 374 }
 375
 376 static void pxa_camera_dma_irq_u(void *data)
 377 {
 378         struct pxa_camera_dev *pcdev = data;
 379
 380         pxa_camera_dma_irq(pcdev, DMA_U);
 381 }
 382
 383 static void pxa_camera_dma_irq_v(void *data)
 384 {
 385         struct pxa_camera_dev *pcdev = data;
 386
 387         pxa_camera_dma_irq(pcdev, DMA_V);
 388 }
 389
 390 /**
 391  * pxa_init_dma_channel - init dma descriptors
 392  * @pcdev: pxa camera device
 393  * @vb: videobuffer2 buffer
 394  * @dma: dma video buffer
 395  * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
 396  * @cibr: camera Receive Buffer Register
 397  *
 398  * Prepares the pxa dma descriptors to transfer one camera channel.
 399  *
 400  * Returns 0 if success or -ENOMEM if no memory is available
 401  */
 402 static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
 403                                 struct pxa_buffer *buf, int channel,
 404                                 struct scatterlist *sg, int sglen)
 405 {
 406         struct dma_chan *dma_chan = pcdev->dma_chans[channel];
 407         struct dma_async_tx_descriptor *tx;
 408
 409         tx = dmaengine_prep_slave_sg(dma_chan, sg, sglen, DMA_DEV_TO_MEM,
 410                                      DMA_PREP_INTERRUPT | DMA_CTRL_REUSE);
 411         if (!tx) {
 412                 dev_err(pcdev_to_dev(pcdev),
 413                         "dmaengine_prep_slave_sg failed\n");
 414                 goto fail;
 415         }
 416
 417         tx->callback_param = pcdev;
 418         switch (channel) {
 419         case 0:
 420                 tx->callback = pxa_camera_dma_irq_y;
 421                 break;
 422         case 1:
 423                 tx->callback = pxa_camera_dma_irq_u;
 424                 break;
 425         case 2:
 426                 tx->callback = pxa_camera_dma_irq_v;
 427                 break;
 428         }
 429
 430         buf->descs[channel] = tx;
 431         return 0;
 432 fail:
 433         dev_dbg(pcdev_to_dev(pcdev),
 434                 "%s (vb=%p) dma_tx=%p\n",
 435                 __func__, buf, tx);
 436
 437         return -ENOMEM;
 438 }
 439
 440 static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
 441                                     struct pxa_buffer *buf)
 442 {
 443         buf->active_dma = DMA_Y;
 444         if (buf->nb_planes == 3)
 445                 buf->active_dma |= DMA_U | DMA_V;
 446 }
 447
 448 /**
 449  * pxa_dma_start_channels - start DMA channel for active buffer
 450  * @pcdev: pxa camera device
 451  *
 452  * Initialize DMA channels to the beginning of the active video buffer, and
 453  * start these channels.
 454  */
 455 static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
 456 {
 457         int i;
 458         struct pxa_buffer *active;
 459
 460         active = pcdev->active;
 461
 462         for (i = 0; i < pcdev->channels; i++) {
 463                 dev_dbg(pcdev_to_dev(pcdev),
 464                         "%s (channel=%d)\n", __func__, i);
 465                 dma_async_issue_pending(pcdev->dma_chans[i]);
 466         }
 467 }
 468
 469 static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
 470 {
 471         int i;
 472
 473         for (i = 0; i < pcdev->channels; i++) {
 474                 dev_dbg(pcdev_to_dev(pcdev),
 475                         "%s (channel=%d)\n", __func__, i);
 476                 dmaengine_terminate_all(pcdev->dma_chans[i]);
 477         }
 478 }
 479
 480 static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
 481                                  struct pxa_buffer *buf)
 482 {
 483         int i;
 484
 485         for (i = 0; i < pcdev->channels; i++) {
 486                 buf->cookie[i] = dmaengine_submit(buf->descs[i]);
 487                 dev_dbg(pcdev_to_dev(pcdev),
 488                         "%s (channel=%d) : submit vb=%p cookie=%d\n",
 489                         __func__, i, buf, buf->descs[i]->cookie);
 490         }
 491 }
 492
 493 /**
 494  * pxa_camera_start_capture - start video capturing
 495  * @pcdev: camera device
 496  *
 497  * Launch capturing. DMA channels should not be active yet. They should get
 498  * activated at the end of frame interrupt, to capture only whole frames, and
 499  * never begin the capture of a partial frame.
 500  */
 501 static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
 502 {
 503         unsigned long cicr0;
 504
 505         dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
 506         __raw_writel(__raw_readl(pcdev->base + CISR), pcdev->base + CISR);
 507         /* Enable End-Of-Frame Interrupt */
 508         cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
 509         cicr0 &= ~CICR0_EOFM;
 510         __raw_writel(cicr0, pcdev->base + CICR0);
 511 }
 512
 513 static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
 514 {
 515         unsigned long cicr0;
 516
 517         pxa_dma_stop_channels(pcdev);
 518
 519         cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
 520         __raw_writel(cicr0, pcdev->base + CICR0);
 521
 522         pcdev->active = NULL;
 523         dev_dbg(pcdev_to_dev(pcdev), "%s\n", __func__);
 524 }
 525
 526 static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
 527                               struct pxa_buffer *buf,
 528                               enum vb2_buffer_state state)
 529 {
 530         struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
 531         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 532
 533         /* _init is used to debug races, see comment in pxa_camera_reqbufs() */
 534         list_del_init(&buf->queue);
 535         vb->timestamp = ktime_get_ns();
 536         vbuf->sequence = pcdev->buf_sequence++;
 537         vbuf->field = V4L2_FIELD_NONE;
 538         vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 539         dev_dbg(pcdev_to_dev(pcdev), "%s dequeud buffer (buf=0x%p)\n",
 540                 __func__, buf);
 541
 542         if (list_empty(&pcdev->capture)) {
 543                 pxa_camera_stop_capture(pcdev);
 544                 return;
 545         }
 546
 547         pcdev->active = list_entry(pcdev->capture.next,
 548                                    struct pxa_buffer, queue);
 549 }
 550
 551 /**
 552  * pxa_camera_check_link_miss - check missed DMA linking
 553  * @pcdev: camera device
 554  *
 555  * The DMA chaining is done with DMA running. This means a tiny temporal window
 556  * remains, where a buffer is queued on the chain, while the chain is already
 557  * stopped. This means the tailed buffer would never be transferred by DMA.
 558  * This function restarts the capture for this corner case, where :
 559  *  - DADR() == DADDR_STOP
 560  *  - a videobuffer is queued on the pcdev->capture list
 561  *
 562  * Please check the "DMA hot chaining timeslice issue" in
 563  *   Documentation/video4linux/pxa_camera.txt
 564  *
 565  * Context: should only be called within the dma irq handler
 566  */
 567 static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev,
 568                                        dma_cookie_t last_submitted,
 569                                        dma_cookie_t last_issued)
 570 {
 571         bool is_dma_stopped = last_submitted != last_issued;
 572
 573         dev_dbg(pcdev_to_dev(pcdev),
 574                 "%s : top queued buffer=%p, is_dma_stopped=%d\n",
 575                 __func__, pcdev->active, is_dma_stopped);
 576
 577         if (pcdev->active && is_dma_stopped)
 578                 pxa_camera_start_capture(pcdev);
 579 }
 580
 581 static void pxa_camera_dma_irq(struct pxa_camera_dev *pcdev,
 582                                enum pxa_camera_active_dma act_dma)
 583 {
 584         struct pxa_buffer *buf, *last_buf;
 585         unsigned long flags;
 586         u32 camera_status, overrun;
 587         int chan;
 588         enum dma_status last_status;
 589         dma_cookie_t last_issued;
 590
 591         spin_lock_irqsave(&pcdev->lock, flags);
 592
 593         camera_status = __raw_readl(pcdev->base + CISR);
 594         dev_dbg(pcdev_to_dev(pcdev), "camera dma irq, cisr=0x%x dma=%d\n",
 595                 camera_status, act_dma);
 596         overrun = CISR_IFO_0;
 597         if (pcdev->channels == 3)
 598                 overrun |= CISR_IFO_1 | CISR_IFO_2;
 599
 600         /*
 601          * pcdev->active should not be NULL in DMA irq handler.
 602          *
 603          * But there is one corner case : if capture was stopped due to an
 604          * overrun of channel 1, and at that same channel 2 was completed.
 605          *
 606          * When handling the overrun in DMA irq for channel 1, we'll stop the
 607          * capture and restart it (and thus set pcdev->active to NULL). But the
 608          * DMA irq handler will already be pending for channel 2. So on entering
 609          * the DMA irq handler for channel 2 there will be no active buffer, yet
 610          * that is normal.
 611          */
 612         if (!pcdev->active)
 613                 goto out;
 614
 615         buf = pcdev->active;
 616         WARN_ON(buf->inwork || list_empty(&buf->queue));
 617
 618         /*
 619          * It's normal if the last frame creates an overrun, as there
 620          * are no more DMA descriptors to fetch from QCI fifos
 621          */
 622         switch (act_dma) {
 623         case DMA_U:
 624                 chan = 1;
 625                 break;
 626         case DMA_V:
 627                 chan = 2;
 628                 break;
 629         default:
 630                 chan = 0;
 631                 break;
 632         }
 633         last_buf = list_entry(pcdev->capture.prev,
 634                               struct pxa_buffer, queue);
 635         last_status = dma_async_is_tx_complete(pcdev->dma_chans[chan],
 636                                                last_buf->cookie[chan],
 637                                                NULL, &last_issued);
 638         if (camera_status & overrun &&
 639             last_status != DMA_COMPLETE) {
 640                 dev_dbg(pcdev_to_dev(pcdev), "FIFO overrun! CISR: %x\n",
 641                         camera_status);
 642                 pxa_camera_stop_capture(pcdev);
 643                 list_for_each_entry(buf, &pcdev->capture, queue)
 644                         pxa_dma_add_tail_buf(pcdev, buf);
 645                 pxa_camera_start_capture(pcdev);
 646                 goto out;
 647         }
 648         buf->active_dma &= ~act_dma;
 649         if (!buf->active_dma) {
 650                 pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_DONE);
 651                 pxa_camera_check_link_miss(pcdev, last_buf->cookie[chan],
 652                                            last_issued);
 653         }
 654
 655 out:
 656         spin_unlock_irqrestore(&pcdev->lock, flags);
 657 }
 658
 659 static u32 mclk_get_divisor(struct platform_device *pdev,
 660                             struct pxa_camera_dev *pcdev)
 661 {
 662         unsigned long mclk = pcdev->mclk;
 663         u32 div;
 664         unsigned long lcdclk;
 665
 666         lcdclk = clk_get_rate(pcdev->clk);
 667         pcdev->ciclk = lcdclk;
 668
 669         /* mclk <= ciclk / 4 (27.4.2) */
 670         if (mclk > lcdclk / 4) {
 671                 mclk = lcdclk / 4;
 672                 dev_warn(pcdev_to_dev(pcdev),
 673                          "Limiting master clock to %lu\n", mclk);
 674         }
 675
 676         /* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
 677         div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
 678
 679         /* If we're not supplying MCLK, leave it at 0 */
 680         if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 681                 pcdev->mclk = lcdclk / (2 * (div + 1));
 682
 683         dev_dbg(pcdev_to_dev(pcdev), "LCD clock %luHz, target freq %luHz, divisor %u\n",
 684                 lcdclk, mclk, div);
 685
 686         return div;
 687 }
 688
 689 static void recalculate_fifo_timeout(struct pxa_camera_dev *pcdev,
 690                                      unsigned long pclk)
 691 {
 692         /* We want a timeout > 1 pixel time, not ">=" */
 693         u32 ciclk_per_pixel = pcdev->ciclk / pclk + 1;
 694
 695         __raw_writel(ciclk_per_pixel, pcdev->base + CITOR);
 696 }
 697
 698 static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
 699 {
 700         u32 cicr4 = 0;
 701
 702         /* disable all interrupts */
 703         __raw_writel(0x3ff, pcdev->base + CICR0);
 704
 705         if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 706                 cicr4 |= CICR4_PCLK_EN;
 707         if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 708                 cicr4 |= CICR4_MCLK_EN;
 709         if (pcdev->platform_flags & PXA_CAMERA_PCP)
 710                 cicr4 |= CICR4_PCP;
 711         if (pcdev->platform_flags & PXA_CAMERA_HSP)
 712                 cicr4 |= CICR4_HSP;
 713         if (pcdev->platform_flags & PXA_CAMERA_VSP)
 714                 cicr4 |= CICR4_VSP;
 715
 716         __raw_writel(pcdev->mclk_divisor | cicr4, pcdev->base + CICR4);
 717
 718         if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 719                 /* Initialise the timeout under the assumption pclk = mclk */
 720                 recalculate_fifo_timeout(pcdev, pcdev->mclk);
 721         else
 722                 /* "Safe default" - 13MHz */
 723                 recalculate_fifo_timeout(pcdev, 13000000);
 724
 725         clk_prepare_enable(pcdev->clk);
 726 }
 727
 728 static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
 729 {
 730         clk_disable_unprepare(pcdev->clk);
 731 }
 732
 733 static void pxa_camera_eof(unsigned long arg)
 734 {
 735         struct pxa_camera_dev *pcdev = (struct pxa_camera_dev *)arg;
 736         unsigned long cifr;
 737         struct pxa_buffer *buf;
 738
 739         dev_dbg(pcdev_to_dev(pcdev),
 740                 "Camera interrupt status 0x%x\n",
 741                 __raw_readl(pcdev->base + CISR));
 742
 743         /* Reset the FIFOs */
 744         cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
 745         __raw_writel(cifr, pcdev->base + CIFR);
 746
 747         pcdev->active = list_first_entry(&pcdev->capture,
 748                                          struct pxa_buffer, queue);
 749         buf = pcdev->active;
 750         pxa_videobuf_set_actdma(pcdev, buf);
 751
 752         pxa_dma_start_channels(pcdev);
 753 }
 754
 755 static irqreturn_t pxa_camera_irq(int irq, void *data)
 756 {
 757         struct pxa_camera_dev *pcdev = data;
 758         unsigned long status, cicr0;
 759
 760         status = __raw_readl(pcdev->base + CISR);
 761         dev_dbg(pcdev_to_dev(pcdev),
 762                 "Camera interrupt status 0x%lx\n", status);
 763
 764         if (!status)
 765                 return IRQ_NONE;
 766
 767         __raw_writel(status, pcdev->base + CISR);
 768
 769         if (status & CISR_EOF) {
 770                 cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
 771                 __raw_writel(cicr0, pcdev->base + CICR0);
 772                 tasklet_schedule(&pcdev->task_eof);
 773         }
 774
 775         return IRQ_HANDLED;
 776 }
 777
 778 static int test_platform_param(struct pxa_camera_dev *pcdev,
 779                                unsigned char buswidth, unsigned long *flags)
 780 {
 781         /*
 782          * Platform specified synchronization and pixel clock polarities are
 783          * only a recommendation and are only used during probing. The PXA270
 784          * quick capture interface supports both.
 785          */
 786         *flags = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
 787                   V4L2_MBUS_MASTER : V4L2_MBUS_SLAVE) |
 788                 V4L2_MBUS_HSYNC_ACTIVE_HIGH |
 789                 V4L2_MBUS_HSYNC_ACTIVE_LOW |
 790                 V4L2_MBUS_VSYNC_ACTIVE_HIGH |
 791                 V4L2_MBUS_VSYNC_ACTIVE_LOW |
 792                 V4L2_MBUS_DATA_ACTIVE_HIGH |
 793                 V4L2_MBUS_PCLK_SAMPLE_RISING |
 794                 V4L2_MBUS_PCLK_SAMPLE_FALLING;
 795
 796         /* If requested data width is supported by the platform, use it */
 797         if ((1 << (buswidth - 1)) & pcdev->width_flags)
 798                 return 0;
 799
 800         return -EINVAL;
 801 }
 802
 803 static void pxa_camera_setup_cicr(struct pxa_camera_dev *pcdev,
 804                                   unsigned long flags, __u32 pixfmt)
 805 {
 806         unsigned long dw, bpp;
 807         u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
 808         int ret = sensor_call(pcdev, sensor, g_skip_top_lines, &y_skip_top);
 809
 810         if (ret < 0)
 811                 y_skip_top = 0;
 812
 813         /*
 814          * Datawidth is now guaranteed to be equal to one of the three values.
 815          * We fix bit-per-pixel equal to data-width...
 816          */
 817         switch (pcdev->current_fmt->host_fmt->bits_per_sample) {
 818         case 10:
 819                 dw = 4;
 820                 bpp = 0x40;
 821                 break;
 822         case 9:
 823                 dw = 3;
 824                 bpp = 0x20;
 825                 break;
 826         default:
 827                 /*
 828                  * Actually it can only be 8 now,
 829                  * default is just to silence compiler warnings
 830                  */
 831         case 8:
 832                 dw = 2;
 833                 bpp = 0;
 834         }
 835
 836         if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
 837                 cicr4 |= CICR4_PCLK_EN;
 838         if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
 839                 cicr4 |= CICR4_MCLK_EN;
 840         if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
 841                 cicr4 |= CICR4_PCP;
 842         if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
 843                 cicr4 |= CICR4_HSP;
 844         if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
 845                 cicr4 |= CICR4_VSP;
 846
 847         cicr0 = __raw_readl(pcdev->base + CICR0);
 848         if (cicr0 & CICR0_ENB)
 849                 __raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
 850
 851         cicr1 = CICR1_PPL_VAL(pcdev->current_pix.width - 1) | bpp | dw;
 852
 853         switch (pixfmt) {
 854         case V4L2_PIX_FMT_YUV422P:
 855                 pcdev->channels = 3;
 856                 cicr1 |= CICR1_YCBCR_F;
 857                 /*
 858                  * Normally, pxa bus wants as input UYVY format. We allow all
 859                  * reorderings of the YUV422 format, as no processing is done,
 860                  * and the YUV stream is just passed through without any
 861                  * transformation. Note that UYVY is the only format that
 862                  * should be used if pxa framebuffer Overlay2 is used.
 863                  */
 864         case V4L2_PIX_FMT_UYVY:
 865         case V4L2_PIX_FMT_VYUY:
 866         case V4L2_PIX_FMT_YUYV:
 867         case V4L2_PIX_FMT_YVYU:
 868                 cicr1 |= CICR1_COLOR_SP_VAL(2);
 869                 break;
 870         case V4L2_PIX_FMT_RGB555:
 871                 cicr1 |= CICR1_RGB_BPP_VAL(1) | CICR1_RGBT_CONV_VAL(2) |
 872                         CICR1_TBIT | CICR1_COLOR_SP_VAL(1);
 873                 break;
 874         case V4L2_PIX_FMT_RGB565:
 875                 cicr1 |= CICR1_COLOR_SP_VAL(1) | CICR1_RGB_BPP_VAL(2);
 876                 break;
 877         }
 878
 879         cicr2 = 0;
 880         cicr3 = CICR3_LPF_VAL(pcdev->current_pix.height - 1) |
 881                 CICR3_BFW_VAL(min((u32)255, y_skip_top));
 882         cicr4 |= pcdev->mclk_divisor;
 883
 884         __raw_writel(cicr1, pcdev->base + CICR1);
 885         __raw_writel(cicr2, pcdev->base + CICR2);
 886         __raw_writel(cicr3, pcdev->base + CICR3);
 887         __raw_writel(cicr4, pcdev->base + CICR4);
 888
 889         /* CIF interrupts are not used, only DMA */
 890         cicr0 = (cicr0 & CICR0_ENB) | (pcdev->platform_flags & PXA_CAMERA_MASTER ?
 891                 CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP));
 892         cicr0 |= CICR0_DMAEN | CICR0_IRQ_MASK;
 893         __raw_writel(cicr0, pcdev->base + CICR0);
 894 }
 895
 896 /*
 897  * Videobuf2 section
 898  */
 899 static void pxa_buffer_cleanup(struct pxa_buffer *buf)
 900 {
 901         int i;
 902
 903         for (i = 0; i < 3 && buf->descs[i]; i++) {
 904                 dmaengine_desc_free(buf->descs[i]);
 905                 kfree(buf->sg[i]);
 906                 buf->descs[i] = NULL;
 907                 buf->sg[i] = NULL;
 908                 buf->sg_len[i] = 0;
 909                 buf->plane_sizes[i] = 0;
 910         }
 911         buf->nb_planes = 0;
 912 }
 913
 914 static int pxa_buffer_init(struct pxa_camera_dev *pcdev,
 915                            struct pxa_buffer *buf)
 916 {
 917         struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
 918         struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 919         int nb_channels = pcdev->channels;
 920         int i, ret = 0;
 921         unsigned long size = vb2_plane_size(vb, 0);
 922
 923         switch (nb_channels) {
 924         case 1:
 925                 buf->plane_sizes[0] = size;
 926                 break;
 927         case 3:
 928                 buf->plane_sizes[0] = size / 2;
 929                 buf->plane_sizes[1] = size / 4;
 930                 buf->plane_sizes[2] = size / 4;
 931                 break;
 932         default:
 933                 return -EINVAL;
 934         };
 935         buf->nb_planes = nb_channels;
 936
 937         ret = sg_split(sgt->sgl, sgt->nents, 0, nb_channels,
 938                        buf->plane_sizes, buf->sg, buf->sg_len, GFP_KERNEL);
 939         if (ret < 0) {
 940                 dev_err(pcdev_to_dev(pcdev),
 941                         "sg_split failed: %d\n", ret);
 942                 return ret;
 943         }
 944         for (i = 0; i < nb_channels; i++) {
 945                 ret = pxa_init_dma_channel(pcdev, buf, i,
 946                                            buf->sg[i], buf->sg_len[i]);
 947                 if (ret) {
 948                         pxa_buffer_cleanup(buf);
 949                         return ret;
 950                 }
 951         }
 952         INIT_LIST_HEAD(&buf->queue);
 953
 954         return ret;
 955 }
 956
 957 static void pxac_vb2_cleanup(struct vb2_buffer *vb)
 958 {
 959         struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
 960         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
 961
 962         dev_dbg(pcdev_to_dev(pcdev),
 963                  "%s(vb=%p)\n", __func__, vb);
 964         pxa_buffer_cleanup(buf);
 965 }
 966
 967 static void pxac_vb2_queue(struct vb2_buffer *vb)
 968 {
 969         struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
 970         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
 971
 972         dev_dbg(pcdev_to_dev(pcdev),
 973                  "%s(vb=%p) nb_channels=%d size=%lu active=%p\n",
 974                 __func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0),
 975                 pcdev->active);
 976
 977         list_add_tail(&buf->queue, &pcdev->capture);
 978
 979         pxa_dma_add_tail_buf(pcdev, buf);
 980 }
 981
 982 /*
 983  * Please check the DMA prepared buffer structure in :
 984  *   Documentation/video4linux/pxa_camera.txt
 985  * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
 986  * modification while DMA chain is running will work anyway.
 987  */
 988 static int pxac_vb2_prepare(struct vb2_buffer *vb)
 989 {
 990         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
 991         struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
 992         int ret = 0;
 993
 994         switch (pcdev->channels) {
 995         case 1:
 996         case 3:
 997                 vb2_set_plane_payload(vb, 0, pcdev->current_pix.sizeimage);
 998                 break;
 999         default:
1000                 return -EINVAL;
1001         }
1002
1003         dev_dbg(pcdev_to_dev(pcdev),
1004                  "%s (vb=%p) nb_channels=%d size=%lu\n",
1005                 __func__, vb, pcdev->channels, vb2_get_plane_payload(vb, 0));
1006
1007         WARN_ON(!pcdev->current_fmt);
1008
1009 #ifdef DEBUG
1010         /*
1011          * This can be useful if you want to see if we actually fill
1012          * the buffer with something
1013          */
1014         for (i = 0; i < vb->num_planes; i++)
1015                 memset((void *)vb2_plane_vaddr(vb, i),
1016                        0xaa, vb2_get_plane_payload(vb, i));
1017 #endif
1018
1019         /*
1020          * I think, in buf_prepare you only have to protect global data,
1021          * the actual buffer is yours
1022          */
1023         buf->inwork = 0;
1024         pxa_videobuf_set_actdma(pcdev, buf);
1025
1026         return ret;
1027 }
1028
1029 static int pxac_vb2_init(struct vb2_buffer *vb)
1030 {
1031         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vb->vb2_queue);
1032         struct pxa_buffer *buf = vb2_to_pxa_buffer(vb);
1033
1034         dev_dbg(pcdev_to_dev(pcdev),
1035                  "%s(nb_channels=%d)\n",
1036                 __func__, pcdev->channels);
1037
1038         return pxa_buffer_init(pcdev, buf);
1039 }
1040
1041 static int pxac_vb2_queue_setup(struct vb2_queue *vq,
1042                                 unsigned int *nbufs,
1043                                 unsigned int *num_planes, unsigned int sizes[],
1044                                 struct device *alloc_devs[])
1045 {
1046         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
1047         int size = pcdev->current_pix.sizeimage;
1048
1049         dev_dbg(pcdev_to_dev(pcdev),
1050                  "%s(vq=%p nbufs=%d num_planes=%d size=%d)\n",
1051                 __func__, vq, *nbufs, *num_planes, size);
1052         /*
1053          * Called from VIDIOC_REQBUFS or in compatibility mode For YUV422P
1054          * format, even if there are 3 planes Y, U and V, we reply there is only
1055          * one plane, containing Y, U and V data, one after the other.
1056          */
1057         if (*num_planes)
1058                 return sizes[0] < size ? -EINVAL : 0;
1059
1060         *num_planes = 1;
1061         switch (pcdev->channels) {
1062         case 1:
1063         case 3:
1064                 sizes[0] = size;
1065                 break;
1066         default:
1067                 return -EINVAL;
1068         }
1069
1070         if (!*nbufs)
1071                 *nbufs = 1;
1072
1073         return 0;
1074 }
1075
1076 static int pxac_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
1077 {
1078         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
1079
1080         dev_dbg(pcdev_to_dev(pcdev), "%s(count=%d) active=%p\n",
1081                 __func__, count, pcdev->active);
1082
1083         pcdev->buf_sequence = 0;
1084         if (!pcdev->active)
1085                 pxa_camera_start_capture(pcdev);
1086
1087         return 0;
1088 }
1089
1090 static void pxac_vb2_stop_streaming(struct vb2_queue *vq)
1091 {
1092         struct pxa_camera_dev *pcdev = vb2_get_drv_priv(vq);
1093         struct pxa_buffer *buf, *tmp;
1094
1095         dev_dbg(pcdev_to_dev(pcdev), "%s active=%p\n",
1096                 __func__, pcdev->active);
1097         pxa_camera_stop_capture(pcdev);
1098
1099         list_for_each_entry_safe(buf, tmp, &pcdev->capture, queue)
1100                 pxa_camera_wakeup(pcdev, buf, VB2_BUF_STATE_ERROR);
1101 }
1102
1103 static struct vb2_ops pxac_vb2_ops = {
1104         .queue_setup            = pxac_vb2_queue_setup,
1105         .buf_init               = pxac_vb2_init,
1106         .buf_prepare            = pxac_vb2_prepare,
1107         .buf_queue              = pxac_vb2_queue,
1108         .buf_cleanup            = pxac_vb2_cleanup,
1109         .start_streaming        = pxac_vb2_start_streaming,
1110         .stop_streaming         = pxac_vb2_stop_streaming,
1111         .wait_prepare           = vb2_ops_wait_prepare,
1112         .wait_finish            = vb2_ops_wait_finish,
1113 };
1114
1115 static int pxa_camera_init_videobuf2(struct pxa_camera_dev *pcdev)
1116 {
1117         int ret;
1118         struct vb2_queue *vq = &pcdev->vb2_vq;
1119
1120         memset(vq, 0, sizeof(*vq));
1121         vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1122         vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1123         vq->drv_priv = pcdev;
1124         vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1125         vq->buf_struct_size = sizeof(struct pxa_buffer);
1126         vq->dev = pcdev->v4l2_dev.dev;
1127
1128         vq->ops = &pxac_vb2_ops;
1129         vq->mem_ops = &vb2_dma_sg_memops;
1130         vq->lock = &pcdev->mlock;
1131
1132         ret = vb2_queue_init(vq);
1133         dev_dbg(pcdev_to_dev(pcdev),
1134                  "vb2_queue_init(vq=%p): %d\n", vq, ret);
1135
1136         return ret;
1137 }
1138
1139 /*
1140  * Video ioctls section
1141  */
1142 static int pxa_camera_set_bus_param(struct pxa_camera_dev *pcdev)
1143 {
1144         struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
1145         u32 pixfmt = pcdev->current_fmt->host_fmt->fourcc;
1146         unsigned long bus_flags, common_flags;
1147         int ret;
1148
1149         ret = test_platform_param(pcdev,
1150                                   pcdev->current_fmt->host_fmt->bits_per_sample,
1151                                   &bus_flags);
1152         if (ret < 0)
1153                 return ret;
1154
1155         ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
1156         if (!ret) {
1157                 common_flags = soc_mbus_config_compatible(&cfg,
1158                                                           bus_flags);
1159                 if (!common_flags) {
1160                         dev_warn(pcdev_to_dev(pcdev),
1161                                  "Flags incompatible: camera 0x%x, host 0x%lx\n",
1162                                  cfg.flags, bus_flags);
1163                         return -EINVAL;
1164                 }
1165         } else if (ret != -ENOIOCTLCMD) {
1166                 return ret;
1167         } else {
1168                 common_flags = bus_flags;
1169         }
1170
1171         pcdev->channels = 1;
1172
1173         /* Make choises, based on platform preferences */
1174         if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
1175             (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
1176                 if (pcdev->platform_flags & PXA_CAMERA_HSP)
1177                         common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
1178                 else
1179                         common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
1180         }
1181
1182         if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
1183             (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
1184                 if (pcdev->platform_flags & PXA_CAMERA_VSP)
1185                         common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
1186                 else
1187                         common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
1188         }
1189
1190         if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
1191             (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
1192                 if (pcdev->platform_flags & PXA_CAMERA_PCP)
1193                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
1194                 else
1195                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
1196         }
1197
1198         cfg.flags = common_flags;
1199         ret = sensor_call(pcdev, video, s_mbus_config, &cfg);
1200         if (ret < 0 && ret != -ENOIOCTLCMD) {
1201                 dev_dbg(pcdev_to_dev(pcdev),
1202                         "camera s_mbus_config(0x%lx) returned %d\n",
1203                         common_flags, ret);
1204                 return ret;
1205         }
1206
1207         pxa_camera_setup_cicr(pcdev, common_flags, pixfmt);
1208
1209         return 0;
1210 }
1211
1212 static int pxa_camera_try_bus_param(struct pxa_camera_dev *pcdev,
1213                                     unsigned char buswidth)
1214 {
1215         struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
1216         unsigned long bus_flags, common_flags;
1217         int ret = test_platform_param(pcdev, buswidth, &bus_flags);
1218
1219         if (ret < 0)
1220                 return ret;
1221
1222         ret = sensor_call(pcdev, video, g_mbus_config, &cfg);
1223         if (!ret) {
1224                 common_flags = soc_mbus_config_compatible(&cfg,
1225                                                           bus_flags);
1226                 if (!common_flags) {
1227                         dev_warn(pcdev_to_dev(pcdev),
1228                                  "Flags incompatible: camera 0x%x, host 0x%lx\n",
1229                                  cfg.flags, bus_flags);
1230                         return -EINVAL;
1231                 }
1232         } else if (ret == -ENOIOCTLCMD) {
1233                 ret = 0;
1234         }
1235
1236         return ret;
1237 }
1238
1239 static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
1240         {
1241                 .fourcc                 = V4L2_PIX_FMT_YUV422P,
1242                 .name                   = "Planar YUV422 16 bit",
1243                 .bits_per_sample        = 8,
1244                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1245                 .order                  = SOC_MBUS_ORDER_LE,
1246                 .layout                 = SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
1247         },
1248 };
1249
1250 /* This will be corrected as we get more formats */
1251 static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
1252 {
1253         return  fmt->packing == SOC_MBUS_PACKING_NONE ||
1254                 (fmt->bits_per_sample == 8 &&
1255                  fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
1256                 (fmt->bits_per_sample > 8 &&
1257                  fmt->packing == SOC_MBUS_PACKING_EXTEND16);
1258 }
1259
1260 static int pxa_camera_get_formats(struct v4l2_device *v4l2_dev,
1261                                   unsigned int idx,
1262                                   struct soc_camera_format_xlate *xlate)
1263 {
1264         struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
1265         int formats = 0, ret;
1266         struct v4l2_subdev_mbus_code_enum code = {
1267                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1268                 .index = idx,
1269         };
1270         const struct soc_mbus_pixelfmt *fmt;
1271
1272         ret = sensor_call(pcdev, pad, enum_mbus_code, NULL, &code);
1273         if (ret < 0)
1274                 /* No more formats */
1275                 return 0;
1276
1277         fmt = soc_mbus_get_fmtdesc(code.code);
1278         if (!fmt) {
1279                 dev_err(pcdev_to_dev(pcdev),
1280                         "Invalid format code #%u: %d\n", idx, code.code);
1281                 return 0;
1282         }
1283
1284         /* This also checks support for the requested bits-per-sample */
1285         ret = pxa_camera_try_bus_param(pcdev, fmt->bits_per_sample);
1286         if (ret < 0)
1287                 return 0;
1288
1289         switch (code.code) {
1290         case MEDIA_BUS_FMT_UYVY8_2X8:
1291                 formats++;
1292                 if (xlate) {
1293                         xlate->host_fmt = &pxa_camera_formats[0];
1294                         xlate->code     = code.code;
1295                         xlate++;
1296                         dev_dbg(pcdev_to_dev(pcdev),
1297                                 "Providing format %s using code %d\n",
1298                                 pxa_camera_formats[0].name, code.code);
1299                 }
1300         /* fall through */
1301         case MEDIA_BUS_FMT_VYUY8_2X8:
1302         case MEDIA_BUS_FMT_YUYV8_2X8:
1303         case MEDIA_BUS_FMT_YVYU8_2X8:
1304         case MEDIA_BUS_FMT_RGB565_2X8_LE:
1305         case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
1306                 if (xlate)
1307                         dev_dbg(pcdev_to_dev(pcdev),
1308                                 "Providing format %s packed\n",
1309                                 fmt->name);
1310                 break;
1311         default:
1312                 if (!pxa_camera_packing_supported(fmt))
1313                         return 0;
1314                 if (xlate)
1315                         dev_dbg(pcdev_to_dev(pcdev),
1316                                 "Providing format %s in pass-through mode\n",
1317                                 fmt->name);
1318                 break;
1319         }
1320
1321         /* Generic pass-through */
1322         formats++;
1323         if (xlate) {
1324                 xlate->host_fmt = fmt;
1325                 xlate->code     = code.code;
1326                 xlate++;
1327         }
1328
1329         return formats;
1330 }
1331
1332 static int pxa_camera_build_formats(struct pxa_camera_dev *pcdev)
1333 {
1334         struct soc_camera_format_xlate *xlate;
1335
1336         xlate = soc_mbus_build_fmts_xlate(&pcdev->v4l2_dev, pcdev->sensor,
1337                                           pxa_camera_get_formats);
1338         if (IS_ERR(xlate))
1339                 return PTR_ERR(xlate);
1340
1341         pcdev->user_formats = xlate;
1342         return 0;
1343 }
1344
1345 static void pxa_camera_destroy_formats(struct pxa_camera_dev *pcdev)
1346 {
1347         kfree(pcdev->user_formats);
1348 }
1349
1350 static int pxa_camera_check_frame(u32 width, u32 height)
1351 {
1352         /* limit to pxa hardware capabilities */
1353         return height < 32 || height > 2048 || width < 48 || width > 2048 ||
1354                 (width & 0x01);
1355 }
1356
1357 #ifdef CONFIG_VIDEO_ADV_DEBUG
1358 static int pxac_vidioc_g_register(struct file *file, void *priv,
1359                                   struct v4l2_dbg_register *reg)
1360 {
1361         struct pxa_camera_dev *pcdev = video_drvdata(file);
1362
1363         if (reg->reg > CIBR2)
1364                 return -ERANGE;
1365
1366         reg->val = __raw_readl(pcdev->base + reg->reg);
1367         reg->size = sizeof(__u32);
1368         return 0;
1369 }
1370
1371 static int pxac_vidioc_s_register(struct file *file, void *priv,
1372                                   const struct v4l2_dbg_register *reg)
1373 {
1374         struct pxa_camera_dev *pcdev = video_drvdata(file);
1375
1376         if (reg->reg > CIBR2)
1377                 return -ERANGE;
1378         if (reg->size != sizeof(__u32))
1379                 return -EINVAL;
1380         __raw_writel(reg->val, pcdev->base + reg->reg);
1381         return 0;
1382 }
1383 #endif
1384
1385 static int pxac_vidioc_enum_fmt_vid_cap(struct file *filp, void  *priv,
1386                                         struct v4l2_fmtdesc *f)
1387 {
1388         struct pxa_camera_dev *pcdev = video_drvdata(filp);
1389         const struct soc_mbus_pixelfmt *format;
1390         unsigned int idx;
1391
1392         for (idx = 0; pcdev->user_formats[idx].code; idx++);
1393         if (f->index >= idx)
1394                 return -EINVAL;
1395
1396         format = pcdev->user_formats[f->index].host_fmt;
1397         f->pixelformat = format->fourcc;
1398         return 0;
1399 }
1400
1401 static int pxac_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
1402                                     struct v4l2_format *f)
1403 {
1404         struct pxa_camera_dev *pcdev = video_drvdata(filp);
1405         struct v4l2_pix_format *pix = &f->fmt.pix;
1406
1407         pix->width              = pcdev->current_pix.width;
1408         pix->height             = pcdev->current_pix.height;
1409         pix->bytesperline       = pcdev->current_pix.bytesperline;
1410         pix->sizeimage          = pcdev->current_pix.sizeimage;
1411         pix->field              = pcdev->current_pix.field;
1412         pix->pixelformat        = pcdev->current_fmt->host_fmt->fourcc;
1413         pix->colorspace         = pcdev->current_pix.colorspace;
1414         dev_dbg(pcdev_to_dev(pcdev), "current_fmt->fourcc: 0x%08x\n",
1415                 pcdev->current_fmt->host_fmt->fourcc);
1416         return 0;
1417 }
1418
1419 static int pxac_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
1420                                       struct v4l2_format *f)
1421 {
1422         struct pxa_camera_dev *pcdev = video_drvdata(filp);
1423         const struct soc_camera_format_xlate *xlate;
1424         struct v4l2_pix_format *pix = &f->fmt.pix;
1425         struct v4l2_subdev_pad_config pad_cfg;
1426         struct v4l2_subdev_format format = {
1427                 .which = V4L2_SUBDEV_FORMAT_TRY,
1428         };
1429         struct v4l2_mbus_framefmt *mf = &format.format;
1430         __u32 pixfmt = pix->pixelformat;
1431         int ret;
1432
1433         xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats, pixfmt);
1434         if (!xlate) {
1435                 dev_warn(pcdev_to_dev(pcdev), "Format %x not found\n", pixfmt);
1436                 return -EINVAL;
1437         }
1438
1439         /*
1440          * Limit to pxa hardware capabilities.  YUV422P planar format requires
1441          * images size to be a multiple of 16 bytes.  If not, zeros will be
1442          * inserted between Y and U planes, and U and V planes, which violates
1443          * the YUV422P standard.
1444          */
1445         v4l_bound_align_image(&pix->width, 48, 2048, 1,
1446                               &pix->height, 32, 2048, 0,
1447                               pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
1448
1449         v4l2_fill_mbus_format(mf, pix, xlate->code);
1450         ret = sensor_call(pcdev, pad, set_fmt, &pad_cfg, &format);
1451         if (ret < 0)
1452                 return ret;
1453
1454         v4l2_fill_pix_format(pix, mf);
1455
1456         /* Only progressive video supported so far */
1457         switch (mf->field) {
1458         case V4L2_FIELD_ANY:
1459         case V4L2_FIELD_NONE:
1460                 pix->field = V4L2_FIELD_NONE;
1461                 break;
1462         default:
1463                 /* TODO: support interlaced at least in pass-through mode */
1464                 dev_err(pcdev_to_dev(pcdev), "Field type %d unsupported.\n",
1465                         mf->field);
1466                 return -EINVAL;
1467         }
1468
1469         ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
1470         if (ret < 0)
1471                 return ret;
1472
1473         pix->bytesperline = ret;
1474         ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
1475                                   pix->height);
1476         if (ret < 0)
1477                 return ret;
1478
1479         pix->sizeimage = ret;
1480         return 0;
1481 }
1482
1483 static int pxac_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
1484                                     struct v4l2_format *f)
1485 {
1486         struct pxa_camera_dev *pcdev = video_drvdata(filp);
1487         const struct soc_camera_format_xlate *xlate;
1488         struct v4l2_pix_format *pix = &f->fmt.pix;
1489         struct v4l2_subdev_format format = {
1490                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1491         };
1492         unsigned long flags;
1493         int ret, is_busy;
1494
1495         dev_dbg(pcdev_to_dev(pcdev),
1496                 "s_fmt_vid_cap(pix=%dx%d:%x)\n",
1497                 pix->width, pix->height, pix->pixelformat);
1498
1499         spin_lock_irqsave(&pcdev->lock, flags);
1500         is_busy = pcdev->active || vb2_is_busy(&pcdev->vb2_vq);
1501         spin_unlock_irqrestore(&pcdev->lock, flags);
1502
1503         if (is_busy)
1504                 return -EBUSY;
1505
1506         ret = pxac_vidioc_try_fmt_vid_cap(filp, priv, f);
1507         if (ret)
1508                 return ret;
1509
1510         xlate = soc_mbus_xlate_by_fourcc(pcdev->user_formats,
1511                                          pix->pixelformat);
1512         v4l2_fill_mbus_format(&format.format, pix, xlate->code);
1513         ret = sensor_call(pcdev, pad, set_fmt, NULL, &format);
1514         if (ret < 0) {
1515                 dev_warn(pcdev_to_dev(pcdev),
1516                          "Failed to configure for format %x\n",
1517                          pix->pixelformat);
1518         } else if (pxa_camera_check_frame(pix->width, pix->height)) {
1519                 dev_warn(pcdev_to_dev(pcdev),
1520                          "Camera driver produced an unsupported frame %dx%d\n",
1521                          pix->width, pix->height);
1522                 return -EINVAL;
1523         }
1524
1525         pcdev->current_fmt = xlate;
1526         pcdev->current_pix = *pix;
1527
1528         ret = pxa_camera_set_bus_param(pcdev);
1529         return ret;
1530 }
1531
1532 static int pxac_vidioc_querycap(struct file *file, void *priv,
1533                                 struct v4l2_capability *cap)
1534 {
1535         strlcpy(cap->bus_info, "platform:pxa-camera", sizeof(cap->bus_info));
1536         strlcpy(cap->driver, PXA_CAM_DRV_NAME, sizeof(cap->driver));
1537         strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
1538         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1539         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1540
1541         return 0;
1542 }
1543
1544 static int pxac_vidioc_enum_input(struct file *file, void *priv,
1545                                   struct v4l2_input *i)
1546 {
1547         if (i->index > 0)
1548                 return -EINVAL;
1549
1550         i->type = V4L2_INPUT_TYPE_CAMERA;
1551         strlcpy(i->name, "Camera", sizeof(i->name));
1552
1553         return 0;
1554 }
1555
1556 static int pxac_vidioc_g_input(struct file *file, void *priv, unsigned int *i)
1557 {
1558         *i = 0;
1559
1560         return 0;
1561 }
1562
1563 static int pxac_vidioc_s_input(struct file *file, void *priv, unsigned int i)
1564 {
1565         if (i > 0)
1566                 return -EINVAL;
1567
1568         return 0;
1569 }
1570
1571 static int pxac_fops_camera_open(struct file *filp)
1572 {
1573         struct pxa_camera_dev *pcdev = video_drvdata(filp);
1574         int ret;
1575
1576         mutex_lock(&pcdev->mlock);
1577         ret = v4l2_fh_open(filp);
1578         if (ret < 0)
1579                 goto out;
1580
1581         ret = sensor_call(pcdev, core, s_power, 1);
1582         if (ret)
1583                 v4l2_fh_release(filp);
1584 out:
1585         mutex_unlock(&pcdev->mlock);
1586         return ret;
1587 }
1588
1589 static int pxac_fops_camera_release(struct file *filp)
1590 {
1591         struct pxa_camera_dev *pcdev = video_drvdata(filp);
1592         int ret;
1593
1594         ret = vb2_fop_release(filp);
1595         if (ret < 0)
1596                 return ret;
1597
1598         mutex_lock(&pcdev->mlock);
1599         ret = sensor_call(pcdev, core, s_power, 0);
1600         mutex_unlock(&pcdev->mlock);
1601
1602         return ret;
1603 }
1604
1605 static const struct v4l2_file_operations pxa_camera_fops = {
1606         .owner          = THIS_MODULE,
1607         .open           = pxac_fops_camera_open,
1608         .release        = pxac_fops_camera_release,
1609         .read           = vb2_fop_read,
1610         .poll           = vb2_fop_poll,
1611         .mmap           = vb2_fop_mmap,
1612         .unlocked_ioctl = video_ioctl2,
1613 };
1614
1615 static const struct v4l2_ioctl_ops pxa_camera_ioctl_ops = {
1616         .vidioc_querycap                = pxac_vidioc_querycap,
1617
1618         .vidioc_enum_input              = pxac_vidioc_enum_input,
1619         .vidioc_g_input                 = pxac_vidioc_g_input,
1620         .vidioc_s_input                 = pxac_vidioc_s_input,
1621
1622         .vidioc_enum_fmt_vid_cap        = pxac_vidioc_enum_fmt_vid_cap,
1623         .vidioc_g_fmt_vid_cap           = pxac_vidioc_g_fmt_vid_cap,
1624         .vidioc_s_fmt_vid_cap           = pxac_vidioc_s_fmt_vid_cap,
1625         .vidioc_try_fmt_vid_cap         = pxac_vidioc_try_fmt_vid_cap,
1626
1627         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1628         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1629         .vidioc_querybuf                = vb2_ioctl_querybuf,
1630         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1631         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1632         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1633         .vidioc_streamon                = vb2_ioctl_streamon,
1634         .vidioc_streamoff               = vb2_ioctl_streamoff,
1635 #ifdef CONFIG_VIDEO_ADV_DEBUG
1636         .vidioc_g_register              = pxac_vidioc_g_register,
1637         .vidioc_s_register              = pxac_vidioc_s_register,
1638 #endif
1639 };
1640
1641 static struct v4l2_clk_ops pxa_camera_mclk_ops = {
1642 };
1643
1644 static const struct video_device pxa_camera_videodev_template = {
1645         .name = "pxa-camera",
1646         .minor = -1,
1647         .fops = &pxa_camera_fops,
1648         .ioctl_ops = &pxa_camera_ioctl_ops,
1649         .release = video_device_release_empty,
1650         .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
1651 };
1652
1653 static int pxa_camera_sensor_bound(struct v4l2_async_notifier *notifier,
1654                      struct v4l2_subdev *subdev,
1655                      struct v4l2_async_subdev *asd)
1656 {
1657         int err;
1658         struct v4l2_device *v4l2_dev = notifier->v4l2_dev;
1659         struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(v4l2_dev);
1660         struct video_device *vdev = &pcdev->vdev;
1661         struct v4l2_pix_format *pix = &pcdev->current_pix;
1662         struct v4l2_subdev_format format = {
1663                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1664         };
1665         struct v4l2_mbus_framefmt *mf = &format.format;
1666
1667         dev_info(pcdev_to_dev(pcdev), "%s(): trying to bind a device\n",
1668                  __func__);
1669         mutex_lock(&pcdev->mlock);
1670         *vdev = pxa_camera_videodev_template;
1671         vdev->v4l2_dev = v4l2_dev;
1672         vdev->lock = &pcdev->mlock;
1673         pcdev->sensor = subdev;
1674         pcdev->vdev.queue = &pcdev->vb2_vq;
1675         pcdev->vdev.v4l2_dev = &pcdev->v4l2_dev;
1676         pcdev->vdev.ctrl_handler = subdev->ctrl_handler;
1677         video_set_drvdata(&pcdev->vdev, pcdev);
1678
1679         err = pxa_camera_build_formats(pcdev);
1680         if (err) {
1681                 dev_err(pcdev_to_dev(pcdev), "building formats failed: %d\n",
1682                         err);
1683                 goto out;
1684         }
1685
1686         pcdev->current_fmt = pcdev->user_formats;
1687         pix->field = V4L2_FIELD_NONE;
1688         pix->width = DEFAULT_WIDTH;
1689         pix->height = DEFAULT_HEIGHT;
1690         pix->bytesperline =
1691                 soc_mbus_bytes_per_line(pix->width,
1692                                         pcdev->current_fmt->host_fmt);
1693         pix->sizeimage =
1694                 soc_mbus_image_size(pcdev->current_fmt->host_fmt,
1695                                     pix->bytesperline, pix->height);
1696         pix->pixelformat = pcdev->current_fmt->host_fmt->fourcc;
1697         v4l2_fill_mbus_format(mf, pix, pcdev->current_fmt->code);
1698         err = sensor_call(pcdev, pad, set_fmt, NULL, &format);
1699         if (err)
1700                 goto out;
1701
1702         v4l2_fill_pix_format(pix, mf);
1703         pr_info("%s(): colorspace=0x%x pixfmt=0x%x\n",
1704                 __func__, pix->colorspace, pix->pixelformat);
1705
1706         err = pxa_camera_init_videobuf2(pcdev);
1707         if (err)
1708                 goto out;
1709
1710         err = video_register_device(&pcdev->vdev, VFL_TYPE_GRABBER, -1);
1711         if (err) {
1712                 v4l2_err(v4l2_dev, "register video device failed: %d\n", err);
1713                 pcdev->sensor = NULL;
1714         } else {
1715                 dev_info(pcdev_to_dev(pcdev),
1716                          "PXA Camera driver attached to camera %s\n",
1717                          subdev->name);
1718         }
1719 out:
1720         mutex_unlock(&pcdev->mlock);
1721         return err;
1722 }
1723
1724 static void pxa_camera_sensor_unbind(struct v4l2_async_notifier *notifier,
1725                      struct v4l2_subdev *subdev,
1726                      struct v4l2_async_subdev *asd)
1727 {
1728         struct pxa_camera_dev *pcdev = v4l2_dev_to_pcdev(notifier->v4l2_dev);
1729
1730         mutex_lock(&pcdev->mlock);
1731         dev_info(pcdev_to_dev(pcdev),
1732                  "PXA Camera driver detached from camera %s\n",
1733                  subdev->name);
1734
1735         /* disable capture, disable interrupts */
1736         __raw_writel(0x3ff, pcdev->base + CICR0);
1737
1738         /* Stop DMA engine */
1739         pxa_dma_stop_channels(pcdev);
1740
1741         pxa_camera_destroy_formats(pcdev);
1742         video_unregister_device(&pcdev->vdev);
1743         pcdev->sensor = NULL;
1744
1745         mutex_unlock(&pcdev->mlock);
1746 }
1747
1748 /*
1749  * Driver probe, remove, suspend and resume operations
1750  */
1751 static int pxa_camera_suspend(struct device *dev)
1752 {
1753         struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
1754         int i = 0, ret = 0;
1755
1756         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
1757         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR1);
1758         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR2);
1759         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
1760         pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
1761
1762         if (pcdev->sensor) {
1763                 ret = sensor_call(pcdev, core, s_power, 0);
1764                 if (ret == -ENOIOCTLCMD)
1765                         ret = 0;
1766         }
1767
1768         return ret;
1769 }
1770
1771 static int pxa_camera_resume(struct device *dev)
1772 {
1773         struct pxa_camera_dev *pcdev = dev_get_drvdata(dev);
1774         int i = 0, ret = 0;
1775
1776         __raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
1777         __raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR1);
1778         __raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR2);
1779         __raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
1780         __raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
1781
1782         if (pcdev->sensor) {
1783                 ret = sensor_call(pcdev, core, s_power, 1);
1784                 if (ret == -ENOIOCTLCMD)
1785                         ret = 0;
1786         }
1787
1788         /* Restart frame capture if active buffer exists */
1789         if (!ret && pcdev->active)
1790                 pxa_camera_start_capture(pcdev);
1791
1792         return ret;
1793 }
1794
1795 static int pxa_camera_pdata_from_dt(struct device *dev,
1796                                     struct pxa_camera_dev *pcdev,
1797                                     struct v4l2_async_subdev *asd)
1798 {
1799         u32 mclk_rate;
1800         struct device_node *remote, *np = dev->of_node;
1801         struct v4l2_of_endpoint ep;
1802         int err = of_property_read_u32(np, "clock-frequency",
1803                                        &mclk_rate);
1804         if (!err) {
1805                 pcdev->platform_flags |= PXA_CAMERA_MCLK_EN;
1806                 pcdev->mclk = mclk_rate;
1807         }
1808
1809         np = of_graph_get_next_endpoint(np, NULL);
1810         if (!np) {
1811                 dev_err(dev, "could not find endpoint\n");
1812                 return -EINVAL;
1813         }
1814
1815         err = v4l2_of_parse_endpoint(np, &ep);
1816         if (err) {
1817                 dev_err(dev, "could not parse endpoint\n");
1818                 goto out;
1819         }
1820
1821         switch (ep.bus.parallel.bus_width) {
1822         case 4:
1823                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_4;
1824                 break;
1825         case 5:
1826                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_5;
1827                 break;
1828         case 8:
1829                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_8;
1830                 break;
1831         case 9:
1832                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_9;
1833                 break;
1834         case 10:
1835                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
1836                 break;
1837         default:
1838                 break;
1839         }
1840
1841         if (ep.bus.parallel.flags & V4L2_MBUS_MASTER)
1842                 pcdev->platform_flags |= PXA_CAMERA_MASTER;
1843         if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
1844                 pcdev->platform_flags |= PXA_CAMERA_HSP;
1845         if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
1846                 pcdev->platform_flags |= PXA_CAMERA_VSP;
1847         if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
1848                 pcdev->platform_flags |= PXA_CAMERA_PCLK_EN | PXA_CAMERA_PCP;
1849         if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
1850                 pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
1851
1852         asd->match_type = V4L2_ASYNC_MATCH_OF;
1853         remote = of_graph_get_remote_port(np);
1854         if (remote) {
1855                 asd->match.of.node = remote;
1856                 of_node_put(remote);
1857         } else {
1858                 dev_notice(dev, "no remote for %s\n", of_node_full_name(np));
1859         }
1860
1861 out:
1862         of_node_put(np);
1863
1864         return err;
1865 }
1866
1867 static int pxa_camera_probe(struct platform_device *pdev)
1868 {
1869         struct pxa_camera_dev *pcdev;
1870         struct resource *res;
1871         void __iomem *base;
1872         struct dma_slave_config config = {
1873                 .src_addr_width = 0,
1874                 .src_maxburst = 8,
1875                 .direction = DMA_DEV_TO_MEM,
1876         };
1877         dma_cap_mask_t mask;
1878         struct pxad_param params;
1879         char clk_name[V4L2_CLK_NAME_SIZE];
1880         int irq;
1881         int err = 0, i;
1882
1883         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1884         irq = platform_get_irq(pdev, 0);
1885         if (!res || irq < 0)
1886                 return -ENODEV;
1887
1888         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
1889         if (!pcdev) {
1890                 dev_err(&pdev->dev, "Could not allocate pcdev\n");
1891                 return -ENOMEM;
1892         }
1893
1894         pcdev->clk = devm_clk_get(&pdev->dev, NULL);
1895         if (IS_ERR(pcdev->clk))
1896                 return PTR_ERR(pcdev->clk);
1897
1898         pcdev->res = res;
1899
1900         pcdev->pdata = pdev->dev.platform_data;
1901         if (&pdev->dev.of_node && !pcdev->pdata) {
1902                 err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev, &pcdev->asd);
1903         } else {
1904                 pcdev->platform_flags = pcdev->pdata->flags;
1905                 pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
1906                 pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
1907                 pcdev->asd.match.i2c.adapter_id =
1908                         pcdev->pdata->sensor_i2c_adapter_id;
1909                 pcdev->asd.match.i2c.address = pcdev->pdata->sensor_i2c_address;
1910         }
1911         if (err < 0)
1912                 return err;
1913
1914         if (!(pcdev->platform_flags & (PXA_CAMERA_DATAWIDTH_8 |
1915                         PXA_CAMERA_DATAWIDTH_9 | PXA_CAMERA_DATAWIDTH_10))) {
1916                 /*
1917                  * Platform hasn't set available data widths. This is bad.
1918                  * Warn and use a default.
1919                  */
1920                 dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
1921                          "data widths, using default 10 bit\n");
1922                 pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
1923         }
1924         if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8)
1925                 pcdev->width_flags = 1 << 7;
1926         if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9)
1927                 pcdev->width_flags |= 1 << 8;
1928         if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10)
1929                 pcdev->width_flags |= 1 << 9;
1930         if (!pcdev->mclk) {
1931                 dev_warn(&pdev->dev,
1932                          "mclk == 0! Please, fix your platform data. "
1933                          "Using default 20MHz\n");
1934                 pcdev->mclk = 20000000;
1935         }
1936
1937         pcdev->mclk_divisor = mclk_get_divisor(pdev, pcdev);
1938
1939         INIT_LIST_HEAD(&pcdev->capture);
1940         spin_lock_init(&pcdev->lock);
1941         mutex_init(&pcdev->mlock);
1942
1943         /*
1944          * Request the regions.
1945          */
1946         base = devm_ioremap_resource(&pdev->dev, res);
1947         if (IS_ERR(base))
1948                 return PTR_ERR(base);
1949
1950         pcdev->irq = irq;
1951         pcdev->base = base;
1952
1953         /* request dma */
1954         dma_cap_zero(mask);
1955         dma_cap_set(DMA_SLAVE, mask);
1956         dma_cap_set(DMA_PRIVATE, mask);
1957
1958         params.prio = 0;
1959         params.drcmr = 68;
1960         pcdev->dma_chans[0] =
1961                 dma_request_slave_channel_compat(mask, pxad_filter_fn,
1962                                                  &params, &pdev->dev, "CI_Y");
1963         if (!pcdev->dma_chans[0]) {
1964                 dev_err(&pdev->dev, "Can't request DMA for Y\n");
1965                 return -ENODEV;
1966         }
1967
1968         params.drcmr = 69;
1969         pcdev->dma_chans[1] =
1970                 dma_request_slave_channel_compat(mask, pxad_filter_fn,
1971                                                  &params, &pdev->dev, "CI_U");
1972         if (!pcdev->dma_chans[1]) {
1973                 dev_err(&pdev->dev, "Can't request DMA for Y\n");
1974                 goto exit_free_dma_y;
1975         }
1976
1977         params.drcmr = 70;
1978         pcdev->dma_chans[2] =
1979                 dma_request_slave_channel_compat(mask, pxad_filter_fn,
1980                                                  &params, &pdev->dev, "CI_V");
1981         if (!pcdev->dma_chans[2]) {
1982                 dev_err(&pdev->dev, "Can't request DMA for V\n");
1983                 goto exit_free_dma_u;
1984         }
1985
1986         for (i = 0; i < 3; i++) {
1987                 config.src_addr = pcdev->res->start + CIBR0 + i * 8;
1988                 err = dmaengine_slave_config(pcdev->dma_chans[i], &config);
1989                 if (err < 0) {
1990                         dev_err(&pdev->dev, "dma slave config failed: %d\n",
1991                                 err);
1992                         goto exit_free_dma;
1993                 }
1994         }
1995
1996         /* request irq */
1997         err = devm_request_irq(&pdev->dev, pcdev->irq, pxa_camera_irq, 0,
1998                                PXA_CAM_DRV_NAME, pcdev);
1999         if (err) {
2000                 dev_err(&pdev->dev, "Camera interrupt register failed\n");
2001                 goto exit_free_dma;
2002         }
2003
2004         tasklet_init(&pcdev->task_eof, pxa_camera_eof, (unsigned long)pcdev);
2005
2006         pxa_camera_activate(pcdev);
2007
2008         dev_set_drvdata(&pdev->dev, pcdev);
2009         err = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
2010         if (err)
2011                 goto exit_free_dma;
2012
2013         pcdev->asds[0] = &pcdev->asd;
2014         pcdev->notifier.subdevs = pcdev->asds;
2015         pcdev->notifier.num_subdevs = 1;
2016         pcdev->notifier.bound = pxa_camera_sensor_bound;
2017         pcdev->notifier.unbind = pxa_camera_sensor_unbind;
2018
2019         if (!of_have_populated_dt())
2020                 pcdev->asd.match_type = V4L2_ASYNC_MATCH_I2C;
2021
2022         err = pxa_camera_init_videobuf2(pcdev);
2023         if (err)
2024                 goto exit_free_v4l2dev;
2025
2026         if (pcdev->mclk) {
2027                 v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
2028                                   pcdev->asd.match.i2c.adapter_id,
2029                                   pcdev->asd.match.i2c.address);
2030
2031                 pcdev->mclk_clk = v4l2_clk_register(&pxa_camera_mclk_ops,
2032                                                     clk_name, NULL);
2033                 if (IS_ERR(pcdev->mclk_clk))
2034                         return PTR_ERR(pcdev->mclk_clk);
2035         }
2036
2037         err = v4l2_async_notifier_register(&pcdev->v4l2_dev, &pcdev->notifier);
2038         if (err)
2039                 goto exit_free_clk;
2040
2041         return 0;
2042 exit_free_clk:
2043         v4l2_clk_unregister(pcdev->mclk_clk);
2044 exit_free_v4l2dev:
2045         v4l2_device_unregister(&pcdev->v4l2_dev);
2046 exit_free_dma:
2047         dma_release_channel(pcdev->dma_chans[2]);
2048 exit_free_dma_u:
2049         dma_release_channel(pcdev->dma_chans[1]);
2050 exit_free_dma_y:
2051         dma_release_channel(pcdev->dma_chans[0]);
2052         return err;
2053 }
2054
2055 static int pxa_camera_remove(struct platform_device *pdev)
2056 {
2057         struct pxa_camera_dev *pcdev = dev_get_drvdata(&pdev->dev);
2058
2059         pxa_camera_deactivate(pcdev);
2060         dma_release_channel(pcdev->dma_chans[0]);
2061         dma_release_channel(pcdev->dma_chans[1]);
2062         dma_release_channel(pcdev->dma_chans[2]);
2063
2064         v4l2_clk_unregister(pcdev->mclk_clk);
2065         v4l2_device_unregister(&pcdev->v4l2_dev);
2066
2067         dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
2068
2069         return 0;
2070 }
2071
2072 static const struct dev_pm_ops pxa_camera_pm = {
2073         .suspend        = pxa_camera_suspend,
2074         .resume         = pxa_camera_resume,
2075 };
2076
2077 static const struct of_device_id pxa_camera_of_match[] = {
2078         { .compatible = "marvell,pxa270-qci", },
2079         {},
2080 };
2081 MODULE_DEVICE_TABLE(of, pxa_camera_of_match);
2082
2083 static struct platform_driver pxa_camera_driver = {
2084         .driver         = {
2085                 .name   = PXA_CAM_DRV_NAME,
2086                 .pm     = &pxa_camera_pm,
2087                 .of_match_table = of_match_ptr(pxa_camera_of_match),
2088         },
2089         .probe          = pxa_camera_probe,
2090         .remove         = pxa_camera_remove,
2091 };
2092
2093 module_platform_driver(pxa_camera_driver);
2094
2095 MODULE_DESCRIPTION("PXA27x SoC Camera Host driver");
2096 MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
2097 MODULE_LICENSE("GPL");
2098 MODULE_VERSION(PXA_CAM_VERSION);
2099 MODULE_ALIAS("platform:" PXA_CAM_DRV_NAME);