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