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