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

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