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