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