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