drivers/media/platform/marvell-ccic/mcam-core.c

   1 /*
   2  * The Marvell camera core.  This device appears in a number of settings,
   3  * so it needs platform-specific support outside of the core.
   4  *
   5  * Copyright 2011 Jonathan Corbet corbet@lwn.net
   6  */
   7 #include <linux/kernel.h>
   8 #include <linux/module.h>
   9 #include <linux/fs.h>
  10 #include <linux/mm.h>
  11 #include <linux/i2c.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/spinlock.h>
  14 #include <linux/slab.h>
  15 #include <linux/device.h>
  16 #include <linux/wait.h>
  17 #include <linux/list.h>
  18 #include <linux/dma-mapping.h>
  19 #include <linux/delay.h>
  20 #include <linux/vmalloc.h>
  21 #include <linux/io.h>
  22 #include <linux/clk.h>
  23 #include <linux/videodev2.h>
  24 #include <media/v4l2-device.h>
  25 #include <media/v4l2-ioctl.h>
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-event.h>
  28 #include <media/ov7670.h>
  29 #include <media/videobuf2-vmalloc.h>
  30 #include <media/videobuf2-dma-contig.h>
  31 #include <media/videobuf2-dma-sg.h>
  32
  33 #include "mcam-core.h"
  34
  35 #ifdef MCAM_MODE_VMALLOC
  36 /*
  37  * Internal DMA buffer management.  Since the controller cannot do S/G I/O,
  38  * we must have physically contiguous buffers to bring frames into.
  39  * These parameters control how many buffers we use, whether we
  40  * allocate them at load time (better chance of success, but nails down
  41  * memory) or when somebody tries to use the camera (riskier), and,
  42  * for load-time allocation, how big they should be.
  43  *
  44  * The controller can cycle through three buffers.  We could use
  45  * more by flipping pointers around, but it probably makes little
  46  * sense.
  47  */
  48
  49 static bool alloc_bufs_at_read;
  50 module_param(alloc_bufs_at_read, bool, 0444);
  51 MODULE_PARM_DESC(alloc_bufs_at_read,
  52                 "Non-zero value causes DMA buffers to be allocated when the "
  53                 "video capture device is read, rather than at module load "
  54                 "time.  This saves memory, but decreases the chances of "
  55                 "successfully getting those buffers.  This parameter is "
  56                 "only used in the vmalloc buffer mode");
  57
  58 static int n_dma_bufs = 3;
  59 module_param(n_dma_bufs, uint, 0644);
  60 MODULE_PARM_DESC(n_dma_bufs,
  61                 "The number of DMA buffers to allocate.  Can be either two "
  62                 "(saves memory, makes timing tighter) or three.");
  63
  64 static int dma_buf_size = VGA_WIDTH * VGA_HEIGHT * 2;  /* Worst case */
  65 module_param(dma_buf_size, uint, 0444);
  66 MODULE_PARM_DESC(dma_buf_size,
  67                 "The size of the allocated DMA buffers.  If actual operating "
  68                 "parameters require larger buffers, an attempt to reallocate "
  69                 "will be made.");
  70 #else /* MCAM_MODE_VMALLOC */
  71 static const bool alloc_bufs_at_read;
  72 static const int n_dma_bufs = 3;  /* Used by S/G_PARM */
  73 #endif /* MCAM_MODE_VMALLOC */
  74
  75 static bool flip;
  76 module_param(flip, bool, 0444);
  77 MODULE_PARM_DESC(flip,
  78                 "If set, the sensor will be instructed to flip the image "
  79                 "vertically.");
  80
  81 static int buffer_mode = -1;
  82 module_param(buffer_mode, int, 0444);
  83 MODULE_PARM_DESC(buffer_mode,
  84                 "Set the buffer mode to be used; default is to go with what "
  85                 "the platform driver asks for.  Set to 0 for vmalloc, 1 for "
  86                 "DMA contiguous.");
  87
  88 /*
  89  * Status flags.  Always manipulated with bit operations.
  90  */
  91 #define CF_BUF0_VALID    0      /* Buffers valid - first three */
  92 #define CF_BUF1_VALID    1
  93 #define CF_BUF2_VALID    2
  94 #define CF_DMA_ACTIVE    3      /* A frame is incoming */
  95 #define CF_CONFIG_NEEDED 4      /* Must configure hardware */
  96 #define CF_SINGLE_BUFFER 5      /* Running with a single buffer */
  97 #define CF_SG_RESTART    6      /* SG restart needed */
  98 #define CF_FRAME_SOF0    7      /* Frame 0 started */
  99 #define CF_FRAME_SOF1    8
 100 #define CF_FRAME_SOF2    9
 101
 102 #define sensor_call(cam, o, f, args...) \
 103         v4l2_subdev_call(cam->sensor, o, f, ##args)
 104
 105 static struct mcam_format_struct {
 106         __u8 *desc;
 107         __u32 pixelformat;
 108         int bpp;   /* Bytes per pixel */
 109         bool planar;
 110         u32 mbus_code;
 111 } mcam_formats[] = {
 112         {
 113                 .desc           = "YUYV 4:2:2",
 114                 .pixelformat    = V4L2_PIX_FMT_YUYV,
 115                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
 116                 .bpp            = 2,
 117                 .planar         = false,
 118         },
 119         {
 120                 .desc           = "YVYU 4:2:2",
 121                 .pixelformat    = V4L2_PIX_FMT_YVYU,
 122                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
 123                 .bpp            = 2,
 124                 .planar         = false,
 125         },
 126         {
 127                 .desc           = "YUV 4:2:0 PLANAR",
 128                 .pixelformat    = V4L2_PIX_FMT_YUV420,
 129                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
 130                 .bpp            = 1,
 131                 .planar         = true,
 132         },
 133         {
 134                 .desc           = "YVU 4:2:0 PLANAR",
 135                 .pixelformat    = V4L2_PIX_FMT_YVU420,
 136                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
 137                 .bpp            = 1,
 138                 .planar         = true,
 139         },
 140         {
 141                 .desc           = "XRGB 444",
 142                 .pixelformat    = V4L2_PIX_FMT_XRGB444,
 143                 .mbus_code      = MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE,
 144                 .bpp            = 2,
 145                 .planar         = false,
 146         },
 147         {
 148                 .desc           = "RGB 565",
 149                 .pixelformat    = V4L2_PIX_FMT_RGB565,
 150                 .mbus_code      = MEDIA_BUS_FMT_RGB565_2X8_LE,
 151                 .bpp            = 2,
 152                 .planar         = false,
 153         },
 154         {
 155                 .desc           = "Raw RGB Bayer",
 156                 .pixelformat    = V4L2_PIX_FMT_SBGGR8,
 157                 .mbus_code      = MEDIA_BUS_FMT_SBGGR8_1X8,
 158                 .bpp            = 1,
 159                 .planar         = false,
 160         },
 161 };
 162 #define N_MCAM_FMTS ARRAY_SIZE(mcam_formats)
 163
 164 static struct mcam_format_struct *mcam_find_format(u32 pixelformat)
 165 {
 166         unsigned i;
 167
 168         for (i = 0; i < N_MCAM_FMTS; i++)
 169                 if (mcam_formats[i].pixelformat == pixelformat)
 170                         return mcam_formats + i;
 171         /* Not found? Then return the first format. */
 172         return mcam_formats;
 173 }
 174
 175 /*
 176  * The default format we use until somebody says otherwise.
 177  */
 178 static const struct v4l2_pix_format mcam_def_pix_format = {
 179         .width          = VGA_WIDTH,
 180         .height         = VGA_HEIGHT,
 181         .pixelformat    = V4L2_PIX_FMT_YUYV,
 182         .field          = V4L2_FIELD_NONE,
 183         .bytesperline   = VGA_WIDTH*2,
 184         .sizeimage      = VGA_WIDTH*VGA_HEIGHT*2,
 185         .colorspace     = V4L2_COLORSPACE_SRGB,
 186 };
 187
 188 static const u32 mcam_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
 189
 190
 191 /*
 192  * The two-word DMA descriptor format used by the Armada 610 and like.  There
 193  * Is a three-word format as well (set C1_DESC_3WORD) where the third
 194  * word is a pointer to the next descriptor, but we don't use it.  Two-word
 195  * descriptors have to be contiguous in memory.
 196  */
 197 struct mcam_dma_desc {
 198         u32 dma_addr;
 199         u32 segment_len;
 200 };
 201
 202 /*
 203  * Our buffer type for working with videobuf2.  Note that the vb2
 204  * developers have decreed that struct vb2_buffer must be at the
 205  * beginning of this structure.
 206  */
 207 struct mcam_vb_buffer {
 208         struct vb2_buffer vb_buf;
 209         struct list_head queue;
 210         struct mcam_dma_desc *dma_desc; /* Descriptor virtual address */
 211         dma_addr_t dma_desc_pa;         /* Descriptor physical address */
 212         int dma_desc_nent;              /* Number of mapped descriptors */
 213 };
 214
 215 static inline struct mcam_vb_buffer *vb_to_mvb(struct vb2_buffer *vb)
 216 {
 217         return container_of(vb, struct mcam_vb_buffer, vb_buf);
 218 }
 219
 220 /*
 221  * Hand a completed buffer back to user space.
 222  */
 223 static void mcam_buffer_done(struct mcam_camera *cam, int frame,
 224                 struct vb2_buffer *vbuf)
 225 {
 226         vbuf->v4l2_buf.bytesused = cam->pix_format.sizeimage;
 227         vbuf->v4l2_buf.sequence = cam->buf_seq[frame];
 228         vbuf->v4l2_buf.field = V4L2_FIELD_NONE;
 229         v4l2_get_timestamp(&vbuf->v4l2_buf.timestamp);
 230         vb2_set_plane_payload(vbuf, 0, cam->pix_format.sizeimage);
 231         vb2_buffer_done(vbuf, VB2_BUF_STATE_DONE);
 232 }
 233
 234
 235
 236 /*
 237  * Debugging and related.
 238  */
 239 #define cam_err(cam, fmt, arg...) \
 240         dev_err((cam)->dev, fmt, ##arg);
 241 #define cam_warn(cam, fmt, arg...) \
 242         dev_warn((cam)->dev, fmt, ##arg);
 243 #define cam_dbg(cam, fmt, arg...) \
 244         dev_dbg((cam)->dev, fmt, ##arg);
 245
 246
 247 /*
 248  * Flag manipulation helpers
 249  */
 250 static void mcam_reset_buffers(struct mcam_camera *cam)
 251 {
 252         int i;
 253
 254         cam->next_buf = -1;
 255         for (i = 0; i < cam->nbufs; i++) {
 256                 clear_bit(i, &cam->flags);
 257                 clear_bit(CF_FRAME_SOF0 + i, &cam->flags);
 258         }
 259 }
 260
 261 static inline int mcam_needs_config(struct mcam_camera *cam)
 262 {
 263         return test_bit(CF_CONFIG_NEEDED, &cam->flags);
 264 }
 265
 266 static void mcam_set_config_needed(struct mcam_camera *cam, int needed)
 267 {
 268         if (needed)
 269                 set_bit(CF_CONFIG_NEEDED, &cam->flags);
 270         else
 271                 clear_bit(CF_CONFIG_NEEDED, &cam->flags);
 272 }
 273
 274 /* ------------------------------------------------------------------- */
 275 /*
 276  * Make the controller start grabbing images.  Everything must
 277  * be set up before doing this.
 278  */
 279 static void mcam_ctlr_start(struct mcam_camera *cam)
 280 {
 281         /* set_bit performs a read, so no other barrier should be
 282            needed here */
 283         mcam_reg_set_bit(cam, REG_CTRL0, C0_ENABLE);
 284 }
 285
 286 static void mcam_ctlr_stop(struct mcam_camera *cam)
 287 {
 288         mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
 289 }
 290
 291 static void mcam_enable_mipi(struct mcam_camera *mcam)
 292 {
 293         /* Using MIPI mode and enable MIPI */
 294         cam_dbg(mcam, "camera: DPHY3=0x%x, DPHY5=0x%x, DPHY6=0x%x\n",
 295                         mcam->dphy[0], mcam->dphy[1], mcam->dphy[2]);
 296         mcam_reg_write(mcam, REG_CSI2_DPHY3, mcam->dphy[0]);
 297         mcam_reg_write(mcam, REG_CSI2_DPHY5, mcam->dphy[1]);
 298         mcam_reg_write(mcam, REG_CSI2_DPHY6, mcam->dphy[2]);
 299
 300         if (!mcam->mipi_enabled) {
 301                 if (mcam->lane > 4 || mcam->lane <= 0) {
 302                         cam_warn(mcam, "lane number error\n");
 303                         mcam->lane = 1; /* set the default value */
 304                 }
 305                 /*
 306                  * 0x41 actives 1 lane
 307                  * 0x43 actives 2 lanes
 308                  * 0x45 actives 3 lanes (never happen)
 309                  * 0x47 actives 4 lanes
 310                  */
 311                 mcam_reg_write(mcam, REG_CSI2_CTRL0,
 312                         CSI2_C0_MIPI_EN | CSI2_C0_ACT_LANE(mcam->lane));
 313                 mcam_reg_write(mcam, REG_CLKCTRL,
 314                         (mcam->mclk_src << 29) | mcam->mclk_div);
 315
 316                 mcam->mipi_enabled = true;
 317         }
 318 }
 319
 320 static void mcam_disable_mipi(struct mcam_camera *mcam)
 321 {
 322         /* Using Parallel mode or disable MIPI */
 323         mcam_reg_write(mcam, REG_CSI2_CTRL0, 0x0);
 324         mcam_reg_write(mcam, REG_CSI2_DPHY3, 0x0);
 325         mcam_reg_write(mcam, REG_CSI2_DPHY5, 0x0);
 326         mcam_reg_write(mcam, REG_CSI2_DPHY6, 0x0);
 327         mcam->mipi_enabled = false;
 328 }
 329
 330 static bool mcam_fmt_is_planar(__u32 pfmt)
 331 {
 332         struct mcam_format_struct *f;
 333
 334         f = mcam_find_format(pfmt);
 335         return f->planar;
 336 }
 337
 338 static void mcam_write_yuv_bases(struct mcam_camera *cam,
 339                                  unsigned frame, dma_addr_t base)
 340 {
 341         struct v4l2_pix_format *fmt = &cam->pix_format;
 342         u32 pixel_count = fmt->width * fmt->height;
 343         dma_addr_t y, u = 0, v = 0;
 344
 345         y = base;
 346
 347         switch (fmt->pixelformat) {
 348         case V4L2_PIX_FMT_YUV420:
 349                 u = y + pixel_count;
 350                 v = u + pixel_count / 4;
 351                 break;
 352         case V4L2_PIX_FMT_YVU420:
 353                 v = y + pixel_count;
 354                 u = v + pixel_count / 4;
 355                 break;
 356         default:
 357                 break;
 358         }
 359
 360         mcam_reg_write(cam, REG_Y0BAR + frame * 4, y);
 361         if (mcam_fmt_is_planar(fmt->pixelformat)) {
 362                 mcam_reg_write(cam, REG_U0BAR + frame * 4, u);
 363                 mcam_reg_write(cam, REG_V0BAR + frame * 4, v);
 364         }
 365 }
 366
 367 /* ------------------------------------------------------------------- */
 368
 369 #ifdef MCAM_MODE_VMALLOC
 370 /*
 371  * Code specific to the vmalloc buffer mode.
 372  */
 373
 374 /*
 375  * Allocate in-kernel DMA buffers for vmalloc mode.
 376  */
 377 static int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
 378 {
 379         int i;
 380
 381         mcam_set_config_needed(cam, 1);
 382         if (loadtime)
 383                 cam->dma_buf_size = dma_buf_size;
 384         else
 385                 cam->dma_buf_size = cam->pix_format.sizeimage;
 386         if (n_dma_bufs > 3)
 387                 n_dma_bufs = 3;
 388
 389         cam->nbufs = 0;
 390         for (i = 0; i < n_dma_bufs; i++) {
 391                 cam->dma_bufs[i] = dma_alloc_coherent(cam->dev,
 392                                 cam->dma_buf_size, cam->dma_handles + i,
 393                                 GFP_KERNEL);
 394                 if (cam->dma_bufs[i] == NULL) {
 395                         cam_warn(cam, "Failed to allocate DMA buffer\n");
 396                         break;
 397                 }
 398                 (cam->nbufs)++;
 399         }
 400
 401         switch (cam->nbufs) {
 402         case 1:
 403                 dma_free_coherent(cam->dev, cam->dma_buf_size,
 404                                 cam->dma_bufs[0], cam->dma_handles[0]);
 405                 cam->nbufs = 0;
 406         case 0:
 407                 cam_err(cam, "Insufficient DMA buffers, cannot operate\n");
 408                 return -ENOMEM;
 409
 410         case 2:
 411                 if (n_dma_bufs > 2)
 412                         cam_warn(cam, "Will limp along with only 2 buffers\n");
 413                 break;
 414         }
 415         return 0;
 416 }
 417
 418 static void mcam_free_dma_bufs(struct mcam_camera *cam)
 419 {
 420         int i;
 421
 422         for (i = 0; i < cam->nbufs; i++) {
 423                 dma_free_coherent(cam->dev, cam->dma_buf_size,
 424                                 cam->dma_bufs[i], cam->dma_handles[i]);
 425                 cam->dma_bufs[i] = NULL;
 426         }
 427         cam->nbufs = 0;
 428 }
 429
 430
 431 /*
 432  * Set up DMA buffers when operating in vmalloc mode
 433  */
 434 static void mcam_ctlr_dma_vmalloc(struct mcam_camera *cam)
 435 {
 436         /*
 437          * Store the first two YUV buffers. Then either
 438          * set the third if it exists, or tell the controller
 439          * to just use two.
 440          */
 441         mcam_write_yuv_bases(cam, 0, cam->dma_handles[0]);
 442         mcam_write_yuv_bases(cam, 1, cam->dma_handles[1]);
 443         if (cam->nbufs > 2) {
 444                 mcam_write_yuv_bases(cam, 2, cam->dma_handles[2]);
 445                 mcam_reg_clear_bit(cam, REG_CTRL1, C1_TWOBUFS);
 446         } else
 447                 mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
 448         if (cam->chip_id == MCAM_CAFE)
 449                 mcam_reg_write(cam, REG_UBAR, 0); /* 32 bits only */
 450 }
 451
 452 /*
 453  * Copy data out to user space in the vmalloc case
 454  */
 455 static void mcam_frame_tasklet(unsigned long data)
 456 {
 457         struct mcam_camera *cam = (struct mcam_camera *) data;
 458         int i;
 459         unsigned long flags;
 460         struct mcam_vb_buffer *buf;
 461
 462         spin_lock_irqsave(&cam->dev_lock, flags);
 463         for (i = 0; i < cam->nbufs; i++) {
 464                 int bufno = cam->next_buf;
 465
 466                 if (cam->state != S_STREAMING || bufno < 0)
 467                         break;  /* I/O got stopped */
 468                 if (++(cam->next_buf) >= cam->nbufs)
 469                         cam->next_buf = 0;
 470                 if (!test_bit(bufno, &cam->flags))
 471                         continue;
 472                 if (list_empty(&cam->buffers)) {
 473                         cam->frame_state.singles++;
 474                         break;  /* Leave it valid, hope for better later */
 475                 }
 476                 cam->frame_state.delivered++;
 477                 clear_bit(bufno, &cam->flags);
 478                 buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
 479                                 queue);
 480                 list_del_init(&buf->queue);
 481                 /*
 482                  * Drop the lock during the big copy.  This *should* be safe...
 483                  */
 484                 spin_unlock_irqrestore(&cam->dev_lock, flags);
 485                 memcpy(vb2_plane_vaddr(&buf->vb_buf, 0), cam->dma_bufs[bufno],
 486                                 cam->pix_format.sizeimage);
 487                 mcam_buffer_done(cam, bufno, &buf->vb_buf);
 488                 spin_lock_irqsave(&cam->dev_lock, flags);
 489         }
 490         spin_unlock_irqrestore(&cam->dev_lock, flags);
 491 }
 492
 493
 494 /*
 495  * Make sure our allocated buffers are up to the task.
 496  */
 497 static int mcam_check_dma_buffers(struct mcam_camera *cam)
 498 {
 499         if (cam->nbufs > 0 && cam->dma_buf_size < cam->pix_format.sizeimage)
 500                         mcam_free_dma_bufs(cam);
 501         if (cam->nbufs == 0)
 502                 return mcam_alloc_dma_bufs(cam, 0);
 503         return 0;
 504 }
 505
 506 static void mcam_vmalloc_done(struct mcam_camera *cam, int frame)
 507 {
 508         tasklet_schedule(&cam->s_tasklet);
 509 }
 510
 511 #else /* MCAM_MODE_VMALLOC */
 512
 513 static inline int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
 514 {
 515         return 0;
 516 }
 517
 518 static inline void mcam_free_dma_bufs(struct mcam_camera *cam)
 519 {
 520         return;
 521 }
 522
 523 static inline int mcam_check_dma_buffers(struct mcam_camera *cam)
 524 {
 525         return 0;
 526 }
 527
 528
 529
 530 #endif /* MCAM_MODE_VMALLOC */
 531
 532
 533 #ifdef MCAM_MODE_DMA_CONTIG
 534 /* ---------------------------------------------------------------------- */
 535 /*
 536  * DMA-contiguous code.
 537  */
 538
 539 /*
 540  * Set up a contiguous buffer for the given frame.  Here also is where
 541  * the underrun strategy is set: if there is no buffer available, reuse
 542  * the buffer from the other BAR and set the CF_SINGLE_BUFFER flag to
 543  * keep the interrupt handler from giving that buffer back to user
 544  * space.  In this way, we always have a buffer to DMA to and don't
 545  * have to try to play games stopping and restarting the controller.
 546  */
 547 static void mcam_set_contig_buffer(struct mcam_camera *cam, int frame)
 548 {
 549         struct mcam_vb_buffer *buf;
 550         dma_addr_t dma_handle;
 551         struct vb2_buffer *vb;
 552
 553         /*
 554          * If there are no available buffers, go into single mode
 555          */
 556         if (list_empty(&cam->buffers)) {
 557                 buf = cam->vb_bufs[frame ^ 0x1];
 558                 set_bit(CF_SINGLE_BUFFER, &cam->flags);
 559                 cam->frame_state.singles++;
 560         } else {
 561                 /*
 562                  * OK, we have a buffer we can use.
 563                  */
 564                 buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
 565                                         queue);
 566                 list_del_init(&buf->queue);
 567                 clear_bit(CF_SINGLE_BUFFER, &cam->flags);
 568         }
 569
 570         cam->vb_bufs[frame] = buf;
 571         vb = &buf->vb_buf;
 572
 573         dma_handle = vb2_dma_contig_plane_dma_addr(vb, 0);
 574         mcam_write_yuv_bases(cam, frame, dma_handle);
 575 }
 576
 577 /*
 578  * Initial B_DMA_contig setup.
 579  */
 580 static void mcam_ctlr_dma_contig(struct mcam_camera *cam)
 581 {
 582         mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
 583         cam->nbufs = 2;
 584         mcam_set_contig_buffer(cam, 0);
 585         mcam_set_contig_buffer(cam, 1);
 586 }
 587
 588 /*
 589  * Frame completion handling.
 590  */
 591 static void mcam_dma_contig_done(struct mcam_camera *cam, int frame)
 592 {
 593         struct mcam_vb_buffer *buf = cam->vb_bufs[frame];
 594
 595         if (!test_bit(CF_SINGLE_BUFFER, &cam->flags)) {
 596                 cam->frame_state.delivered++;
 597                 cam->vb_bufs[frame] = NULL;
 598                 mcam_buffer_done(cam, frame, &buf->vb_buf);
 599         }
 600         mcam_set_contig_buffer(cam, frame);
 601 }
 602
 603 #endif /* MCAM_MODE_DMA_CONTIG */
 604
 605 #ifdef MCAM_MODE_DMA_SG
 606 /* ---------------------------------------------------------------------- */
 607 /*
 608  * Scatter/gather-specific code.
 609  */
 610
 611 /*
 612  * Set up the next buffer for S/G I/O; caller should be sure that
 613  * the controller is stopped and a buffer is available.
 614  */
 615 static void mcam_sg_next_buffer(struct mcam_camera *cam)
 616 {
 617         struct mcam_vb_buffer *buf;
 618
 619         buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer, queue);
 620         list_del_init(&buf->queue);
 621         /*
 622          * Very Bad Not Good Things happen if you don't clear
 623          * C1_DESC_ENA before making any descriptor changes.
 624          */
 625         mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_ENA);
 626         mcam_reg_write(cam, REG_DMA_DESC_Y, buf->dma_desc_pa);
 627         mcam_reg_write(cam, REG_DESC_LEN_Y,
 628                         buf->dma_desc_nent*sizeof(struct mcam_dma_desc));
 629         mcam_reg_write(cam, REG_DESC_LEN_U, 0);
 630         mcam_reg_write(cam, REG_DESC_LEN_V, 0);
 631         mcam_reg_set_bit(cam, REG_CTRL1, C1_DESC_ENA);
 632         cam->vb_bufs[0] = buf;
 633 }
 634
 635 /*
 636  * Initial B_DMA_sg setup
 637  */
 638 static void mcam_ctlr_dma_sg(struct mcam_camera *cam)
 639 {
 640         /*
 641          * The list-empty condition can hit us at resume time
 642          * if the buffer list was empty when the system was suspended.
 643          */
 644         if (list_empty(&cam->buffers)) {
 645                 set_bit(CF_SG_RESTART, &cam->flags);
 646                 return;
 647         }
 648
 649         mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_3WORD);
 650         mcam_sg_next_buffer(cam);
 651         cam->nbufs = 3;
 652 }
 653
 654
 655 /*
 656  * Frame completion with S/G is trickier.  We can't muck with
 657  * a descriptor chain on the fly, since the controller buffers it
 658  * internally.  So we have to actually stop and restart; Marvell
 659  * says this is the way to do it.
 660  *
 661  * Of course, stopping is easier said than done; experience shows
 662  * that the controller can start a frame *after* C0_ENABLE has been
 663  * cleared.  So when running in S/G mode, the controller is "stopped"
 664  * on receipt of the start-of-frame interrupt.  That means we can
 665  * safely change the DMA descriptor array here and restart things
 666  * (assuming there's another buffer waiting to go).
 667  */
 668 static void mcam_dma_sg_done(struct mcam_camera *cam, int frame)
 669 {
 670         struct mcam_vb_buffer *buf = cam->vb_bufs[0];
 671
 672         /*
 673          * If we're no longer supposed to be streaming, don't do anything.
 674          */
 675         if (cam->state != S_STREAMING)
 676                 return;
 677         /*
 678          * If we have another buffer available, put it in and
 679          * restart the engine.
 680          */
 681         if (!list_empty(&cam->buffers)) {
 682                 mcam_sg_next_buffer(cam);
 683                 mcam_ctlr_start(cam);
 684         /*
 685          * Otherwise set CF_SG_RESTART and the controller will
 686          * be restarted once another buffer shows up.
 687          */
 688         } else {
 689                 set_bit(CF_SG_RESTART, &cam->flags);
 690                 cam->frame_state.singles++;
 691                 cam->vb_bufs[0] = NULL;
 692         }
 693         /*
 694          * Now we can give the completed frame back to user space.
 695          */
 696         cam->frame_state.delivered++;
 697         mcam_buffer_done(cam, frame, &buf->vb_buf);
 698 }
 699
 700
 701 /*
 702  * Scatter/gather mode requires stopping the controller between
 703  * frames so we can put in a new DMA descriptor array.  If no new
 704  * buffer exists at frame completion, the controller is left stopped;
 705  * this function is charged with gettig things going again.
 706  */
 707 static void mcam_sg_restart(struct mcam_camera *cam)
 708 {
 709         mcam_ctlr_dma_sg(cam);
 710         mcam_ctlr_start(cam);
 711         clear_bit(CF_SG_RESTART, &cam->flags);
 712 }
 713
 714 #else /* MCAM_MODE_DMA_SG */
 715
 716 static inline void mcam_sg_restart(struct mcam_camera *cam)
 717 {
 718         return;
 719 }
 720
 721 #endif /* MCAM_MODE_DMA_SG */
 722
 723 /* ---------------------------------------------------------------------- */
 724 /*
 725  * Buffer-mode-independent controller code.
 726  */
 727
 728 /*
 729  * Image format setup
 730  */
 731 static void mcam_ctlr_image(struct mcam_camera *cam)
 732 {
 733         struct v4l2_pix_format *fmt = &cam->pix_format;
 734         u32 widthy = 0, widthuv = 0, imgsz_h, imgsz_w;
 735
 736         cam_dbg(cam, "camera: bytesperline = %d; height = %d\n",
 737                 fmt->bytesperline, fmt->sizeimage / fmt->bytesperline);
 738         imgsz_h = (fmt->height << IMGSZ_V_SHIFT) & IMGSZ_V_MASK;
 739         imgsz_w = (fmt->width * 2) & IMGSZ_H_MASK;
 740
 741         switch (fmt->pixelformat) {
 742         case V4L2_PIX_FMT_YUYV:
 743         case V4L2_PIX_FMT_YVYU:
 744                 widthy = fmt->width * 2;
 745                 widthuv = 0;
 746                 break;
 747         case V4L2_PIX_FMT_YUV420:
 748         case V4L2_PIX_FMT_YVU420:
 749                 widthy = fmt->width;
 750                 widthuv = fmt->width / 2;
 751                 break;
 752         default:
 753                 widthy = fmt->bytesperline;
 754                 widthuv = 0;
 755                 break;
 756         }
 757
 758         mcam_reg_write_mask(cam, REG_IMGPITCH, widthuv << 16 | widthy,
 759                         IMGP_YP_MASK | IMGP_UVP_MASK);
 760         mcam_reg_write(cam, REG_IMGSIZE, imgsz_h | imgsz_w);
 761         mcam_reg_write(cam, REG_IMGOFFSET, 0x0);
 762
 763         /*
 764          * Tell the controller about the image format we are using.
 765          */
 766         switch (fmt->pixelformat) {
 767         case V4L2_PIX_FMT_YUV420:
 768         case V4L2_PIX_FMT_YVU420:
 769                 mcam_reg_write_mask(cam, REG_CTRL0,
 770                         C0_DF_YUV | C0_YUV_420PL | C0_YUVE_VYUY, C0_DF_MASK);
 771                 break;
 772         case V4L2_PIX_FMT_YUYV:
 773                 mcam_reg_write_mask(cam, REG_CTRL0,
 774                         C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_NOSWAP, C0_DF_MASK);
 775                 break;
 776         case V4L2_PIX_FMT_YVYU:
 777                 mcam_reg_write_mask(cam, REG_CTRL0,
 778                         C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_SWAP24, C0_DF_MASK);
 779                 break;
 780         case V4L2_PIX_FMT_XRGB444:
 781                 mcam_reg_write_mask(cam, REG_CTRL0,
 782                         C0_DF_RGB | C0_RGBF_444 | C0_RGB4_XBGR, C0_DF_MASK);
 783                 break;
 784         case V4L2_PIX_FMT_RGB565:
 785                 mcam_reg_write_mask(cam, REG_CTRL0,
 786                         C0_DF_RGB | C0_RGBF_565 | C0_RGB5_BGGR, C0_DF_MASK);
 787                 break;
 788         case V4L2_PIX_FMT_SBGGR8:
 789                 mcam_reg_write_mask(cam, REG_CTRL0,
 790                         C0_DF_RGB | C0_RGB5_GRBG, C0_DF_MASK);
 791                 break;
 792         default:
 793                 cam_err(cam, "camera: unknown format: %#x\n", fmt->pixelformat);
 794                 break;
 795         }
 796
 797         /*
 798          * Make sure it knows we want to use hsync/vsync.
 799          */
 800         mcam_reg_write_mask(cam, REG_CTRL0, C0_SIF_HVSYNC, C0_SIFM_MASK);
 801         /*
 802          * This field controls the generation of EOF(DVP only)
 803          */
 804         if (cam->bus_type != V4L2_MBUS_CSI2)
 805                 mcam_reg_set_bit(cam, REG_CTRL0,
 806                                 C0_EOF_VSYNC | C0_VEDGE_CTRL);
 807 }
 808
 809
 810 /*
 811  * Configure the controller for operation; caller holds the
 812  * device mutex.
 813  */
 814 static int mcam_ctlr_configure(struct mcam_camera *cam)
 815 {
 816         unsigned long flags;
 817
 818         spin_lock_irqsave(&cam->dev_lock, flags);
 819         clear_bit(CF_SG_RESTART, &cam->flags);
 820         cam->dma_setup(cam);
 821         mcam_ctlr_image(cam);
 822         mcam_set_config_needed(cam, 0);
 823         spin_unlock_irqrestore(&cam->dev_lock, flags);
 824         return 0;
 825 }
 826
 827 static void mcam_ctlr_irq_enable(struct mcam_camera *cam)
 828 {
 829         /*
 830          * Clear any pending interrupts, since we do not
 831          * expect to have I/O active prior to enabling.
 832          */
 833         mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS);
 834         mcam_reg_set_bit(cam, REG_IRQMASK, FRAMEIRQS);
 835 }
 836
 837 static void mcam_ctlr_irq_disable(struct mcam_camera *cam)
 838 {
 839         mcam_reg_clear_bit(cam, REG_IRQMASK, FRAMEIRQS);
 840 }
 841
 842
 843
 844 static void mcam_ctlr_init(struct mcam_camera *cam)
 845 {
 846         unsigned long flags;
 847
 848         spin_lock_irqsave(&cam->dev_lock, flags);
 849         /*
 850          * Make sure it's not powered down.
 851          */
 852         mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
 853         /*
 854          * Turn off the enable bit.  It sure should be off anyway,
 855          * but it's good to be sure.
 856          */
 857         mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
 858         /*
 859          * Clock the sensor appropriately.  Controller clock should
 860          * be 48MHz, sensor "typical" value is half that.
 861          */
 862         mcam_reg_write_mask(cam, REG_CLKCTRL, 2, CLK_DIV_MASK);
 863         spin_unlock_irqrestore(&cam->dev_lock, flags);
 864 }
 865
 866
 867 /*
 868  * Stop the controller, and don't return until we're really sure that no
 869  * further DMA is going on.
 870  */
 871 static void mcam_ctlr_stop_dma(struct mcam_camera *cam)
 872 {
 873         unsigned long flags;
 874
 875         /*
 876          * Theory: stop the camera controller (whether it is operating
 877          * or not).  Delay briefly just in case we race with the SOF
 878          * interrupt, then wait until no DMA is active.
 879          */
 880         spin_lock_irqsave(&cam->dev_lock, flags);
 881         clear_bit(CF_SG_RESTART, &cam->flags);
 882         mcam_ctlr_stop(cam);
 883         cam->state = S_IDLE;
 884         spin_unlock_irqrestore(&cam->dev_lock, flags);
 885         /*
 886          * This is a brutally long sleep, but experience shows that
 887          * it can take the controller a while to get the message that
 888          * it needs to stop grabbing frames.  In particular, we can
 889          * sometimes (on mmp) get a frame at the end WITHOUT the
 890          * start-of-frame indication.
 891          */
 892         msleep(150);
 893         if (test_bit(CF_DMA_ACTIVE, &cam->flags))
 894                 cam_err(cam, "Timeout waiting for DMA to end\n");
 895                 /* This would be bad news - what now? */
 896         spin_lock_irqsave(&cam->dev_lock, flags);
 897         mcam_ctlr_irq_disable(cam);
 898         spin_unlock_irqrestore(&cam->dev_lock, flags);
 899 }
 900
 901 /*
 902  * Power up and down.
 903  */
 904 static int mcam_ctlr_power_up(struct mcam_camera *cam)
 905 {
 906         unsigned long flags;
 907         int ret;
 908
 909         spin_lock_irqsave(&cam->dev_lock, flags);
 910         ret = cam->plat_power_up(cam);
 911         if (ret) {
 912                 spin_unlock_irqrestore(&cam->dev_lock, flags);
 913                 return ret;
 914         }
 915         mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
 916         spin_unlock_irqrestore(&cam->dev_lock, flags);
 917         msleep(5); /* Just to be sure */
 918         return 0;
 919 }
 920
 921 static void mcam_ctlr_power_down(struct mcam_camera *cam)
 922 {
 923         unsigned long flags;
 924
 925         spin_lock_irqsave(&cam->dev_lock, flags);
 926         /*
 927          * School of hard knocks department: be sure we do any register
 928          * twiddling on the controller *before* calling the platform
 929          * power down routine.
 930          */
 931         mcam_reg_set_bit(cam, REG_CTRL1, C1_PWRDWN);
 932         cam->plat_power_down(cam);
 933         spin_unlock_irqrestore(&cam->dev_lock, flags);
 934 }
 935
 936 /* -------------------------------------------------------------------- */
 937 /*
 938  * Communications with the sensor.
 939  */
 940
 941 static int __mcam_cam_reset(struct mcam_camera *cam)
 942 {
 943         return sensor_call(cam, core, reset, 0);
 944 }
 945
 946 /*
 947  * We have found the sensor on the i2c.  Let's try to have a
 948  * conversation.
 949  */
 950 static int mcam_cam_init(struct mcam_camera *cam)
 951 {
 952         int ret;
 953
 954         if (cam->state != S_NOTREADY)
 955                 cam_warn(cam, "Cam init with device in funky state %d",
 956                                 cam->state);
 957         ret = __mcam_cam_reset(cam);
 958         /* Get/set parameters? */
 959         cam->state = S_IDLE;
 960         mcam_ctlr_power_down(cam);
 961         return ret;
 962 }
 963
 964 /*
 965  * Configure the sensor to match the parameters we have.  Caller should
 966  * hold s_mutex
 967  */
 968 static int mcam_cam_set_flip(struct mcam_camera *cam)
 969 {
 970         struct v4l2_control ctrl;
 971
 972         memset(&ctrl, 0, sizeof(ctrl));
 973         ctrl.id = V4L2_CID_VFLIP;
 974         ctrl.value = flip;
 975         return sensor_call(cam, core, s_ctrl, &ctrl);
 976 }
 977
 978
 979 static int mcam_cam_configure(struct mcam_camera *cam)
 980 {
 981         struct v4l2_subdev_format format = {
 982                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 983         };
 984         int ret;
 985
 986         v4l2_fill_mbus_format(&format.format, &cam->pix_format, cam->mbus_code);
 987         ret = sensor_call(cam, core, init, 0);
 988         if (ret == 0)
 989                 ret = sensor_call(cam, pad, set_fmt, NULL, &format);
 990         /*
 991          * OV7670 does weird things if flip is set *before* format...
 992          */
 993         ret += mcam_cam_set_flip(cam);
 994         return ret;
 995 }
 996
 997 /*
 998  * Get everything ready, and start grabbing frames.
 999  */
