drivers/media/video/omap1_camera.c

   1 /*
   2  * V4L2 SoC Camera driver for OMAP1 Camera Interface
   3  *
   4  * Copyright (C) 2010, Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>
   5  *
   6  * Based on V4L2 Driver for i.MXL/i.MXL camera (CSI) host
   7  * Copyright (C) 2008, Paulius Zaleckas <paulius.zaleckas@teltonika.lt>
   8  * Copyright (C) 2009, Darius Augulis <augulis.darius@gmail.com>
   9  *
  10  * Based on PXA SoC camera driver
  11  * Copyright (C) 2006, Sascha Hauer, Pengutronix
  12  * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
  13  *
  14  * Hardware specific bits initialy based on former work by Matt Callow
  15  * drivers/media/video/omap/omap1510cam.c
  16  * Copyright (C) 2006 Matt Callow
  17  *
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License version 2 as
  20  * published by the Free Software Foundation.
  21  */
  22
  23
  24 #include <linux/clk.h>
  25 #include <linux/dma-mapping.h>
  26 #include <linux/interrupt.h>
  27 #include <linux/platform_device.h>
  28 #include <linux/slab.h>
  29
  30 #include <media/omap1_camera.h>
  31 #include <media/soc_camera.h>
  32 #include <media/soc_mediabus.h>
  33 #include <media/videobuf-dma-contig.h>
  34 #include <media/videobuf-dma-sg.h>
  35
  36 #include <plat/dma.h>
  37
  38
  39 #define DRIVER_NAME             "omap1-camera"
  40 #define DRIVER_VERSION          "0.0.2"
  41
  42
  43 /*
  44  * ---------------------------------------------------------------------------
  45  *  OMAP1 Camera Interface registers
  46  * ---------------------------------------------------------------------------
  47  */
  48
  49 #define REG_CTRLCLOCK           0x00
  50 #define REG_IT_STATUS           0x04
  51 #define REG_MODE                0x08
  52 #define REG_STATUS              0x0C
  53 #define REG_CAMDATA             0x10
  54 #define REG_GPIO                0x14
  55 #define REG_PEAK_COUNTER        0x18
  56
  57 /* CTRLCLOCK bit shifts */
  58 #define LCLK_EN                 BIT(7)
  59 #define DPLL_EN                 BIT(6)
  60 #define MCLK_EN                 BIT(5)
  61 #define CAMEXCLK_EN             BIT(4)
  62 #define POLCLK                  BIT(3)
  63 #define FOSCMOD_SHIFT           0
  64 #define FOSCMOD_MASK            (0x7 << FOSCMOD_SHIFT)
  65 #define FOSCMOD_12MHz           0x0
  66 #define FOSCMOD_6MHz            0x2
  67 #define FOSCMOD_9_6MHz          0x4
  68 #define FOSCMOD_24MHz           0x5
  69 #define FOSCMOD_8MHz            0x6
  70
  71 /* IT_STATUS bit shifts */
  72 #define DATA_TRANSFER           BIT(5)
  73 #define FIFO_FULL               BIT(4)
  74 #define H_DOWN                  BIT(3)
  75 #define H_UP                    BIT(2)
  76 #define V_DOWN                  BIT(1)
  77 #define V_UP                    BIT(0)
  78
  79 /* MODE bit shifts */
  80 #define RAZ_FIFO                BIT(18)
  81 #define EN_FIFO_FULL            BIT(17)
  82 #define EN_NIRQ                 BIT(16)
  83 #define THRESHOLD_SHIFT         9
  84 #define THRESHOLD_MASK          (0x7f << THRESHOLD_SHIFT)
  85 #define DMA                     BIT(8)
  86 #define EN_H_DOWN               BIT(7)
  87 #define EN_H_UP                 BIT(6)
  88 #define EN_V_DOWN               BIT(5)
  89 #define EN_V_UP                 BIT(4)
  90 #define ORDERCAMD               BIT(3)
  91
  92 #define IRQ_MASK                (EN_V_UP | EN_V_DOWN | EN_H_UP | EN_H_DOWN | \
  93                                  EN_NIRQ | EN_FIFO_FULL)
  94
  95 /* STATUS bit shifts */
  96 #define HSTATUS                 BIT(1)
  97 #define VSTATUS                 BIT(0)
  98
  99 /* GPIO bit shifts */
 100 #define CAM_RST                 BIT(0)
 101
 102 /* end of OMAP1 Camera Interface registers */
 103
 104
 105 #define SOCAM_BUS_FLAGS (V4L2_MBUS_MASTER | \
 106                         V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
 107                         V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
 108                         V4L2_MBUS_DATA_ACTIVE_HIGH)
 109
 110
 111 #define FIFO_SIZE               ((THRESHOLD_MASK >> THRESHOLD_SHIFT) + 1)
 112 #define FIFO_SHIFT              __fls(FIFO_SIZE)
 113
 114 #define DMA_BURST_SHIFT         (1 + OMAP_DMA_DATA_BURST_4)
 115 #define DMA_BURST_SIZE          (1 << DMA_BURST_SHIFT)
 116
 117 #define DMA_ELEMENT_SHIFT       OMAP_DMA_DATA_TYPE_S32
 118 #define DMA_ELEMENT_SIZE        (1 << DMA_ELEMENT_SHIFT)
 119
 120 #define DMA_FRAME_SHIFT_CONTIG  (FIFO_SHIFT - 1)
 121 #define DMA_FRAME_SHIFT_SG      DMA_BURST_SHIFT
 122
 123 #define DMA_FRAME_SHIFT(x)      ((x) == OMAP1_CAM_DMA_CONTIG ? \
 124                                                 DMA_FRAME_SHIFT_CONTIG : \
 125                                                 DMA_FRAME_SHIFT_SG)
 126 #define DMA_FRAME_SIZE(x)       (1 << DMA_FRAME_SHIFT(x))
 127 #define DMA_SYNC                OMAP_DMA_SYNC_FRAME
 128 #define THRESHOLD_LEVEL         DMA_FRAME_SIZE
 129
 130
 131 #define MAX_VIDEO_MEM           4       /* arbitrary video memory limit in MB */
 132
 133
 134 /*
 135  * Structures
 136  */
 137
 138 /* buffer for one video frame */
 139 struct omap1_cam_buf {
 140         struct videobuf_buffer          vb;
 141         enum v4l2_mbus_pixelcode        code;
 142         int                             inwork;
 143         struct scatterlist              *sgbuf;
 144         int                             sgcount;
 145         int                             bytes_left;
 146         enum videobuf_state             result;
 147 };
 148
 149 struct omap1_cam_dev {
 150         struct soc_camera_host          soc_host;
 151         struct soc_camera_device        *icd;
 152         struct clk                      *clk;
 153
 154         unsigned int                    irq;
 155         void __iomem                    *base;
 156
 157         int                             dma_ch;
 158
 159         struct omap1_cam_platform_data  *pdata;
 160         struct resource                 *res;
 161         unsigned long                   pflags;
 162         unsigned long                   camexclk;
 163
 164         struct list_head                capture;
 165
 166         /* lock used to protect videobuf */
 167         spinlock_t                      lock;
 168
 169         /* Pointers to DMA buffers */
 170         struct omap1_cam_buf            *active;
 171         struct omap1_cam_buf            *ready;
 172
 173         enum omap1_cam_vb_mode          vb_mode;
 174         int                             (*mmap_mapper)(struct videobuf_queue *q,
 175                                                 struct videobuf_buffer *buf,
 176                                                 struct vm_area_struct *vma);
 177
 178         u32                             reg_cache[0];
 179 };
 180
 181
 182 static void cam_write(struct omap1_cam_dev *pcdev, u16 reg, u32 val)
 183 {
 184         pcdev->reg_cache[reg / sizeof(u32)] = val;
 185         __raw_writel(val, pcdev->base + reg);
 186 }
 187
 188 static u32 cam_read(struct omap1_cam_dev *pcdev, u16 reg, bool from_cache)
 189 {
 190         return !from_cache ? __raw_readl(pcdev->base + reg) :
 191                         pcdev->reg_cache[reg / sizeof(u32)];
 192 }
 193
 194 #define CAM_READ(pcdev, reg) \
 195                 cam_read(pcdev, REG_##reg, false)
 196 #define CAM_WRITE(pcdev, reg, val) \
 197                 cam_write(pcdev, REG_##reg, val)
 198 #define CAM_READ_CACHE(pcdev, reg) \
 199                 cam_read(pcdev, REG_##reg, true)
 200
 201 /*
 202  *  Videobuf operations
 203  */
 204 static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 205                 unsigned int *size)
 206 {
 207         struct soc_camera_device *icd = vq->priv_data;
 208         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 209                         icd->current_fmt->host_fmt);
 210         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 211         struct omap1_cam_dev *pcdev = ici->priv;
 212
 213         if (bytes_per_line < 0)
 214                 return bytes_per_line;
 215
 216         *size = bytes_per_line * icd->user_height;
 217
 218         if (!*count || *count < OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode))
 219                 *count = OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode);
 220
 221         if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
 222                 *count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
 223
 224         dev_dbg(icd->parent,
 225                         "%s: count=%d, size=%d\n", __func__, *count, *size);
 226
 227         return 0;
 228 }
 229
 230 static void free_buffer(struct videobuf_queue *vq, struct omap1_cam_buf *buf,
 231                 enum omap1_cam_vb_mode vb_mode)
 232 {
 233         struct videobuf_buffer *vb = &buf->vb;
 234
 235         BUG_ON(in_interrupt());
 236
 237         videobuf_waiton(vq, vb, 0, 0);
 238
 239         if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
 240                 videobuf_dma_contig_free(vq, vb);
 241         } else {
 242                 struct soc_camera_device *icd = vq->priv_data;
 243                 struct device *dev = icd->parent;
 244                 struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
 245
 246                 videobuf_dma_unmap(dev, dma);
 247                 videobuf_dma_free(dma);
 248         }
 249
 250         vb->state = VIDEOBUF_NEEDS_INIT;
 251 }
 252
 253 static int omap1_videobuf_prepare(struct videobuf_queue *vq,
 254                 struct videobuf_buffer *vb, enum v4l2_field field)
 255 {
 256         struct soc_camera_device *icd = vq->priv_data;
 257         struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
 258         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 259                         icd->current_fmt->host_fmt);
 260         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 261         struct omap1_cam_dev *pcdev = ici->priv;
 262         int ret;
 263
 264         if (bytes_per_line < 0)
 265                 return bytes_per_line;
 266
 267         WARN_ON(!list_empty(&vb->queue));
 268
 269         BUG_ON(NULL == icd->current_fmt);
 270
 271         buf->inwork = 1;
 272
 273         if (buf->code != icd->current_fmt->code || vb->field != field ||
 274                         vb->width  != icd->user_width ||
 275                         vb->height != icd->user_height) {
 276                 buf->code  = icd->current_fmt->code;
 277                 vb->width  = icd->user_width;
 278                 vb->height = icd->user_height;
 279                 vb->field  = field;
 280                 vb->state  = VIDEOBUF_NEEDS_INIT;
 281         }
 282
 283         vb->size = bytes_per_line * vb->height;
 284
 285         if (vb->baddr && vb->bsize < vb->size) {
 286                 ret = -EINVAL;
 287                 goto out;
 288         }
 289
 290         if (vb->state == VIDEOBUF_NEEDS_INIT) {
 291                 ret = videobuf_iolock(vq, vb, NULL);
 292                 if (ret)
 293                         goto fail;
 294
 295                 vb->state = VIDEOBUF_PREPARED;
 296         }
 297         buf->inwork = 0;
 298
 299         return 0;
 300 fail:
 301         free_buffer(vq, buf, pcdev->vb_mode);
 302 out:
 303         buf->inwork = 0;
 304         return ret;
 305 }
 306
 307 static void set_dma_dest_params(int dma_ch, struct omap1_cam_buf *buf,
 308                 enum omap1_cam_vb_mode vb_mode)
 309 {
 310         dma_addr_t dma_addr;
 311         unsigned int block_size;
 312
 313         if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
 314                 dma_addr = videobuf_to_dma_contig(&buf->vb);
 315                 block_size = buf->vb.size;
 316         } else {
 317                 if (WARN_ON(!buf->sgbuf)) {
 318                         buf->result = VIDEOBUF_ERROR;
 319                         return;
 320                 }
 321                 dma_addr = sg_dma_address(buf->sgbuf);
 322                 if (WARN_ON(!dma_addr)) {
 323                         buf->sgbuf = NULL;
 324                         buf->result = VIDEOBUF_ERROR;
 325                         return;
 326                 }
 327                 block_size = sg_dma_len(buf->sgbuf);
 328                 if (WARN_ON(!block_size)) {
 329                         buf->sgbuf = NULL;
 330                         buf->result = VIDEOBUF_ERROR;
 331                         return;
 332                 }
 333                 if (unlikely(buf->bytes_left < block_size))
 334                         block_size = buf->bytes_left;
 335                 if (WARN_ON(dma_addr & (DMA_FRAME_SIZE(vb_mode) *
 336                                 DMA_ELEMENT_SIZE - 1))) {
 337                         dma_addr = ALIGN(dma_addr, DMA_FRAME_SIZE(vb_mode) *
 338                                         DMA_ELEMENT_SIZE);
 339                         block_size &= ~(DMA_FRAME_SIZE(vb_mode) *
 340                                         DMA_ELEMENT_SIZE - 1);
 341                 }
 342                 buf->bytes_left -= block_size;
 343                 buf->sgcount++;
 344         }
 345
 346         omap_set_dma_dest_params(dma_ch,
 347                 OMAP_DMA_PORT_EMIFF, OMAP_DMA_AMODE_POST_INC, dma_addr, 0, 0);
 348         omap_set_dma_transfer_params(dma_ch,
 349                 OMAP_DMA_DATA_TYPE_S32, DMA_FRAME_SIZE(vb_mode),
 350                 block_size >> (DMA_FRAME_SHIFT(vb_mode) + DMA_ELEMENT_SHIFT),
 351                 DMA_SYNC, 0, 0);
 352 }
 353
 354 static struct omap1_cam_buf *prepare_next_vb(struct omap1_cam_dev *pcdev)
 355 {
 356         struct omap1_cam_buf *buf;
 357
 358         /*
 359          * If there is already a buffer pointed out by the pcdev->ready,
 360          * (re)use it, otherwise try to fetch and configure a new one.
 361          */
 362         buf = pcdev->ready;
 363         if (!buf) {
 364                 if (list_empty(&pcdev->capture))
 365                         return buf;
 366                 buf = list_entry(pcdev->capture.next,
 367                                 struct omap1_cam_buf, vb.queue);
 368                 buf->vb.state = VIDEOBUF_ACTIVE;
 369                 pcdev->ready = buf;
 370                 list_del_init(&buf->vb.queue);
 371         }
 372
 373         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
 374                 /*
 375                  * In CONTIG mode, we can safely enter next buffer parameters
 376                  * into the DMA programming register set after the DMA
 377                  * has already been activated on the previous buffer
 378                  */
 379                 set_dma_dest_params(pcdev->dma_ch, buf, pcdev->vb_mode);
 380         } else {
 381                 /*
 382                  * In SG mode, the above is not safe since there are probably
 383                  * a bunch of sgbufs from previous sglist still pending.
 384                  * Instead, mark the sglist fresh for the upcoming
 385                  * try_next_sgbuf().
 386                  */
 387                 buf->sgbuf = NULL;
 388         }
 389
 390         return buf;
 391 }
 392
 393 static struct scatterlist *try_next_sgbuf(int dma_ch, struct omap1_cam_buf *buf)
 394 {
 395         struct scatterlist *sgbuf;
 396
 397         if (likely(buf->sgbuf)) {
 398                 /* current sglist is active */
 399                 if (unlikely(!buf->bytes_left)) {
 400                         /* indicate sglist complete */
 401                         sgbuf = NULL;
 402                 } else {
 403                         /* process next sgbuf */
 404                         sgbuf = sg_next(buf->sgbuf);
 405                         if (WARN_ON(!sgbuf)) {
 406                                 buf->result = VIDEOBUF_ERROR;
 407                         } else if (WARN_ON(!sg_dma_len(sgbuf))) {
 408                                 sgbuf = NULL;
 409                                 buf->result = VIDEOBUF_ERROR;
 410                         }
 411                 }
 412                 buf->sgbuf = sgbuf;
 413         } else {
 414                 /* sglist is fresh, initialize it before using */
 415                 struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
 416
 417                 sgbuf = dma->sglist;
 418                 if (!(WARN_ON(!sgbuf))) {
 419                         buf->sgbuf = sgbuf;
 420                         buf->sgcount = 0;
 421                         buf->bytes_left = buf->vb.size;
 422                         buf->result = VIDEOBUF_DONE;
 423                 }
 424         }
 425         if (sgbuf)
 426                 /*
 427                  * Put our next sgbuf parameters (address, size)
 428                  * into the DMA programming register set.
 429                  */
 430                 set_dma_dest_params(dma_ch, buf, OMAP1_CAM_DMA_SG);
 431
 432         return sgbuf;
 433 }
 434
 435 static void start_capture(struct omap1_cam_dev *pcdev)
 436 {
 437         struct omap1_cam_buf *buf = pcdev->active;
 438         u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
 439         u32 mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN;
 440
 441         if (WARN_ON(!buf))
 442                 return;
 443
 444         /*
 445          * Enable start of frame interrupt, which we will use for activating
 446          * our end of frame watchdog when capture actually starts.
 447          */
 448         mode |= EN_V_UP;
 449
 450         if (unlikely(ctrlclock & LCLK_EN))
 451                 /* stop pixel clock before FIFO reset */
 452                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
 453         /* reset FIFO */
 454         CAM_WRITE(pcdev, MODE, mode | RAZ_FIFO);
 455
 456         omap_start_dma(pcdev->dma_ch);
 457
 458         if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
 459                 /*
 460                  * In SG mode, it's a good moment for fetching next sgbuf
 461                  * from the current sglist and, if available, already putting
 462                  * its parameters into the DMA programming register set.
 463                  */
 464                 try_next_sgbuf(pcdev->dma_ch, buf);
 465         }
 466
 467         /* (re)enable pixel clock */
 468         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | LCLK_EN);
 469         /* release FIFO reset */
 470         CAM_WRITE(pcdev, MODE, mode);
 471 }
 472
 473 static void suspend_capture(struct omap1_cam_dev *pcdev)
 474 {
 475         u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
 476
 477         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
 478         omap_stop_dma(pcdev->dma_ch);
 479 }
 480
 481 static void disable_capture(struct omap1_cam_dev *pcdev)
 482 {
 483         u32 mode = CAM_READ_CACHE(pcdev, MODE);
 484
 485         CAM_WRITE(pcdev, MODE, mode & ~(IRQ_MASK | DMA));
 486 }
 487
 488 static void omap1_videobuf_queue(struct videobuf_queue *vq,
 489                                                 struct videobuf_buffer *vb)
 490 {
 491         struct soc_camera_device *icd = vq->priv_data;
 492         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 493         struct omap1_cam_dev *pcdev = ici->priv;
 494         struct omap1_cam_buf *buf;
 495         u32 mode;
 496
 497         list_add_tail(&vb->queue, &pcdev->capture);
 498         vb->state = VIDEOBUF_QUEUED;
 499
 500         if (pcdev->active) {
 501                 /*
 502                  * Capture in progress, so don't touch pcdev->ready even if
 503                  * empty. Since the transfer of the DMA programming register set
 504                  * content to the DMA working register set is done automatically
 505                  * by the DMA hardware, this can pretty well happen while we
 506                  * are keeping the lock here. Leave fetching it from the queue
 507                  * to be done when a next DMA interrupt occures instead.
 508                  */
 509                 return;
 510         }
 511
 512         WARN_ON(pcdev->ready);
 513
 514         buf = prepare_next_vb(pcdev);
 515         if (WARN_ON(!buf))
 516                 return;
 517
 518         pcdev->active = buf;
 519         pcdev->ready = NULL;
 520
 521         dev_dbg(icd->parent,
 522                 "%s: capture not active, setup FIFO, start DMA\n", __func__);
 523         mode = CAM_READ_CACHE(pcdev, MODE) & ~THRESHOLD_MASK;
 524         mode |= THRESHOLD_LEVEL(pcdev->vb_mode) << THRESHOLD_SHIFT;
 525         CAM_WRITE(pcdev, MODE, mode | EN_FIFO_FULL | DMA);
 526
 527         if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
 528                 /*
 529                  * In SG mode, the above prepare_next_vb() didn't actually
 530                  * put anything into the DMA programming register set,
 531                  * so we have to do it now, before activating DMA.
 532                  */
 533                 try_next_sgbuf(pcdev->dma_ch, buf);
 534         }
 535
 536         start_capture(pcdev);
 537 }
 538
 539 static void omap1_videobuf_release(struct videobuf_queue *vq,
 540                                  struct videobuf_buffer *vb)
 541 {
 542         struct omap1_cam_buf *buf =
 543                         container_of(vb, struct omap1_cam_buf, vb);
 544         struct soc_camera_device *icd = vq->priv_data;
 545         struct device *dev = icd->parent;
 546         struct soc_camera_host *ici = to_soc_camera_host(dev);
 547         struct omap1_cam_dev *pcdev = ici->priv;
 548
 549         switch (vb->state) {
 550         case VIDEOBUF_DONE:
 551                 dev_dbg(dev, "%s (done)\n", __func__);
 552                 break;
 553         case VIDEOBUF_ACTIVE:
 554                 dev_dbg(dev, "%s (active)\n", __func__);
 555                 break;
 556         case VIDEOBUF_QUEUED:
 557                 dev_dbg(dev, "%s (queued)\n", __func__);
 558                 break;
 559         case VIDEOBUF_PREPARED:
 560                 dev_dbg(dev, "%s (prepared)\n", __func__);
 561                 break;
 562         default:
 563                 dev_dbg(dev, "%s (unknown %d)\n", __func__, vb->state);
 564                 break;
 565         }
 566
 567         free_buffer(vq, buf, pcdev->vb_mode);
 568 }
 569
 570 static void videobuf_done(struct omap1_cam_dev *pcdev,
 571                 enum videobuf_state result)
 572 {
 573         struct omap1_cam_buf *buf = pcdev->active;
 574         struct videobuf_buffer *vb;
 575         struct device *dev = pcdev->icd->parent;
 576
 577         if (WARN_ON(!buf)) {
 578                 suspend_capture(pcdev);
 579                 disable_capture(pcdev);
 580                 return;
 581         }
 582
 583         if (result == VIDEOBUF_ERROR)
 584                 suspend_capture(pcdev);
 585
 586         vb = &buf->vb;
 587         if (waitqueue_active(&vb->done)) {
 588                 if (!pcdev->ready && result != VIDEOBUF_ERROR) {
 589                         /*
 590                          * No next buffer has been entered into the DMA
 591                          * programming register set on time (could be done only
 592                          * while the previous DMA interurpt was processed, not
 593                          * later), so the last DMA block, be it a whole buffer
 594                          * if in CONTIG or its last sgbuf if in SG mode, is
 595                          * about to be reused by the just autoreinitialized DMA
 596                          * engine, and overwritten with next frame data. Best we
 597                          * can do is stopping the capture as soon as possible,
 598                          * hopefully before the next frame start.
 599                          */
 600                         suspend_capture(pcdev);
 601                 }
 602                 vb->state = result;
 603                 do_gettimeofday(&vb->ts);
 604                 if (result != VIDEOBUF_ERROR)
 605                         vb->field_count++;
 606                 wake_up(&vb->done);
 607
 608                 /* shift in next buffer */
 609                 buf = pcdev->ready;
 610                 pcdev->active = buf;
 611                 pcdev->ready = NULL;
 612
 613                 if (!buf) {
 614                         /*
 615                          * No next buffer was ready on time (see above), so
 616                          * indicate error condition to force capture restart or
 617                          * stop, depending on next buffer already queued or not.
 618                          */
 619                         result = VIDEOBUF_ERROR;
 620                         prepare_next_vb(pcdev);
 621
 622                         buf = pcdev->ready;
 623                         pcdev->active = buf;
 624                         pcdev->ready = NULL;
 625                 }
 626         } else if (pcdev->ready) {
 627                 /*
 628                  * In both CONTIG and SG mode, the DMA engine has possibly
 629                  * been already autoreinitialized with the preprogrammed
 630                  * pcdev->ready buffer.  We can either accept this fact
 631                  * and just swap the buffers, or provoke an error condition
 632                  * and restart capture.  The former seems less intrusive.
 633                  */
 634                 dev_dbg(dev, "%s: nobody waiting on videobuf, swap with next\n",
 635                                 __func__);
 636                 pcdev->active = pcdev->ready;
 637
 638                 if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
 639                         /*
 640                          * In SG mode, we have to make sure that the buffer we
 641                          * are putting back into the pcdev->ready is marked
 642                          * fresh.
 643                          */
 644                         buf->sgbuf = NULL;
 645                 }
 646                 pcdev->ready = buf;
 647
 648                 buf = pcdev->active;
 649         } else {
 650                 /*
 651                  * No next buffer has been entered into
 652                  * the DMA programming register set on time.
 653                  */
 654                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
 655                         /*
 656                          * In CONTIG mode, the DMA engine has already been
 657                          * reinitialized with the current buffer. Best we can do
 658                          * is not touching it.
 659                          */
 660                         dev_dbg(dev,
 661                                 "%s: nobody waiting on videobuf, reuse it\n",
 662                                 __func__);
 663                 } else {
 664                         /*
 665                          * In SG mode, the DMA engine has just been
 666                          * autoreinitialized with the last sgbuf from the
 667                          * current list. Restart capture in order to transfer
 668                          * next frame start into the first sgbuf, not the last
 669                          * one.
 670                          */
 671                         if (result != VIDEOBUF_ERROR) {
 672                                 suspend_capture(pcdev);
 673                                 result = VIDEOBUF_ERROR;
 674                         }
 675                 }
 676         }
 677
 678         if (!buf) {
 679                 dev_dbg(dev, "%s: no more videobufs, stop capture\n", __func__);
 680                 disable_capture(pcdev);
 681                 return;
 682         }
 683
 684         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
 685                 /*
 686                  * In CONTIG mode, the current buffer parameters had already
 687                  * been entered into the DMA programming register set while the
 688                  * buffer was fetched with prepare_next_vb(), they may have also
 689                  * been transferred into the runtime set and already active if
 690                  * the DMA still running.
 691                  */
 692         } else {
 693                 /* In SG mode, extra steps are required */
 694                 if (result == VIDEOBUF_ERROR)
 695                         /* make sure we (re)use sglist from start on error */
 696                         buf->sgbuf = NULL;
 697
 698                 /*
 699                  * In any case, enter the next sgbuf parameters into the DMA
 700                  * programming register set.  They will be used either during
 701                  * nearest DMA autoreinitialization or, in case of an error,
 702                  * on DMA startup below.
 703                  */
 704                 try_next_sgbuf(pcdev->dma_ch, buf);
 705         }
 706
 707         if (result == VIDEOBUF_ERROR) {
 708                 dev_dbg(dev, "%s: videobuf error; reset FIFO, restart DMA\n",
 709                                 __func__);
 710                 start_capture(pcdev);
 711                 /*
 712                  * In SG mode, the above also resulted in the next sgbuf
 713                  * parameters being entered into the DMA programming register
 714                  * set, making them ready for next DMA autoreinitialization.
 715                  */
 716         }
 717
 718         /*
 719          * Finally, try fetching next buffer.
 720          * In CONTIG mode, it will also enter it into the DMA programming
 721          * register set, making it ready for next DMA autoreinitialization.
 722          */
 723         prepare_next_vb(pcdev);
 724 }
 725
 726 static void dma_isr(int channel, unsigned short status, void *data)
 727 {
 728         struct omap1_cam_dev *pcdev = data;
 729         struct omap1_cam_buf *buf = pcdev->active;
 730         unsigned long flags;
 731
 732         spin_lock_irqsave(&pcdev->lock, flags);
 733
 734         if (WARN_ON(!buf)) {
 735                 suspend_capture(pcdev);
 736                 disable_capture(pcdev);
 737                 goto out;
 738         }
 739
 740         if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
 741                 /*
 742                  * In CONTIG mode, assume we have just managed to collect the
 743                  * whole frame, hopefully before our end of frame watchdog is
 744                  * triggered. Then, all we have to do is disabling the watchdog
 745                  * for this frame, and calling videobuf_done() with success
 746                  * indicated.
 747                  */
 748                 CAM_WRITE(pcdev, MODE,
 749                                 CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN);
 750                 videobuf_done(pcdev, VIDEOBUF_DONE);
 751         } else {
 752                 /*
 753                  * In SG mode, we have to process every sgbuf from the current
 754                  * sglist, one after another.
 755                  */
 756                 if (buf->sgbuf) {
 757                         /*
 758                          * Current sglist not completed yet, try fetching next
 759                          * sgbuf, hopefully putting it into the DMA programming
 760                          * register set, making it ready for next DMA
 761                          * autoreinitialization.
 762                          */
 763                         try_next_sgbuf(pcdev->dma_ch, buf);
 764                         if (buf->sgbuf)
 765                                 goto out;
 766
 767                         /*
 768                          * No more sgbufs left in the current sglist. This
 769                          * doesn't mean that the whole videobuffer is already
 770                          * complete, but only that the last sgbuf from the
 771                          * current sglist is about to be filled. It will be
 772                          * ready on next DMA interrupt, signalled with the
 773                          * buf->sgbuf set back to NULL.
 774                          */
 775                         if (buf->result != VIDEOBUF_ERROR) {
 776                                 /*
 777                                  * Video frame collected without errors so far,
 778                                  * we can prepare for collecting a next one
 779                                  * as soon as DMA gets autoreinitialized
 780                                  * after the current (last) sgbuf is completed.
 781                                  */
 782                                 buf = prepare_next_vb(pcdev);
 783                                 if (!buf)
 784                                         goto out;
 785
 786                                 try_next_sgbuf(pcdev->dma_ch, buf);
 787                                 goto out;
 788                         }
 789                 }
 790                 /* end of videobuf */
 791                 videobuf_done(pcdev, buf->result);
 792         }
 793
 794 out:
 795         spin_unlock_irqrestore(&pcdev->lock, flags);
 796 }
 797
 798 static irqreturn_t cam_isr(int irq, void *data)
 799 {
 800         struct omap1_cam_dev *pcdev = data;
 801         struct device *dev = pcdev->icd->parent;
 802         struct omap1_cam_buf *buf = pcdev->active;
 803         u32 it_status;
 804         unsigned long flags;
 805
 806         it_status = CAM_READ(pcdev, IT_STATUS);
 807         if (!it_status)
 808                 return IRQ_NONE;
 809
 810         spin_lock_irqsave(&pcdev->lock, flags);
 811
 812         if (WARN_ON(!buf)) {
 813                 dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
 814                          __func__, it_status);
 815                 suspend_capture(pcdev);
 816                 disable_capture(pcdev);
 817                 goto out;
 818         }
 819
 820         if (unlikely(it_status & FIFO_FULL)) {
 821                 dev_warn(dev, "%s: FIFO overflow\n", __func__);
 822
 823         } else if (it_status & V_DOWN) {
 824                 /* end of video frame watchdog */
 825                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
 826                         /*
 827                          * In CONTIG mode, the watchdog is disabled with
 828                          * successful DMA end of block interrupt, and reenabled
 829                          * on next frame start. If we get here, there is nothing
 830                          * to check, we must be out of sync.
 831                          */
 832                 } else {
 833                         if (buf->sgcount == 2) {
 834                                 /*
 835                                  * If exactly 2 sgbufs from the next sglist have
 836                                  * been programmed into the DMA engine (the
 837                                  * first one already transferred into the DMA
 838                                  * runtime register set, the second one still
 839                                  * in the programming set), then we are in sync.
 840                                  */
 841                                 goto out;
 842                         }
 843                 }
 844                 dev_notice(dev, "%s: unexpected end of video frame\n",
 845                                 __func__);
 846
 847         } else if (it_status & V_UP) {
 848                 u32 mode;
 849
 850                 if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
 851                         /*
 852                          * In CONTIG mode, we need this interrupt every frame
 853                          * in oredr to reenable our end of frame watchdog.
 854                          */
 855                         mode = CAM_READ_CACHE(pcdev, MODE);
 856                 } else {
 857                         /*
 858                          * In SG mode, the below enabled end of frame watchdog
 859                          * is kept on permanently, so we can turn this one shot
 860                          * setup off.
 861                          */
 862                         mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_UP;
 863                 }
 864
 865                 if (!(mode & EN_V_DOWN)) {
 866                         /* (re)enable end of frame watchdog interrupt */
 867                         mode |= EN_V_DOWN;
 868                 }
 869                 CAM_WRITE(pcdev, MODE, mode);
 870                 goto out;
 871
 872         } else {
 873                 dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
 874                                 __func__, it_status);
 875                 goto out;
 876         }
 877
 878         videobuf_done(pcdev, VIDEOBUF_ERROR);
 879 out:
 880         spin_unlock_irqrestore(&pcdev->lock, flags);
 881         return IRQ_HANDLED;
 882 }
 883
 884 static struct videobuf_queue_ops omap1_videobuf_ops = {
 885         .buf_setup      = omap1_videobuf_setup,
 886         .buf_prepare    = omap1_videobuf_prepare,
 887         .buf_queue      = omap1_videobuf_queue,
 888         .buf_release    = omap1_videobuf_release,
 889 };
 890
 891
 892 /*
 893  * SOC Camera host operations
 894  */
 895
 896 static void sensor_reset(struct omap1_cam_dev *pcdev, bool reset)
 897 {
 898         /* apply/release camera sensor reset if requested by platform data */
 899         if (pcdev->pflags & OMAP1_CAMERA_RST_HIGH)
 900                 CAM_WRITE(pcdev, GPIO, reset);
 901         else if (pcdev->pflags & OMAP1_CAMERA_RST_LOW)
 902                 CAM_WRITE(pcdev, GPIO, !reset);
 903 }
 904
 905 /*
 906  * The following two functions absolutely depend on the fact, that
 907  * there can be only one camera on OMAP1 camera sensor interface
 908  */
 909 static int omap1_cam_add_device(struct soc_camera_device *icd)
 910 {
 911         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 912         struct omap1_cam_dev *pcdev = ici->priv;
 913         u32 ctrlclock;
 914
 915         if (pcdev->icd)
 916                 return -EBUSY;
 917
 918         clk_enable(pcdev->clk);
 919
 920         /* setup sensor clock */
 921         ctrlclock = CAM_READ(pcdev, CTRLCLOCK);
 922         ctrlclock &= ~(CAMEXCLK_EN | MCLK_EN | DPLL_EN);
 923         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
 924
 925         ctrlclock &= ~FOSCMOD_MASK;
 926         switch (pcdev->camexclk) {
 927         case 6000000:
 928                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_6MHz;
 929                 break;
 930         case 8000000:
 931                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_8MHz | DPLL_EN;
 932                 break;
 933         case 9600000:
 934                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_9_6MHz | DPLL_EN;
 935                 break;
 936         case 12000000:
 937                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_12MHz;
 938                 break;
 939         case 24000000:
 940                 ctrlclock |= CAMEXCLK_EN | FOSCMOD_24MHz | DPLL_EN;
 941         default:
 942                 break;
 943         }
 944         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~DPLL_EN);
 945
 946         /* enable internal clock */
 947         ctrlclock |= MCLK_EN;
 948         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
 949
 950         sensor_reset(pcdev, false);
 951
 952         pcdev->icd = icd;
 953
 954         dev_dbg(icd->parent, "OMAP1 Camera driver attached to camera %d\n",
 955                         icd->devnum);
 956         return 0;
 957 }
 958
 959 static void omap1_cam_remove_device(struct soc_camera_device *icd)
 960 {
 961         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 962         struct omap1_cam_dev *pcdev = ici->priv;
 963         u32 ctrlclock;
 964
 965         BUG_ON(icd != pcdev->icd);
 966
 967         suspend_capture(pcdev);
 968         disable_capture(pcdev);
 969
 970         sensor_reset(pcdev, true);
 971
 972         /* disable and release system clocks */
 973         ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
 974         ctrlclock &= ~(MCLK_EN | DPLL_EN | CAMEXCLK_EN);
 975         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
 976
 977         ctrlclock = (ctrlclock & ~FOSCMOD_MASK) | FOSCMOD_12MHz;
 978         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
 979         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | MCLK_EN);
 980
 981         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~MCLK_EN);
 982
 983         clk_disable(pcdev->clk);
 984
 985         pcdev->icd = NULL;
 986
 987         dev_dbg(icd->parent,
 988                 "OMAP1 Camera driver detached from camera %d\n", icd->devnum);
 989 }
 990
 991 /* Duplicate standard formats based on host capability of byte swapping */
 992 static const struct soc_mbus_lookup omap1_cam_formats[] = {
 993 {
 994         .code = V4L2_MBUS_FMT_UYVY8_2X8,
 995         .fmt = {
 996                 .fourcc                 = V4L2_PIX_FMT_YUYV,
 997                 .name                   = "YUYV",
 998                 .bits_per_sample        = 8,
 999                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1000                 .order                  = SOC_MBUS_ORDER_BE,
1001         },
1002 }, {
1003         .code = V4L2_MBUS_FMT_VYUY8_2X8,
1004         .fmt = {
1005                 .fourcc                 = V4L2_PIX_FMT_YVYU,
1006                 .name                   = "YVYU",
1007                 .bits_per_sample        = 8,
1008                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1009                 .order                  = SOC_MBUS_ORDER_BE,
1010         },
1011 }, {
1012         .code = V4L2_MBUS_FMT_YUYV8_2X8,
1013         .fmt = {
1014                 .fourcc                 = V4L2_PIX_FMT_UYVY,
1015                 .name                   = "UYVY",
1016                 .bits_per_sample        = 8,
1017                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1018                 .order                  = SOC_MBUS_ORDER_BE,
1019         },
1020 }, {
1021         .code = V4L2_MBUS_FMT_YVYU8_2X8,
1022         .fmt = {
1023                 .fourcc                 = V4L2_PIX_FMT_VYUY,
1024                 .name                   = "VYUY",
1025                 .bits_per_sample        = 8,
1026                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1027                 .order                  = SOC_MBUS_ORDER_BE,
1028         },
1029 }, {
1030         .code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE,
1031         .fmt = {
1032                 .fourcc                 = V4L2_PIX_FMT_RGB555,
1033                 .name                   = "RGB555",
1034                 .bits_per_sample        = 8,
1035                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1036                 .order                  = SOC_MBUS_ORDER_BE,
1037         },
1038 }, {
1039         .code = V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE,
1040         .fmt = {
1041                 .fourcc                 = V4L2_PIX_FMT_RGB555X,
1042                 .name                   = "RGB555X",
1043                 .bits_per_sample        = 8,
1044                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1045                 .order                  = SOC_MBUS_ORDER_BE,
1046         },
1047 }, {
1048         .code = V4L2_MBUS_FMT_RGB565_2X8_BE,
1049         .fmt = {
1050                 .fourcc                 = V4L2_PIX_FMT_RGB565,
1051                 .name                   = "RGB565",
1052                 .bits_per_sample        = 8,
1053                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1054                 .order                  = SOC_MBUS_ORDER_BE,
1055         },
1056 }, {
1057         .code = V4L2_MBUS_FMT_RGB565_2X8_LE,
1058         .fmt = {
1059                 .fourcc                 = V4L2_PIX_FMT_RGB565X,
1060                 .name                   = "RGB565X",
1061                 .bits_per_sample        = 8,
1062                 .packing                = SOC_MBUS_PACKING_2X8_PADHI,
1063                 .order                  = SOC_MBUS_ORDER_BE,
1064         },
1065 },
1066 };
1067
1068 static int omap1_cam_get_formats(struct soc_camera_device *icd,
1069                 unsigned int idx, struct soc_camera_format_xlate *xlate)
1070 {
1071         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1072         struct device *dev = icd->parent;
1073         int formats = 0, ret;
1074         enum v4l2_mbus_pixelcode code;
1075         const struct soc_mbus_pixelfmt *fmt;
1076
1077         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1078         if (ret < 0)
1079                 /* No more formats */
1080                 return 0;
1081
1082         fmt = soc_mbus_get_fmtdesc(code);
1083         if (!fmt) {
1084                 dev_warn(dev, "%s: unsupported format code #%d: %d\n", __func__,
1085                                 idx, code);
1086                 return 0;
1087         }
1088
1089         /* Check support for the requested bits-per-sample */
1090         if (fmt->bits_per_sample != 8)
1091                 return 0;
1092
1093         switch (code) {
1094         case V4L2_MBUS_FMT_YUYV8_2X8:
1095         case V4L2_MBUS_FMT_YVYU8_2X8:
1096         case V4L2_MBUS_FMT_UYVY8_2X8:
1097         case V4L2_MBUS_FMT_VYUY8_2X8:
1098         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
1099         case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
1100         case V4L2_MBUS_FMT_RGB565_2X8_BE:
1101         case V4L2_MBUS_FMT_RGB565_2X8_LE:
1102                 formats++;
1103                 if (xlate) {
1104                         xlate->host_fmt = soc_mbus_find_fmtdesc(code,
1105                                                 omap1_cam_formats,
1106                                                 ARRAY_SIZE(omap1_cam_formats));
1107                         xlate->code     = code;
1108                         xlate++;
1109                         dev_dbg(dev,
1110                                 "%s: providing format %s as byte swapped code #%d\n",
1111                                 __func__, xlate->host_fmt->name, code);
1112                 }
1113         default:
1114                 if (xlate)
1115                         dev_dbg(dev,
1116                                 "%s: providing format %s in pass-through mode\n",
1117                                 __func__, fmt->name);
1118         }
1119         formats++;
1120         if (xlate) {
1121                 xlate->host_fmt = fmt;
1122                 xlate->code     = code;
1123                 xlate++;
1124         }
1125
1126         return formats;
1127 }
1128
1129 static bool is_dma_aligned(s32 bytes_per_line, unsigned int height,
1130                 enum omap1_cam_vb_mode vb_mode)
1131 {
1132         int size = bytes_per_line * height;
1133
1134         return IS_ALIGNED(bytes_per_line, DMA_ELEMENT_SIZE) &&
1135                 IS_ALIGNED(size, DMA_FRAME_SIZE(vb_mode) * DMA_ELEMENT_SIZE);
1136 }
1137
1138 static int dma_align(int *width, int *height,
1139                 const struct soc_mbus_pixelfmt *fmt,
1140                 enum omap1_cam_vb_mode vb_mode, bool enlarge)
1141 {
1142         s32 bytes_per_line = soc_mbus_bytes_per_line(*width, fmt);
1143
1144         if (bytes_per_line < 0)
1145                 return bytes_per_line;
1146
1147         if (!is_dma_aligned(bytes_per_line, *height, vb_mode)) {
1148                 unsigned int pxalign = __fls(bytes_per_line / *width);
1149                 unsigned int salign  = DMA_FRAME_SHIFT(vb_mode) +
1150                                 DMA_ELEMENT_SHIFT - pxalign;
1151                 unsigned int incr    = enlarge << salign;
1152
1153                 v4l_bound_align_image(width, 1, *width + incr, 0,
1154                                 height, 1, *height + incr, 0, salign);
1155                 return 0;
1156         }
1157         return 1;
1158 }
1159
1160 #define subdev_call_with_sense(pcdev, dev, icd, sd, function, args...)               \
1161 ({                                                                                   \
1162         struct soc_camera_sense sense = {                                            \
1163                 .master_clock           = pcdev->camexclk,                           \
1164                 .pixel_clock_max        = 0,                                         \
1165         };                                                                           \
1166         int __ret;                                                                   \
1167                                                                                      \
1168         if (pcdev->pdata)                                                            \
1169                 sense.pixel_clock_max = pcdev->pdata->lclk_khz_max * 1000;           \
1170         icd->sense = &sense;                                                         \
1171         __ret = v4l2_subdev_call(sd, video, function, ##args);                       \
1172         icd->sense = NULL;                                                           \
1173                                                                                      \
1174         if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {                                \
1175                 if (sense.pixel_clock > sense.pixel_clock_max) {                     \
1176                         dev_err(dev,                                                 \
1177                                 "%s: pixel clock %lu set by the camera too high!\n", \
1178                                 __func__, sense.pixel_clock);                        \
1179                         __ret = -EINVAL;                                             \
1180                 }                                                                    \
1181         }                                                                            \
1182         __ret;                                                                       \
1183 })
1184
1185 static int set_mbus_format(struct omap1_cam_dev *pcdev, struct device *dev,
1186                 struct soc_camera_device *icd, struct v4l2_subdev *sd,
1187                 struct v4l2_mbus_framefmt *mf,
1188                 const struct soc_camera_format_xlate *xlate)
1189 {
1190         s32 bytes_per_line;
1191         int ret = subdev_call_with_sense(pcdev, dev, icd, sd, s_mbus_fmt, mf);
1192
1193         if (ret < 0) {
1194                 dev_err(dev, "%s: s_mbus_fmt failed\n", __func__);
1195                 return ret;
1196         }
1197
1198         if (mf->code != xlate->code) {
1199                 dev_err(dev, "%s: unexpected pixel code change\n", __func__);
1200                 return -EINVAL;
1201         }
1202
1203         bytes_per_line = soc_mbus_bytes_per_line(mf->width, xlate->host_fmt);
1204         if (bytes_per_line < 0) {
1205                 dev_err(dev, "%s: soc_mbus_bytes_per_line() failed\n",
1206                                 __func__);
1207                 return bytes_per_line;
1208         }
1209
1210         if (!is_dma_aligned(bytes_per_line, mf->height, pcdev->vb_mode)) {
1211                 dev_err(dev, "%s: resulting geometry %ux%u not DMA aligned\n",
1212                                 __func__, mf->width, mf->height);
1213                 return -EINVAL;
1214         }
1215         return 0;
1216 }
1217
1218 static int omap1_cam_set_crop(struct soc_camera_device *icd,
1219                                struct v4l2_crop *crop)
1220 {
1221         struct v4l2_rect *rect = &crop->c;
1222         const struct soc_camera_format_xlate *xlate = icd->current_fmt;
1223         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1224         struct device *dev = icd->parent;
1225         struct soc_camera_host *ici = to_soc_camera_host(dev);
1226         struct omap1_cam_dev *pcdev = ici->priv;
1227         struct v4l2_mbus_framefmt mf;
1228         int ret;
1229
1230         ret = subdev_call_with_sense(pcdev, dev, icd, sd, s_crop, crop);
1231         if (ret < 0) {
1232                 dev_warn(dev, "%s: failed to crop to %ux%u@%u:%u\n", __func__,
1233                          rect->width, rect->height, rect->left, rect->top);
1234                 return ret;
1235         }
1236
1237         ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
1238         if (ret < 0) {
1239                 dev_warn(dev, "%s: failed to fetch current format\n", __func__);
1240                 return ret;
1241         }
1242
1243         ret = dma_align(&mf.width, &mf.height, xlate->host_fmt, pcdev->vb_mode,
1244                         false);
1245         if (ret < 0) {
1246                 dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
1247                                 __func__, mf.width, mf.height,
1248                                 xlate->host_fmt->name);
1249                 return ret;
1250         }
1251
1252         if (!ret) {
1253                 /* sensor returned geometry not DMA aligned, trying to fix */
1254                 ret = set_mbus_format(pcdev, dev, icd, sd, &mf, xlate);
1255                 if (ret < 0) {
1256                         dev_err(dev, "%s: failed to set format\n", __func__);
1257                         return ret;
1258                 }
1259         }
1260
1261         icd->user_width  = mf.width;
1262         icd->user_height = mf.height;
1263
1264         return 0;
1265 }
1266
1267 static int omap1_cam_set_fmt(struct soc_camera_device *icd,
1268                               struct v4l2_format *f)
1269 {
1270         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1271         const struct soc_camera_format_xlate *xlate;
1272         struct device *dev = icd->parent;
1273         struct soc_camera_host *ici = to_soc_camera_host(dev);
1274         struct omap1_cam_dev *pcdev = ici->priv;
1275         struct v4l2_pix_format *pix = &f->fmt.pix;
1276         struct v4l2_mbus_framefmt mf;
1277         int ret;
1278
1279         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1280         if (!xlate) {
1281                 dev_warn(dev, "%s: format %#x not found\n", __func__,
1282                                 pix->pixelformat);
1283                 return -EINVAL;
1284         }
1285
1286         mf.width        = pix->width;
1287         mf.height       = pix->height;
1288         mf.field        = pix->field;
1289         mf.colorspace   = pix->colorspace;
1290         mf.code         = xlate->code;
1291
1292         ret = dma_align(&mf.width, &mf.height, xlate->host_fmt, pcdev->vb_mode,
1293                         true);
1294         if (ret < 0) {
1295                 dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
1296                                 __func__, pix->width, pix->height,
1297                                 xlate->host_fmt->name);
1298                 return ret;
1299         }
1300
1301         ret = set_mbus_format(pcdev, dev, icd, sd, &mf, xlate);
1302         if (ret < 0) {
1303                 dev_err(dev, "%s: failed to set format\n", __func__);
1304                 return ret;
1305         }
1306
1307         pix->width       = mf.width;
1308         pix->height      = mf.height;
1309         pix->field       = mf.field;
1310         pix->colorspace  = mf.colorspace;
1311         icd->current_fmt = xlate;
1312
1313         return 0;
1314 }
1315
1316 static int omap1_cam_try_fmt(struct soc_camera_device *icd,
1317                               struct v4l2_format *f)
1318 {
1319         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1320         const struct soc_camera_format_xlate *xlate;
1321         struct v4l2_pix_format *pix = &f->fmt.pix;
1322         struct v4l2_mbus_framefmt mf;
1323         int ret;
1324         /* TODO: limit to mx1 hardware capabilities */
1325
1326         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1327         if (!xlate) {
1328                 dev_warn(icd->parent, "Format %#x not found\n",
1329                          pix->pixelformat);
1330                 return -EINVAL;
1331         }
1332
1333         mf.width        = pix->width;
1334         mf.height       = pix->height;
1335         mf.field        = pix->field;
1336         mf.colorspace   = pix->colorspace;
1337         mf.code         = xlate->code;
1338
1339         /* limit to sensor capabilities */
1340         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1341         if (ret < 0)
1342                 return ret;
1343
1344         pix->width      = mf.width;
1345         pix->height     = mf.height;
1346         pix->field      = mf.field;
1347         pix->colorspace = mf.colorspace;
1348
1349         return 0;
1350 }
1351
1352 static bool sg_mode;
1353
1354 /*
1355  * Local mmap_mapper wrapper,
1356  * used for detecting videobuf-dma-contig buffer allocation failures
1357  * and switching to videobuf-dma-sg automatically for future attempts.
1358  */
1359 static int omap1_cam_mmap_mapper(struct videobuf_queue *q,
1360                                   struct videobuf_buffer *buf,
1361                                   struct vm_area_struct *vma)
1362 {
1363         struct soc_camera_device *icd = q->priv_data;
1364         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1365         struct omap1_cam_dev *pcdev = ici->priv;
1366         int ret;
1367
1368         ret = pcdev->mmap_mapper(q, buf, vma);
1369
1370         if (ret == -ENOMEM)
1371                 sg_mode = true;
1372
1373         return ret;
1374 }
1375
1376 static void omap1_cam_init_videobuf(struct videobuf_queue *q,
1377                                      struct soc_camera_device *icd)
1378 {
1379         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1380         struct omap1_cam_dev *pcdev = ici->priv;
1381
1382         if (!sg_mode)
1383                 videobuf_queue_dma_contig_init(q, &omap1_videobuf_ops,
1384                                 icd->parent, &pcdev->lock,
1385                                 V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
1386                                 sizeof(struct omap1_cam_buf), icd, &icd->video_lock);
1387         else
1388                 videobuf_queue_sg_init(q, &omap1_videobuf_ops,
1389                                 icd->parent, &pcdev->lock,
1390                                 V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
1391                                 sizeof(struct omap1_cam_buf), icd, &icd->video_lock);
1392
1393         /* use videobuf mode (auto)selected with the module parameter */
1394         pcdev->vb_mode = sg_mode ? OMAP1_CAM_DMA_SG : OMAP1_CAM_DMA_CONTIG;
1395
1396         /*
1397          * Ensure we substitute the videobuf-dma-contig version of the
1398          * mmap_mapper() callback with our own wrapper, used for switching
1399          * automatically to videobuf-dma-sg on buffer allocation failure.
1400          */
1401         if (!sg_mode && q->int_ops->mmap_mapper != omap1_cam_mmap_mapper) {
1402                 pcdev->mmap_mapper = q->int_ops->mmap_mapper;
1403                 q->int_ops->mmap_mapper = omap1_cam_mmap_mapper;
1404         }
1405 }
1406
1407 static int omap1_cam_reqbufs(struct soc_camera_device *icd,
1408                               struct v4l2_requestbuffers *p)
1409 {
1410         int i;
1411
1412         /*
1413          * This is for locking debugging only. I removed spinlocks and now I
1414          * check whether .prepare is ever called on a linked buffer, or whether
1415          * a dma IRQ can occur for an in-work or unlinked buffer. Until now
1416          * it hadn't triggered
1417          */
1418         for (i = 0; i < p->count; i++) {
1419                 struct omap1_cam_buf *buf = container_of(icd->vb_vidq.bufs[i],
1420                                                       struct omap1_cam_buf, vb);
1421                 buf->inwork = 0;
1422                 INIT_LIST_HEAD(&buf->vb.queue);
1423         }
1424
1425         return 0;
1426 }
1427
1428 static int omap1_cam_querycap(struct soc_camera_host *ici,
1429                                struct v4l2_capability *cap)
1430 {
1431         /* cap->name is set by the friendly caller:-> */
1432         strlcpy(cap->card, "OMAP1 Camera", sizeof(cap->card));
1433         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1434
1435         return 0;
1436 }
1437
1438 static int omap1_cam_set_bus_param(struct soc_camera_device *icd,
1439                 __u32 pixfmt)
1440 {
1441         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1442         struct device *dev = icd->parent;
1443         struct soc_camera_host *ici = to_soc_camera_host(dev);
1444         struct omap1_cam_dev *pcdev = ici->priv;
1445         const struct soc_camera_format_xlate *xlate;
1446         const struct soc_mbus_pixelfmt *fmt;
1447         struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
1448         unsigned long common_flags;
1449         u32 ctrlclock, mode;
1450         int ret;
1451
1452         ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
1453         if (!ret) {
1454                 common_flags = soc_mbus_config_compatible(&cfg, SOCAM_BUS_FLAGS);
1455                 if (!common_flags) {
1456                         dev_warn(dev,
1457                                  "Flags incompatible: camera 0x%x, host 0x%x\n",
1458                                  cfg.flags, SOCAM_BUS_FLAGS);
1459                         return -EINVAL;
1460                 }
1461         } else if (ret != -ENOIOCTLCMD) {
1462                 return ret;
1463         } else {
1464                 common_flags = SOCAM_BUS_FLAGS;
1465         }
1466
1467         /* Make choices, possibly based on platform configuration */
1468         if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
1469                         (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
1470                 if (!pcdev->pdata ||
1471                                 pcdev->pdata->flags & OMAP1_CAMERA_LCLK_RISING)
1472                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
1473                 else
1474                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
1475         }
1476
1477         cfg.flags = common_flags;
1478         ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
1479         if (ret < 0 && ret != -ENOIOCTLCMD) {
1480                 dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
1481                         common_flags, ret);
1482                 return ret;
1483         }
1484
1485         ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
1486         if (ctrlclock & LCLK_EN)
1487                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
1488
1489         if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) {
1490                 dev_dbg(dev, "CTRLCLOCK_REG |= POLCLK\n");
1491                 ctrlclock |= POLCLK;
1492         } else {
1493                 dev_dbg(dev, "CTRLCLOCK_REG &= ~POLCLK\n");
1494                 ctrlclock &= ~POLCLK;
1495         }
1496         CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
1497
1498         if (ctrlclock & LCLK_EN)
1499                 CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
1500
1501         /* select bus endianess */
1502         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1503         fmt = xlate->host_fmt;
1504
1505         mode = CAM_READ(pcdev, MODE) & ~(RAZ_FIFO | IRQ_MASK | DMA);
1506         if (fmt->order == SOC_MBUS_ORDER_LE) {
1507                 dev_dbg(dev, "MODE_REG &= ~ORDERCAMD\n");
1508                 CAM_WRITE(pcdev, MODE, mode & ~ORDERCAMD);
1509         } else {
1510                 dev_dbg(dev, "MODE_REG |= ORDERCAMD\n");
1511                 CAM_WRITE(pcdev, MODE, mode | ORDERCAMD);
1512         }
1513
1514         return 0;
1515 }
1516
1517 static unsigned int omap1_cam_poll(struct file *file, poll_table *pt)
1518 {
1519         struct soc_camera_device *icd = file->private_data;
1520         struct omap1_cam_buf *buf;
1521
1522         buf = list_entry(icd->vb_vidq.stream.next, struct omap1_cam_buf,
1523                          vb.stream);
1524
1525         poll_wait(file, &buf->vb.done, pt);
1526
1527         if (buf->vb.state == VIDEOBUF_DONE ||
1528             buf->vb.state == VIDEOBUF_ERROR)
1529                 return POLLIN | POLLRDNORM;
1530
1531         return 0;
1532 }
1533
1534 static struct soc_camera_host_ops omap1_host_ops = {
1535         .owner          = THIS_MODULE,
1536         .add            = omap1_cam_add_device,
1537         .remove         = omap1_cam_remove_device,
1538         .get_formats    = omap1_cam_get_formats,
1539         .set_crop       = omap1_cam_set_crop,
1540         .set_fmt        = omap1_cam_set_fmt,
1541         .try_fmt        = omap1_cam_try_fmt,
1542         .init_videobuf  = omap1_cam_init_videobuf,
1543         .reqbufs        = omap1_cam_reqbufs,
1544         .querycap       = omap1_cam_querycap,
1545         .set_bus_param  = omap1_cam_set_bus_param,
1546         .poll           = omap1_cam_poll,
1547 };
1548
1549 static int __init omap1_cam_probe(struct platform_device *pdev)
1550 {
1551         struct omap1_cam_dev *pcdev;
1552         struct resource *res;
1553         struct clk *clk;
1554         void __iomem *base;
1555         unsigned int irq;
1556         int err = 0;
1557
1558         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1559         irq = platform_get_irq(pdev, 0);
1560         if (!res || (int)irq <= 0) {
1561                 err = -ENODEV;
1562                 goto exit;
1563         }
1564
1565         clk = clk_get(&pdev->dev, "armper_ck");
1566         if (IS_ERR(clk)) {
1567                 err = PTR_ERR(clk);
1568                 goto exit;
1569         }
1570
1571         pcdev = kzalloc(sizeof(*pcdev) + resource_size(res), GFP_KERNEL);
1572         if (!pcdev) {
1573                 dev_err(&pdev->dev, "Could not allocate pcdev\n");
1574                 err = -ENOMEM;
1575                 goto exit_put_clk;
1576         }
1577
1578         pcdev->res = res;
1579         pcdev->clk = clk;
1580
1581         pcdev->pdata = pdev->dev.platform_data;
1582         if (pcdev->pdata) {
1583                 pcdev->pflags = pcdev->pdata->flags;
1584                 pcdev->camexclk = pcdev->pdata->camexclk_khz * 1000;
1585         }
1586
1587         switch (pcdev->camexclk) {
1588         case 6000000:
1589         case 8000000:
1590         case 9600000:
1591         case 12000000:
1592         case 24000000:
1593                 break;
1594         default:
1595                 /* pcdev->camexclk != 0 => pcdev->pdata != NULL */
1596                 dev_warn(&pdev->dev,
1597                                 "Incorrect sensor clock frequency %ld kHz, "
1598                                 "should be one of 0, 6, 8, 9.6, 12 or 24 MHz, "
1599                                 "please correct your platform data\n",
1600                                 pcdev->pdata->camexclk_khz);
1601                 pcdev->camexclk = 0;
1602         case 0:
1603                 dev_info(&pdev->dev, "Not providing sensor clock\n");
1604         }
1605
1606         INIT_LIST_HEAD(&pcdev->capture);
1607         spin_lock_init(&pcdev->lock);
1608
1609         /*
1610          * Request the region.
1611          */
1612         if (!request_mem_region(res->start, resource_size(res), DRIVER_NAME)) {
1613                 err = -EBUSY;
1614                 goto exit_kfree;
1615         }
1616
1617         base = ioremap(res->start, resource_size(res));
1618         if (!base) {
1619                 err = -ENOMEM;
1620                 goto exit_release;
1621         }
1622         pcdev->irq = irq;
1623         pcdev->base = base;
1624
1625         sensor_reset(pcdev, true);
1626
1627         err = omap_request_dma(OMAP_DMA_CAMERA_IF_RX, DRIVER_NAME,
1628                         dma_isr, (void *)pcdev, &pcdev->dma_ch);
1629         if (err < 0) {
1630                 dev_err(&pdev->dev, "Can't request DMA for OMAP1 Camera\n");
1631                 err = -EBUSY;
1632                 goto exit_iounmap;
1633         }
1634         dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_ch);
1635
1636         /* preconfigure DMA */
1637         omap_set_dma_src_params(pcdev->dma_ch, OMAP_DMA_PORT_TIPB,
1638                         OMAP_DMA_AMODE_CONSTANT, res->start + REG_CAMDATA,
1639                         0, 0);
1640         omap_set_dma_dest_burst_mode(pcdev->dma_ch, OMAP_DMA_DATA_BURST_4);
1641         /* setup DMA autoinitialization */
1642         omap_dma_link_lch(pcdev->dma_ch, pcdev->dma_ch);
1643
1644         err = request_irq(pcdev->irq, cam_isr, 0, DRIVER_NAME, pcdev);
1645         if (err) {
1646                 dev_err(&pdev->dev, "Camera interrupt register failed\n");
1647                 goto exit_free_dma;
1648         }
1649
1650         pcdev->soc_host.drv_name        = DRIVER_NAME;
1651         pcdev->soc_host.ops             = &omap1_host_ops;
1652         pcdev->soc_host.priv            = pcdev;
1653         pcdev->soc_host.v4l2_dev.dev    = &pdev->dev;
1654         pcdev->soc_host.nr              = pdev->id;
1655
1656         err = soc_camera_host_register(&pcdev->soc_host);
1657         if (err)
1658                 goto exit_free_irq;
1659
1660         dev_info(&pdev->dev, "OMAP1 Camera Interface driver loaded\n");
1661
1662         return 0;
1663
1664 exit_free_irq:
1665         free_irq(pcdev->irq, pcdev);
1666 exit_free_dma:
1667         omap_free_dma(pcdev->dma_ch);
1668 exit_iounmap:
1669         iounmap(base);
1670 exit_release:
1671         release_mem_region(res->start, resource_size(res));
1672 exit_kfree:
1673         kfree(pcdev);
1674 exit_put_clk:
1675         clk_put(clk);
1676 exit:
1677         return err;
1678 }
1679
1680 static int __exit omap1_cam_remove(struct platform_device *pdev)
1681 {
1682         struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
1683         struct omap1_cam_dev *pcdev = container_of(soc_host,
1684                                         struct omap1_cam_dev, soc_host);
1685         struct resource *res;
1686
1687         free_irq(pcdev->irq, pcdev);
1688
1689         omap_free_dma(pcdev->dma_ch);
1690
1691         soc_camera_host_unregister(soc_host);
1692
1693         iounmap(pcdev->base);
1694
1695         res = pcdev->res;
1696         release_mem_region(res->start, resource_size(res));
1697
1698         clk_put(pcdev->clk);
1699
1700         kfree(pcdev);
1701
1702         dev_info(&pdev->dev, "OMAP1 Camera Interface driver unloaded\n");
1703
1704         return 0;
1705 }
1706
1707 static struct platform_driver omap1_cam_driver = {
1708         .driver         = {
1709                 .name   = DRIVER_NAME,
1710         },
1711         .probe          = omap1_cam_probe,
1712         .remove         = __exit_p(omap1_cam_remove),
1713 };
1714
1715 static int __init omap1_cam_init(void)
1716 {
1717         return platform_driver_register(&omap1_cam_driver);
1718 }
1719 module_init(omap1_cam_init);
1720
1721 static void __exit omap1_cam_exit(void)
1722 {
1723         platform_driver_unregister(&omap1_cam_driver);
1724 }
1725 module_exit(omap1_cam_exit);
1726
1727 module_param(sg_mode, bool, 0644);
1728 MODULE_PARM_DESC(sg_mode, "videobuf mode, 0: dma-contig (default), 1: dma-sg");
1729
1730 MODULE_DESCRIPTION("OMAP1 Camera Interface driver");
1731 MODULE_AUTHOR("Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>");
1732 MODULE_LICENSE("GPL v2");
1733 MODULE_VERSION(DRIVER_VERSION);
1734 MODULE_ALIAS("platform:" DRIVER_NAME);