1000 static int mcam_read_setup(struct mcam_camera *cam)
1001 {
1002         int ret;
1003         unsigned long flags;
1004
1005         /*
1006          * Configuration.  If we still don't have DMA buffers,
1007          * make one last, desperate attempt.
1008          */
1009         if (cam->buffer_mode == B_vmalloc && cam->nbufs == 0 &&
1010                         mcam_alloc_dma_bufs(cam, 0))
1011                 return -ENOMEM;
1012
1013         if (mcam_needs_config(cam)) {
1014                 mcam_cam_configure(cam);
1015                 ret = mcam_ctlr_configure(cam);
1016                 if (ret)
1017                         return ret;
1018         }
1019
1020         /*
1021          * Turn it loose.
1022          */
1023         spin_lock_irqsave(&cam->dev_lock, flags);
1024         clear_bit(CF_DMA_ACTIVE, &cam->flags);
1025         mcam_reset_buffers(cam);
1026         /*
1027          * Update CSI2_DPHY value
1028          */
1029         if (cam->calc_dphy)
1030                 cam->calc_dphy(cam);
1031         cam_dbg(cam, "camera: DPHY sets: dphy3=0x%x, dphy5=0x%x, dphy6=0x%x\n",
1032                         cam->dphy[0], cam->dphy[1], cam->dphy[2]);
1033         if (cam->bus_type == V4L2_MBUS_CSI2)
1034                 mcam_enable_mipi(cam);
1035         else
1036                 mcam_disable_mipi(cam);
1037         mcam_ctlr_irq_enable(cam);
1038         cam->state = S_STREAMING;
1039         if (!test_bit(CF_SG_RESTART, &cam->flags))
1040                 mcam_ctlr_start(cam);
1041         spin_unlock_irqrestore(&cam->dev_lock, flags);
1042         return 0;
1043 }
1044
1045 /* ----------------------------------------------------------------------- */
1046 /*
1047  * Videobuf2 interface code.
1048  */
1049
1050 static int mcam_vb_queue_setup(struct vb2_queue *vq,
1051                 const struct v4l2_format *fmt, unsigned int *nbufs,
1052                 unsigned int *num_planes, unsigned int sizes[],
1053                 void *alloc_ctxs[])
1054 {
1055         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1056         int minbufs = (cam->buffer_mode == B_DMA_contig) ? 3 : 2;
1057
1058         if (fmt && fmt->fmt.pix.sizeimage < cam->pix_format.sizeimage)
1059                 return -EINVAL;
1060         sizes[0] = fmt ? fmt->fmt.pix.sizeimage : cam->pix_format.sizeimage;
1061         *num_planes = 1; /* Someday we have to support planar formats... */
1062         if (*nbufs < minbufs)
1063                 *nbufs = minbufs;
1064         if (cam->buffer_mode == B_DMA_contig)
1065                 alloc_ctxs[0] = cam->vb_alloc_ctx;
1066         else if (cam->buffer_mode == B_DMA_sg)
1067                 alloc_ctxs[0] = cam->vb_alloc_ctx_sg;
1068         return 0;
1069 }
1070
1071
1072 static void mcam_vb_buf_queue(struct vb2_buffer *vb)
1073 {
1074         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1075         struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
1076         unsigned long flags;
1077         int start;
1078
1079         spin_lock_irqsave(&cam->dev_lock, flags);
1080         start = (cam->state == S_BUFWAIT) && !list_empty(&cam->buffers);
1081         list_add(&mvb->queue, &cam->buffers);
1082         if (cam->state == S_STREAMING && test_bit(CF_SG_RESTART, &cam->flags))
1083                 mcam_sg_restart(cam);
1084         spin_unlock_irqrestore(&cam->dev_lock, flags);
1085         if (start)
1086                 mcam_read_setup(cam);
1087 }
1088
1089 static void mcam_vb_requeue_bufs(struct vb2_queue *vq,
1090                                  enum vb2_buffer_state state)
1091 {
1092         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1093         struct mcam_vb_buffer *buf, *node;
1094         unsigned long flags;
1095         unsigned i;
1096
1097         spin_lock_irqsave(&cam->dev_lock, flags);
1098         list_for_each_entry_safe(buf, node, &cam->buffers, queue) {
1099                 vb2_buffer_done(&buf->vb_buf, state);
1100                 list_del(&buf->queue);
1101         }
1102         for (i = 0; i < MAX_DMA_BUFS; i++) {
1103                 buf = cam->vb_bufs[i];
1104
1105                 if (buf) {
1106                         vb2_buffer_done(&buf->vb_buf, state);
1107                         cam->vb_bufs[i] = NULL;
1108                 }
1109         }
1110         spin_unlock_irqrestore(&cam->dev_lock, flags);
1111 }
1112
1113 /*
1114  * These need to be called with the mutex held from vb2
1115  */
1116 static int mcam_vb_start_streaming(struct vb2_queue *vq, unsigned int count)
1117 {
1118         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1119         unsigned int frame;
1120         int ret;
1121
1122         if (cam->state != S_IDLE) {
1123                 mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
1124                 return -EINVAL;
1125         }
1126         cam->frame_state.frames = 0;
1127         cam->frame_state.singles = 0;
1128         cam->frame_state.delivered = 0;
1129         cam->sequence = 0;
1130         /*
1131          * Videobuf2 sneakily hoards all the buffers and won't
1132          * give them to us until *after* streaming starts.  But
1133          * we can't actually start streaming until we have a
1134          * destination.  So go into a wait state and hope they
1135          * give us buffers soon.
1136          */
1137         if (cam->buffer_mode != B_vmalloc && list_empty(&cam->buffers)) {
1138                 cam->state = S_BUFWAIT;
1139                 return 0;
1140         }
1141
1142         /*
1143          * Ensure clear the left over frame flags
1144          * before every really start streaming
1145          */
1146         for (frame = 0; frame < cam->nbufs; frame++)
1147                 clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
1148
1149         ret = mcam_read_setup(cam);
1150         if (ret)
1151                 mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
1152         return ret;
1153 }
1154
1155 static void mcam_vb_stop_streaming(struct vb2_queue *vq)
1156 {
1157         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1158
1159         cam_dbg(cam, "stop_streaming: %d frames, %d singles, %d delivered\n",
1160                         cam->frame_state.frames, cam->frame_state.singles,
1161                         cam->frame_state.delivered);
1162         if (cam->state == S_BUFWAIT) {
1163                 /* They never gave us buffers */
1164                 cam->state = S_IDLE;
1165                 return;
1166         }
1167         if (cam->state != S_STREAMING)
1168                 return;
1169         mcam_ctlr_stop_dma(cam);
1170         /*
1171          * Reset the CCIC PHY after stopping streaming,
1172          * otherwise, the CCIC may be unstable.
1173          */
1174         if (cam->ctlr_reset)
1175                 cam->ctlr_reset(cam);
1176         /*
1177          * VB2 reclaims the buffers, so we need to forget
1178          * about them.
1179          */
1180         mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_ERROR);
1181 }
1182
1183
1184 static const struct vb2_ops mcam_vb2_ops = {
1185         .queue_setup            = mcam_vb_queue_setup,
1186         .buf_queue              = mcam_vb_buf_queue,
1187         .start_streaming        = mcam_vb_start_streaming,
1188         .stop_streaming         = mcam_vb_stop_streaming,
1189         .wait_prepare           = vb2_ops_wait_prepare,
1190         .wait_finish            = vb2_ops_wait_finish,
1191 };
1192
1193
1194 #ifdef MCAM_MODE_DMA_SG
1195 /*
1196  * Scatter/gather mode uses all of the above functions plus a
1197  * few extras to deal with DMA mapping.
1198  */
1199 static int mcam_vb_sg_buf_init(struct vb2_buffer *vb)
1200 {
1201         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1202         struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
1203         int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
1204
1205         mvb->dma_desc = dma_alloc_coherent(cam->dev,
1206                         ndesc * sizeof(struct mcam_dma_desc),
1207                         &mvb->dma_desc_pa, GFP_KERNEL);
1208         if (mvb->dma_desc == NULL) {
1209                 cam_err(cam, "Unable to get DMA descriptor array\n");
1210                 return -ENOMEM;
1211         }
1212         return 0;
1213 }
1214
1215 static int mcam_vb_sg_buf_prepare(struct vb2_buffer *vb)
1216 {
1217         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1218         struct sg_table *sg_table = vb2_dma_sg_plane_desc(vb, 0);
1219         struct mcam_dma_desc *desc = mvb->dma_desc;
1220         struct scatterlist *sg;
1221         int i;
1222
1223         for_each_sg(sg_table->sgl, sg, sg_table->nents, i) {
1224                 desc->dma_addr = sg_dma_address(sg);
1225                 desc->segment_len = sg_dma_len(sg);
1226                 desc++;
1227         }
1228         return 0;
1229 }
1230
1231 static void mcam_vb_sg_buf_cleanup(struct vb2_buffer *vb)
1232 {
1233         struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
1234         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1235         int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
1236
1237         dma_free_coherent(cam->dev, ndesc * sizeof(struct mcam_dma_desc),
1238                         mvb->dma_desc, mvb->dma_desc_pa);
1239 }
1240
1241
1242 static const struct vb2_ops mcam_vb2_sg_ops = {
1243         .queue_setup            = mcam_vb_queue_setup,
1244         .buf_init               = mcam_vb_sg_buf_init,
1245         .buf_prepare            = mcam_vb_sg_buf_prepare,
1246         .buf_queue              = mcam_vb_buf_queue,
1247         .buf_cleanup            = mcam_vb_sg_buf_cleanup,
1248         .start_streaming        = mcam_vb_start_streaming,
1249         .stop_streaming         = mcam_vb_stop_streaming,
1250         .wait_prepare           = vb2_ops_wait_prepare,
1251         .wait_finish            = vb2_ops_wait_finish,
1252 };
1253
1254 #endif /* MCAM_MODE_DMA_SG */
1255
1256 static int mcam_setup_vb2(struct mcam_camera *cam)
1257 {
1258         struct vb2_queue *vq = &cam->vb_queue;
1259
1260         memset(vq, 0, sizeof(*vq));
1261         vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1262         vq->drv_priv = cam;
1263         vq->lock = &cam->s_mutex;
1264         vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1265         vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
1266         vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
1267         INIT_LIST_HEAD(&cam->buffers);
1268         switch (cam->buffer_mode) {
1269         case B_DMA_contig:
1270 #ifdef MCAM_MODE_DMA_CONTIG
1271                 vq->ops = &mcam_vb2_ops;
1272                 vq->mem_ops = &vb2_dma_contig_memops;
1273                 cam->dma_setup = mcam_ctlr_dma_contig;
1274                 cam->frame_complete = mcam_dma_contig_done;
1275                 cam->vb_alloc_ctx = vb2_dma_contig_init_ctx(cam->dev);
1276                 if (IS_ERR(cam->vb_alloc_ctx))
1277                         return PTR_ERR(cam->vb_alloc_ctx);
1278 #endif
1279                 break;
1280         case B_DMA_sg:
1281 #ifdef MCAM_MODE_DMA_SG
1282                 vq->ops = &mcam_vb2_sg_ops;
1283                 vq->mem_ops = &vb2_dma_sg_memops;
1284                 cam->dma_setup = mcam_ctlr_dma_sg;
1285                 cam->frame_complete = mcam_dma_sg_done;
1286                 cam->vb_alloc_ctx_sg = vb2_dma_sg_init_ctx(cam->dev);
1287                 if (IS_ERR(cam->vb_alloc_ctx_sg))
1288                         return PTR_ERR(cam->vb_alloc_ctx_sg);
1289 #endif
1290                 break;
1291         case B_vmalloc:
1292 #ifdef MCAM_MODE_VMALLOC
1293                 tasklet_init(&cam->s_tasklet, mcam_frame_tasklet,
1294                                 (unsigned long) cam);
1295                 vq->ops = &mcam_vb2_ops;
1296                 vq->mem_ops = &vb2_vmalloc_memops;
1297                 cam->dma_setup = mcam_ctlr_dma_vmalloc;
1298                 cam->frame_complete = mcam_vmalloc_done;
1299 #endif
1300                 break;
1301         }
1302         return vb2_queue_init(vq);
1303 }
1304
1305 static void mcam_cleanup_vb2(struct mcam_camera *cam)
1306 {
1307 #ifdef MCAM_MODE_DMA_CONTIG
1308         if (cam->buffer_mode == B_DMA_contig)
1309                 vb2_dma_contig_cleanup_ctx(cam->vb_alloc_ctx);
1310 #endif
1311 #ifdef MCAM_MODE_DMA_SG
1312         if (cam->buffer_mode == B_DMA_sg)
1313                 vb2_dma_sg_cleanup_ctx(cam->vb_alloc_ctx_sg);
1314 #endif
1315 }
1316
1317
1318 /* ---------------------------------------------------------------------- */
1319 /*
1320  * The long list of V4L2 ioctl() operations.
1321  */
1322
1323 static int mcam_vidioc_querycap(struct file *file, void *priv,
1324                 struct v4l2_capability *cap)
1325 {
1326         struct mcam_camera *cam = video_drvdata(file);
1327
1328         strcpy(cap->driver, "marvell_ccic");
1329         strcpy(cap->card, "marvell_ccic");
1330         strlcpy(cap->bus_info, cam->bus_info, sizeof(cap->bus_info));
1331         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
1332                 V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
1333         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1334         return 0;
1335 }
1336
1337
1338 static int mcam_vidioc_enum_fmt_vid_cap(struct file *filp,
1339                 void *priv, struct v4l2_fmtdesc *fmt)
1340 {
1341         if (fmt->index >= N_MCAM_FMTS)
1342                 return -EINVAL;
1343         strlcpy(fmt->description, mcam_formats[fmt->index].desc,
1344                         sizeof(fmt->description));
1345         fmt->pixelformat = mcam_formats[fmt->index].pixelformat;
1346         return 0;
1347 }
1348
1349 static int mcam_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
1350                 struct v4l2_format *fmt)
1351 {
1352         struct mcam_camera *cam = video_drvdata(filp);
1353         struct mcam_format_struct *f;
1354         struct v4l2_pix_format *pix = &fmt->fmt.pix;
1355         struct v4l2_subdev_pad_config pad_cfg;
1356         struct v4l2_subdev_format format = {
1357                 .which = V4L2_SUBDEV_FORMAT_TRY,
1358         };
1359         int ret;
1360
1361         f = mcam_find_format(pix->pixelformat);
1362         pix->pixelformat = f->pixelformat;
1363         v4l2_fill_mbus_format(&format.format, pix, f->mbus_code);
1364         ret = sensor_call(cam, pad, set_fmt, &pad_cfg, &format);
1365         v4l2_fill_pix_format(pix, &format.format);
1366         pix->bytesperline = pix->width * f->bpp;
1367         switch (f->pixelformat) {
1368         case V4L2_PIX_FMT_YUV420:
1369         case V4L2_PIX_FMT_YVU420:
1370                 pix->sizeimage = pix->height * pix->bytesperline * 3 / 2;
1371                 break;
1372         default:
1373                 pix->sizeimage = pix->height * pix->bytesperline;
1374                 break;
1375         }
1376         pix->colorspace = V4L2_COLORSPACE_SRGB;
1377         return ret;
1378 }
1379
1380 static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
1381                 struct v4l2_format *fmt)
1382 {
1383         struct mcam_camera *cam = video_drvdata(filp);
1384         struct mcam_format_struct *f;
1385         int ret;
1386
1387         /*
1388          * Can't do anything if the device is not idle
1389          * Also can't if there are streaming buffers in place.
1390          */
1391         if (cam->state != S_IDLE || vb2_is_busy(&cam->vb_queue))
1392                 return -EBUSY;
1393
1394         f = mcam_find_format(fmt->fmt.pix.pixelformat);
1395
1396         /*
1397          * See if the formatting works in principle.
1398          */
1399         ret = mcam_vidioc_try_fmt_vid_cap(filp, priv, fmt);
1400         if (ret)
1401                 return ret;
1402         /*
1403          * Now we start to change things for real, so let's do it
1404          * under lock.
1405          */
1406         cam->pix_format = fmt->fmt.pix;
1407         cam->mbus_code = f->mbus_code;
1408
1409         /*
1410          * Make sure we have appropriate DMA buffers.
1411          */
1412         if (cam->buffer_mode == B_vmalloc) {
1413                 ret = mcam_check_dma_buffers(cam);
1414                 if (ret)
1415                         goto out;
1416         }
1417         mcam_set_config_needed(cam, 1);
1418 out:
1419         return ret;
1420 }
1421
1422 /*
1423  * Return our stored notion of how the camera is/should be configured.
1424  * The V4l2 spec wants us to be smarter, and actually get this from
1425  * the camera (and not mess with it at open time).  Someday.
1426  */
1427 static int mcam_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
1428                 struct v4l2_format *f)
1429 {
1430         struct mcam_camera *cam = video_drvdata(filp);
1431
1432         f->fmt.pix = cam->pix_format;
1433         return 0;
1434 }
1435
1436 /*
1437  * We only have one input - the sensor - so minimize the nonsense here.
1438  */
1439 static int mcam_vidioc_enum_input(struct file *filp, void *priv,
1440                 struct v4l2_input *input)
1441 {
1442         if (input->index != 0)
1443                 return -EINVAL;
1444
1445         input->type = V4L2_INPUT_TYPE_CAMERA;
1446         strcpy(input->name, "Camera");
1447         return 0;
1448 }
1449
1450 static int mcam_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
1451 {
1452         *i = 0;
1453         return 0;
1454 }
1455
1456 static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
1457 {
1458         if (i != 0)
1459                 return -EINVAL;
1460         return 0;
1461 }
1462
1463 /*
1464  * G/S_PARM.  Most of this is done by the sensor, but we are
1465  * the level which controls the number of read buffers.
1466  */
1467 static int mcam_vidioc_g_parm(struct file *filp, void *priv,
1468                 struct v4l2_streamparm *parms)
1469 {
1470         struct mcam_camera *cam = video_drvdata(filp);
1471         int ret;
1472
1473         ret = sensor_call(cam, video, g_parm, parms);
1474         parms->parm.capture.readbuffers = n_dma_bufs;
1475         return ret;
1476 }
1477
1478 static int mcam_vidioc_s_parm(struct file *filp, void *priv,
1479                 struct v4l2_streamparm *parms)
1480 {
1481         struct mcam_camera *cam = video_drvdata(filp);
1482         int ret;
1483
1484         ret = sensor_call(cam, video, s_parm, parms);
1485         parms->parm.capture.readbuffers = n_dma_bufs;
1486         return ret;
1487 }
1488
1489 static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
1490                 struct v4l2_frmsizeenum *sizes)
1491 {
1492         struct mcam_camera *cam = video_drvdata(filp);
1493         struct mcam_format_struct *f;
1494         struct v4l2_subdev_frame_size_enum fse = {
1495                 .index = sizes->index,
1496                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1497         };
1498         int ret;
1499
1500         f = mcam_find_format(sizes->pixel_format);
1501         if (f->pixelformat != sizes->pixel_format)
1502                 return -EINVAL;
1503         fse.code = f->mbus_code;
1504         ret = sensor_call(cam, pad, enum_frame_size, NULL, &fse);
1505         if (ret)
1506                 return ret;
1507         if (fse.min_width == fse.max_width &&
1508             fse.min_height == fse.max_height) {
1509                 sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1510                 sizes->discrete.width = fse.min_width;
1511                 sizes->discrete.height = fse.min_height;
1512                 return 0;
1513         }
1514         sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
1515         sizes->stepwise.min_width = fse.min_width;
1516         sizes->stepwise.max_width = fse.max_width;
1517         sizes->stepwise.min_height = fse.min_height;
1518         sizes->stepwise.max_height = fse.max_height;
1519         sizes->stepwise.step_width = 1;
1520         sizes->stepwise.step_height = 1;
1521         return 0;
1522 }
1523
1524 static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
1525                 struct v4l2_frmivalenum *interval)
1526 {
1527         struct mcam_camera *cam = video_drvdata(filp);
1528         struct mcam_format_struct *f;
1529         struct v4l2_subdev_frame_interval_enum fie = {
1530                 .index = interval->index,
1531                 .width = interval->width,
1532                 .height = interval->height,
1533                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1534         };
1535         int ret;
1536
1537         f = mcam_find_format(interval->pixel_format);
1538         if (f->pixelformat != interval->pixel_format)
1539                 return -EINVAL;
1540         fie.code = f->mbus_code;
1541         ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
1542         if (ret)
1543                 return ret;
1544         interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1545         interval->discrete = fie.interval;
1546         return 0;
1547 }
1548
1549 #ifdef CONFIG_VIDEO_ADV_DEBUG
1550 static int mcam_vidioc_g_register(struct file *file, void *priv,
1551                 struct v4l2_dbg_register *reg)
1552 {
1553         struct mcam_camera *cam = video_drvdata(file);
1554
1555         if (reg->reg > cam->regs_size - 4)
1556                 return -EINVAL;
1557         reg->val = mcam_reg_read(cam, reg->reg);
1558         reg->size = 4;
1559         return 0;
1560 }
1561
1562 static int mcam_vidioc_s_register(struct file *file, void *priv,
1563                 const struct v4l2_dbg_register *reg)
1564 {
1565         struct mcam_camera *cam = video_drvdata(file);
1566
1567         if (reg->reg > cam->regs_size - 4)
1568                 return -EINVAL;
1569         mcam_reg_write(cam, reg->reg, reg->val);
1570         return 0;
1571 }
1572 #endif
1573
1574 static const struct v4l2_ioctl_ops mcam_v4l_ioctl_ops = {
1575         .vidioc_querycap        = mcam_vidioc_querycap,
1576         .vidioc_enum_fmt_vid_cap = mcam_vidioc_enum_fmt_vid_cap,
1577         .vidioc_try_fmt_vid_cap = mcam_vidioc_try_fmt_vid_cap,
1578         .vidioc_s_fmt_vid_cap   = mcam_vidioc_s_fmt_vid_cap,
1579         .vidioc_g_fmt_vid_cap   = mcam_vidioc_g_fmt_vid_cap,
1580         .vidioc_enum_input      = mcam_vidioc_enum_input,
1581         .vidioc_g_input         = mcam_vidioc_g_input,
1582         .vidioc_s_input         = mcam_vidioc_s_input,
1583         .vidioc_reqbufs         = vb2_ioctl_reqbufs,
1584         .vidioc_create_bufs     = vb2_ioctl_create_bufs,
1585         .vidioc_querybuf        = vb2_ioctl_querybuf,
1586         .vidioc_qbuf            = vb2_ioctl_qbuf,
1587         .vidioc_dqbuf           = vb2_ioctl_dqbuf,
1588         .vidioc_expbuf          = vb2_ioctl_expbuf,
1589         .vidioc_streamon        = vb2_ioctl_streamon,
1590         .vidioc_streamoff       = vb2_ioctl_streamoff,
1591         .vidioc_g_parm          = mcam_vidioc_g_parm,
1592         .vidioc_s_parm          = mcam_vidioc_s_parm,
1593         .vidioc_enum_framesizes = mcam_vidioc_enum_framesizes,
1594         .vidioc_enum_frameintervals = mcam_vidioc_enum_frameintervals,
1595         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1596         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1597 #ifdef CONFIG_VIDEO_ADV_DEBUG
1598         .vidioc_g_register      = mcam_vidioc_g_register,
1599         .vidioc_s_register      = mcam_vidioc_s_register,
1600 #endif
1601 };
1602
1603 /* ---------------------------------------------------------------------- */
1604 /*
1605  * Our various file operations.
1606  */
1607 static int mcam_v4l_open(struct file *filp)
1608 {
1609         struct mcam_camera *cam = video_drvdata(filp);
1610         int ret;
1611
1612         mutex_lock(&cam->s_mutex);
1613         ret = v4l2_fh_open(filp);
1614         if (ret)
1615                 goto out;
1616         if (v4l2_fh_is_singular_file(filp)) {
1617                 ret = mcam_ctlr_power_up(cam);
1618                 if (ret)
1619                         goto out;
1620                 __mcam_cam_reset(cam);
1621                 mcam_set_config_needed(cam, 1);
1622         }
1623 out:
1624         mutex_unlock(&cam->s_mutex);
1625         if (ret)
1626                 v4l2_fh_release(filp);
1627         return ret;
1628 }
1629
1630
1631 static int mcam_v4l_release(struct file *filp)
1632 {
1633         struct mcam_camera *cam = video_drvdata(filp);
1634         bool last_open;
1635
1636         mutex_lock(&cam->s_mutex);
1637         last_open = v4l2_fh_is_singular_file(filp);
1638         _vb2_fop_release(filp, NULL);
1639         if (last_open) {
1640                 mcam_disable_mipi(cam);
1641                 mcam_ctlr_power_down(cam);
1642                 if (cam->buffer_mode == B_vmalloc && alloc_bufs_at_read)
1643                         mcam_free_dma_bufs(cam);
1644         }
1645
1646         mutex_unlock(&cam->s_mutex);
1647         return 0;
1648 }
1649
1650 static const struct v4l2_file_operations mcam_v4l_fops = {
1651         .owner = THIS_MODULE,
1652         .open = mcam_v4l_open,
1653         .release = mcam_v4l_release,
1654         .read = vb2_fop_read,
1655         .poll = vb2_fop_poll,
1656         .mmap = vb2_fop_mmap,
1657         .unlocked_ioctl = video_ioctl2,
1658 };
1659
1660
1661 /*
1662  * This template device holds all of those v4l2 methods; we
1663  * clone it for specific real devices.
1664  */
1665 static struct video_device mcam_v4l_template = {
1666         .name = "mcam",
1667         .fops = &mcam_v4l_fops,
1668         .ioctl_ops = &mcam_v4l_ioctl_ops,
1669         .release = video_device_release_empty,
1670 };
1671
1672 /* ---------------------------------------------------------------------- */
1673 /*
1674  * Interrupt handler stuff
1675  */
1676 static void mcam_frame_complete(struct mcam_camera *cam, int frame)
1677 {
1678         /*
1679          * Basic frame housekeeping.
1680          */
1681         set_bit(frame, &cam->flags);
1682         clear_bit(CF_DMA_ACTIVE, &cam->flags);
1683         cam->next_buf = frame;
1684         cam->buf_seq[frame] = cam->sequence++;
1685         cam->frame_state.frames++;
1686         /*
1687          * "This should never happen"
1688          */
1689         if (cam->state != S_STREAMING)
1690                 return;
1691         /*
1692          * Process the frame and set up the next one.
1693          */
1694         cam->frame_complete(cam, frame);
1695 }
1696
1697
1698 /*
1699  * The interrupt handler; this needs to be called from the
1700  * platform irq handler with the lock held.
1701  */
1702 int mccic_irq(struct mcam_camera *cam, unsigned int irqs)
1703 {
1704         unsigned int frame, handled = 0;
1705
1706         mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS); /* Clear'em all */
1707         /*
1708          * Handle any frame completions.  There really should
1709          * not be more than one of these, or we have fallen
1710          * far behind.
1711          *
1712          * When running in S/G mode, the frame number lacks any
1713          * real meaning - there's only one descriptor array - but
1714          * the controller still picks a different one to signal
1715          * each time.
1716          */
1717         for (frame = 0; frame < cam->nbufs; frame++)
1718                 if (irqs & (IRQ_EOF0 << frame) &&
1719                         test_bit(CF_FRAME_SOF0 + frame, &cam->flags)) {
1720                         mcam_frame_complete(cam, frame);
1721                         handled = 1;
1722                         clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
1723                         if (cam->buffer_mode == B_DMA_sg)
1724                                 break;
1725                 }
1726         /*
1727          * If a frame starts, note that we have DMA active.  This
1728          * code assumes that we won't get multiple frame interrupts
1729          * at once; may want to rethink that.
1730          */
1731         for (frame = 0; frame < cam->nbufs; frame++) {
1732                 if (irqs & (IRQ_SOF0 << frame)) {
1733                         set_bit(CF_FRAME_SOF0 + frame, &cam->flags);
1734                         handled = IRQ_HANDLED;
1735                 }
1736         }
1737
1738         if (handled == IRQ_HANDLED) {
1739                 set_bit(CF_DMA_ACTIVE, &cam->flags);
1740                 if (cam->buffer_mode == B_DMA_sg)
1741                         mcam_ctlr_stop(cam);
1742         }
1743         return handled;
1744 }
1745
1746 /* ---------------------------------------------------------------------- */
1747 /*
1748  * Registration and such.
1749  */
1750 static struct ov7670_config sensor_cfg = {
1751         /*
1752          * Exclude QCIF mode, because it only captures a tiny portion
1753          * of the sensor FOV
1754          */
1755         .min_width = 320,
1756         .min_height = 240,
1757 };
1758
1759
1760 int mccic_register(struct mcam_camera *cam)
1761 {
1762         struct i2c_board_info ov7670_info = {
1763                 .type = "ov7670",
1764                 .addr = 0x42 >> 1,
1765                 .platform_data = &sensor_cfg,
1766         };
1767         int ret;
1768
1769         /*
1770          * Validate the requested buffer mode.
1771          */
1772         if (buffer_mode >= 0)
1773                 cam->buffer_mode = buffer_mode;
1774         if (cam->buffer_mode == B_DMA_sg &&
1775                         cam->chip_id == MCAM_CAFE) {
1776                 printk(KERN_ERR "marvell-cam: Cafe can't do S/G I/O, "
1777                         "attempting vmalloc mode instead\n");
1778                 cam->buffer_mode = B_vmalloc;
1779         }
1780         if (!mcam_buffer_mode_supported(cam->buffer_mode)) {
1781                 printk(KERN_ERR "marvell-cam: buffer mode %d unsupported\n",
1782                                 cam->buffer_mode);
1783                 return -EINVAL;
1784         }
1785         /*
1786          * Register with V4L
1787          */
1788         ret = v4l2_device_register(cam->dev, &cam->v4l2_dev);
1789         if (ret)
1790                 return ret;
1791
1792         mutex_init(&cam->s_mutex);
1793         cam->state = S_NOTREADY;
1794         mcam_set_config_needed(cam, 1);
1795         cam->pix_format = mcam_def_pix_format;
1796         cam->mbus_code = mcam_def_mbus_code;
1797         mcam_ctlr_init(cam);
1798
1799         /*
1800          * Get the v4l2 setup done.
1801          */
1802         ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
1803         if (ret)
1804                 goto out_unregister;
1805         cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
1806
1807         /*
1808          * Try to find the sensor.
1809          */
1810         sensor_cfg.clock_speed = cam->clock_speed;
1811         sensor_cfg.use_smbus = cam->use_smbus;
1812         cam->sensor_addr = ov7670_info.addr;
1813         cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev,
1814                         cam->i2c_adapter, &ov7670_info, NULL);
1815         if (cam->sensor == NULL) {
1816                 ret = -ENODEV;
1817                 goto out_unregister;
1818         }
1819
1820         ret = mcam_cam_init(cam);
1821         if (ret)
1822                 goto out_unregister;
1823
1824         ret = mcam_setup_vb2(cam);
1825         if (ret)
1826                 goto out_unregister;
1827
1828         mutex_lock(&cam->s_mutex);
1829         cam->vdev = mcam_v4l_template;
1830         cam->vdev.v4l2_dev = &cam->v4l2_dev;
1831         cam->vdev.lock = &cam->s_mutex;
1832         cam->vdev.queue = &cam->vb_queue;
1833         video_set_drvdata(&cam->vdev, cam);
1834         ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
1835         if (ret) {
1836                 mutex_unlock(&cam->s_mutex);
1837                 goto out_unregister;
1838         }
1839
1840         /*
1841          * If so requested, try to get our DMA buffers now.
1842          */
1843         if (cam->buffer_mode == B_vmalloc && !alloc_bufs_at_read) {
1844                 if (mcam_alloc_dma_bufs(cam, 1))
1845                         cam_warn(cam, "Unable to alloc DMA buffers at load"
1846                                         " will try again later.");
1847         }
1848
1849         mutex_unlock(&cam->s_mutex);
1850         return 0;
1851
1852 out_unregister:
1853         v4l2_ctrl_handler_free(&cam->ctrl_handler);
1854         v4l2_device_unregister(&cam->v4l2_dev);
1855         return ret;
1856 }
1857
1858
1859 void mccic_shutdown(struct mcam_camera *cam)
1860 {
1861         /*
1862          * If we have no users (and we really, really should have no
1863          * users) the device will already be powered down.  Trying to
1864          * take it down again will wedge the machine, which is frowned
1865          * upon.
1866          */
1867         if (!list_empty(&cam->vdev.fh_list)) {
1868                 cam_warn(cam, "Removing a device with users!\n");
1869                 mcam_ctlr_power_down(cam);
1870         }
1871         mcam_cleanup_vb2(cam);
1872         if (cam->buffer_mode == B_vmalloc)
1873                 mcam_free_dma_bufs(cam);
1874         video_unregister_device(&cam->vdev);
1875         v4l2_ctrl_handler_free(&cam->ctrl_handler);
1876         v4l2_device_unregister(&cam->v4l2_dev);
1877 }
1878
1879 /*
1880  * Power management
1881  */
1882 #ifdef CONFIG_PM
1883
1884 void mccic_suspend(struct mcam_camera *cam)
1885 {
1886         mutex_lock(&cam->s_mutex);
1887         if (!list_empty(&cam->vdev.fh_list)) {
1888                 enum mcam_state cstate = cam->state;
1889
1890                 mcam_ctlr_stop_dma(cam);
1891                 mcam_ctlr_power_down(cam);
1892                 cam->state = cstate;
1893         }
1894         mutex_unlock(&cam->s_mutex);
1895 }
1896
1897 int mccic_resume(struct mcam_camera *cam)
1898 {
1899         int ret = 0;
1900
1901         mutex_lock(&cam->s_mutex);
1902         if (!list_empty(&cam->vdev.fh_list)) {
1903                 ret = mcam_ctlr_power_up(cam);
1904                 if (ret) {
1905                         mutex_unlock(&cam->s_mutex);
1906                         return ret;
1907                 }
1908                 __mcam_cam_reset(cam);
1909         } else {
1910                 mcam_ctlr_power_down(cam);
1911         }
1912         mutex_unlock(&cam->s_mutex);
1913
1914         set_bit(CF_CONFIG_NEEDED, &cam->flags);
1915         if (cam->state == S_STREAMING) {
1916                 /*
1917                  * If there was a buffer in the DMA engine at suspend
1918                  * time, put it back on the queue or we'll forget about it.
1919                  */
1920                 if (cam->buffer_mode == B_DMA_sg && cam->vb_bufs[0])
1921                         list_add(&cam->vb_bufs[0]->queue, &cam->buffers);
1922                 ret = mcam_read_setup(cam);
1923         }
1924         return ret;
1925 }
1926 #endif /* CONFIG_PM */