1 files changed, 1908 insertions, 0 deletions

diff --git a/drivers/media/video/omap24xxcam.c b/drivers/media/video/omap24xxcam.c
new file mode 100644
index 000000000000..85c3c7c92af1
--- /dev/null
+++ b/drivers/media/video/omap24xxcam.c
@@ -0,0 +1,1908 @@
+/*
+ * drivers/media/video/omap24xxcam.c
+ *
+ * OMAP 2 camera block driver.
+ *
+ * Copyright (C) 2004 MontaVista Software, Inc.
+ * Copyright (C) 2004 Texas Instruments.
+ * Copyright (C) 2007-2008 Nokia Corporation.
+ *
+ * Contact: Sakari Ailus <sakari.ailus@nokia.com>
+ *
+ * Based on code from Andy Lowe <source@mvista.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/videodev2.h>
+#include <linux/pci.h>          /* needed for videobufs */
+#include <linux/version.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+
+#include "omap24xxcam.h"
+
+#define OMAP24XXCAM_VERSION KERNEL_VERSION(0, 0, 0)
+
+#define RESET_TIMEOUT_NS 10000
+
+static void omap24xxcam_reset(struct omap24xxcam_device *cam);
+static int omap24xxcam_sensor_if_enable(struct omap24xxcam_device *cam);
+static void omap24xxcam_device_unregister(struct v4l2_int_device *s);
+static int omap24xxcam_remove(struct platform_device *pdev);
+
+/* module parameters */
+static int video_nr = -1;       /* video device minor (-1 ==> auto assign) */
+/*
+ * Maximum amount of memory to use for capture buffers.
+ * Default is 4800KB, enough to double-buffer SXGA.
+ */
+static int capture_mem = 1280 * 960 * 2 * 2;
+
+static struct v4l2_int_device omap24xxcam;
+
+/*
+ *
+ * Clocks.
+ *
+ */
+
+static void omap24xxcam_clock_put(struct omap24xxcam_device *cam)
+{
+        if (cam->ick != NULL && !IS_ERR(cam->ick))
+                clk_put(cam->ick);
+        if (cam->fck != NULL && !IS_ERR(cam->fck))
+                clk_put(cam->fck);
+
+        cam->ick = cam->fck = NULL;
+}
+
+static int omap24xxcam_clock_get(struct omap24xxcam_device *cam)
+{
+        int rval = 0;
+
+        cam->fck = clk_get(cam->dev, "cam_fck");
+        if (IS_ERR(cam->fck)) {
+                dev_err(cam->dev, "can't get cam_fck");
+                rval = PTR_ERR(cam->fck);
+                omap24xxcam_clock_put(cam);
+                return rval;
+        }
+
+        cam->ick = clk_get(cam->dev, "cam_ick");
+        if (IS_ERR(cam->ick)) {
+                dev_err(cam->dev, "can't get cam_ick");
+                rval = PTR_ERR(cam->ick);
+                omap24xxcam_clock_put(cam);
+        }
+
+        return rval;
+}
+
+static void omap24xxcam_clock_on(struct omap24xxcam_device *cam)
+{
+        clk_enable(cam->fck);
+        clk_enable(cam->ick);
+}
+
+static void omap24xxcam_clock_off(struct omap24xxcam_device *cam)
+{
+        clk_disable(cam->fck);
+        clk_disable(cam->ick);
+}
+
+/*
+ *
+ * Camera core
+ *
+ */
+
+/*
+ * Set xclk.
+ *
+ * To disable xclk, use value zero.
+ */
+static void omap24xxcam_core_xclk_set(const struct omap24xxcam_device *cam,
+                                      u32 xclk)
+{
+        if (xclk) {
+                u32 divisor = CAM_MCLK / xclk;
+
+                if (divisor == 1)
+                        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET,
+                                            CC_CTRL_XCLK,
+                                            CC_CTRL_XCLK_DIV_BYPASS);
+                else
+                        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET,
+                                            CC_CTRL_XCLK, divisor);
+        } else
+                omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET,
+                                    CC_CTRL_XCLK, CC_CTRL_XCLK_DIV_STABLE_LOW);
+}
+
+static void omap24xxcam_core_hwinit(const struct omap24xxcam_device *cam)
+{
+        /*
+         * Setting the camera core AUTOIDLE bit causes problems with frame
+         * synchronization, so we will clear the AUTOIDLE bit instead.
+         */
+        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET, CC_SYSCONFIG,
+                            CC_SYSCONFIG_AUTOIDLE);
+
+        /* program the camera interface DMA packet size */
+        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET, CC_CTRL_DMA,
+                            CC_CTRL_DMA_EN | (DMA_THRESHOLD / 4 - 1));
+
+        /* enable camera core error interrupts */
+        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET, CC_IRQENABLE,
+                            CC_IRQENABLE_FW_ERR_IRQ
+                            | CC_IRQENABLE_FSC_ERR_IRQ
+                            | CC_IRQENABLE_SSC_ERR_IRQ
+                            | CC_IRQENABLE_FIFO_OF_IRQ);
+}
+
+/*
+ * Enable the camera core.
+ *
+ * Data transfer to the camera DMA starts from next starting frame.
+ */
+static void omap24xxcam_core_enable(const struct omap24xxcam_device *cam)
+{
+
+        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET, CC_CTRL,
+                            cam->cc_ctrl);
+}
+
+/*
+ * Disable camera core.
+ *
+ * The data transfer will be stopped immediately (CC_CTRL_CC_RST). The
+ * core internal state machines will be reset. Use
+ * CC_CTRL_CC_FRAME_TRIG instead if you want to transfer the current
+ * frame completely.
+ */
+static void omap24xxcam_core_disable(const struct omap24xxcam_device *cam)
+{
+        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET, CC_CTRL,
+                            CC_CTRL_CC_RST);
+}
+
+/* Interrupt service routine for camera core interrupts. */
+static void omap24xxcam_core_isr(struct omap24xxcam_device *cam)
+{
+        u32 cc_irqstatus;
+        const u32 cc_irqstatus_err =
+                CC_IRQSTATUS_FW_ERR_IRQ
+                | CC_IRQSTATUS_FSC_ERR_IRQ
+                | CC_IRQSTATUS_SSC_ERR_IRQ
+                | CC_IRQSTATUS_FIFO_UF_IRQ
+                | CC_IRQSTATUS_FIFO_OF_IRQ;
+
+        cc_irqstatus = omap24xxcam_reg_in(cam->mmio_base + CC_REG_OFFSET,
+                                          CC_IRQSTATUS);
+        omap24xxcam_reg_out(cam->mmio_base + CC_REG_OFFSET, CC_IRQSTATUS,
+                            cc_irqstatus);
+
+        if (cc_irqstatus & cc_irqstatus_err
+            && !atomic_read(&cam->in_reset)) {
+                dev_dbg(cam->dev, "resetting camera, cc_irqstatus 0x%x\n",
+                        cc_irqstatus);
+                omap24xxcam_reset(cam);
+        }
+}
+
+/*
+ *
+ * videobuf_buffer handling.
+ *
+ * Memory for mmapped videobuf_buffers is not allocated
+ * conventionally, but by several kmalloc allocations and then
+ * creating the scatterlist on our own. User-space buffers are handled
+ * normally.
+ *
+ */
+
+/*
+ * Free the memory-mapped buffer memory allocated for a
+ * videobuf_buffer and the associated scatterlist.
+ */
+static void omap24xxcam_vbq_free_mmap_buffer(struct videobuf_buffer *vb)
+{
+        struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
+        size_t alloc_size;
+        struct page *page;
+        int i;
+
+        if (dma->sglist == NULL)
+                return;
+
+        i = dma->sglen;
+        while (i) {
+                i--;
+                alloc_size = sg_dma_len(&dma->sglist[i]);
+                page = sg_page(&dma->sglist[i]);
+                do {
+                        ClearPageReserved(page++);
+                } while (alloc_size -= PAGE_SIZE);
+                __free_pages(sg_page(&dma->sglist[i]),
+                             get_order(sg_dma_len(&dma->sglist[i])));
+        }
+
+        kfree(dma->sglist);
+        dma->sglist = NULL;
+}
+
+/* Release all memory related to the videobuf_queue. */
+static void omap24xxcam_vbq_free_mmap_buffers(struct videobuf_queue *vbq)
+{
+        int i;
+
+        mutex_lock(&vbq->vb_lock);
+
+        for (i = 0; i < VIDEO_MAX_FRAME; i++) {
+                if (NULL == vbq->bufs[i])
+                        continue;
+                if (V4L2_MEMORY_MMAP != vbq->bufs[i]->memory)
+                        continue;
+                vbq->ops->buf_release(vbq, vbq->bufs[i]);
+                omap24xxcam_vbq_free_mmap_buffer(vbq->bufs[i]);
+                kfree(vbq->bufs[i]);
+                vbq->bufs[i] = NULL;
+        }
+
+        mutex_unlock(&vbq->vb_lock);
+
+        videobuf_mmap_free(vbq);
+}
+
+/*
+ * Allocate physically as contiguous as possible buffer for video
+ * frame and allocate and build DMA scatter-gather list for it.
+ */
+static int omap24xxcam_vbq_alloc_mmap_buffer(struct videobuf_buffer *vb)
+{
+        unsigned int order;
+        size_t alloc_size, size = vb->bsize; /* vb->bsize is page aligned */
+        struct page *page;
+        int max_pages, err = 0, i = 0;
+        struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
+
+        /*
+         * allocate maximum size scatter-gather list. Note this is
+         * overhead. We may not use as many entries as we allocate
+         */
+        max_pages = vb->bsize >> PAGE_SHIFT;
+        dma->sglist = kcalloc(max_pages, sizeof(*dma->sglist), GFP_KERNEL);
+        if (dma->sglist == NULL) {
+                err = -ENOMEM;
+                goto out;
+        }
+
+        while (size) {
+                order = get_order(size);
+                /*
+                 * do not over-allocate even if we would get larger
+                 * contiguous chunk that way
+                 */
+                if ((PAGE_SIZE << order) > size)
+                        order--;
+
+                /* try to allocate as many contiguous pages as possible */
+                page = alloc_pages(GFP_KERNEL | GFP_DMA, order);
+                /* if allocation fails, try to allocate smaller amount */
+                while (page == NULL) {
+                        order--;
+                        page = alloc_pages(GFP_KERNEL | GFP_DMA, order);
+                        if (page == NULL && !order) {
+                                err = -ENOMEM;
+                                goto out;
+                        }
+                }
+                size -= (PAGE_SIZE << order);
+
+                /* append allocated chunk of pages into scatter-gather list */
+                sg_set_page(&dma->sglist[i], page, PAGE_SIZE << order, 0);
+                dma->sglen++;
+                i++;
+
+                alloc_size = (PAGE_SIZE << order);
+
+                /* clear pages before giving them to user space */
+                memset(page_address(page), 0, alloc_size);
+
+                /* mark allocated pages reserved */
+                do {
+                        SetPageReserved(page++);
+                } while (alloc_size -= PAGE_SIZE);
+        }
+        /*
+         * REVISIT: not fully correct to assign nr_pages == sglen but
+         * video-buf is passing nr_pages for e.g. unmap_sg calls
+         */
+        dma->nr_pages = dma->sglen;
+        dma->direction = PCI_DMA_FROMDEVICE;
+
+        return 0;
+
+out:
+        omap24xxcam_vbq_free_mmap_buffer(vb);
+        return err;
+}
+
+static int omap24xxcam_vbq_alloc_mmap_buffers(struct videobuf_queue *vbq,
+                                              unsigned int count)
+{
+        int i, err = 0;
+        struct omap24xxcam_fh *fh =
+                container_of(vbq, struct omap24xxcam_fh, vbq);
+
+        mutex_lock(&vbq->vb_lock);
+
+        for (i = 0; i < count; i++) {
+                err = omap24xxcam_vbq_alloc_mmap_buffer(vbq->bufs[i]);
+                if (err)
+                        goto out;
+                dev_dbg(fh->cam->dev, "sglen is %d for buffer %d\n",
+                        videobuf_to_dma(vbq->bufs[i])->sglen, i);
+        }
+
+        mutex_unlock(&vbq->vb_lock);
+
+        return 0;
+out:
+        while (i) {
+                i--;
+                omap24xxcam_vbq_free_mmap_buffer(vbq->bufs[i]);
+        }
+
+        mutex_unlock(&vbq->vb_lock);
+
+        return err;
+}
+
+/*
+ * This routine is called from interrupt context when a scatter-gather DMA
+ * transfer of a videobuf_buffer completes.
+ */
+static void omap24xxcam_vbq_complete(struct omap24xxcam_sgdma *sgdma,
+                                     u32 csr, void *arg)
+{
+        struct omap24xxcam_device *cam =
+                container_of(sgdma, struct omap24xxcam_device, sgdma);
+        struct omap24xxcam_fh *fh = cam->streaming->private_data;
+        struct videobuf_buffer *vb = (struct videobuf_buffer *)arg;
+        const u32 csr_error = CAMDMA_CSR_MISALIGNED_ERR
+                | CAMDMA_CSR_SUPERVISOR_ERR | CAMDMA_CSR_SECURE_ERR
+                | CAMDMA_CSR_TRANS_ERR | CAMDMA_CSR_DROP;
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->core_enable_disable_lock, flags);
+        if (--cam->sgdma_in_queue == 0)
+                omap24xxcam_core_disable(cam);
+        spin_unlock_irqrestore(&cam->core_enable_disable_lock, flags);
+
+        do_gettimeofday(&vb->ts);
+        vb->field_count = atomic_add_return(2, &fh->field_count);
+        if (csr & csr_error) {
+                vb->state = VIDEOBUF_ERROR;
+                if (!atomic_read(&fh->cam->in_reset)) {
+                        dev_dbg(cam->dev, "resetting camera, csr 0x%x\n", csr);
+                        omap24xxcam_reset(cam);
+                }
+        } else
+                vb->state = VIDEOBUF_DONE;
+        wake_up(&vb->done);
+}
+
+static void omap24xxcam_vbq_release(struct videobuf_queue *vbq,
+                                    struct videobuf_buffer *vb)
+{
+        struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
+
+        /* wait for buffer, especially to get out of the sgdma queue */
+        videobuf_waiton(vb, 0, 0);
+        if (vb->memory == V4L2_MEMORY_MMAP) {
+                dma_unmap_sg(vbq->dev, dma->sglist, dma->sglen,
+                             dma->direction);
+                dma->direction = DMA_NONE;
+        } else {
+                videobuf_dma_unmap(vbq, videobuf_to_dma(vb));
+                videobuf_dma_free(videobuf_to_dma(vb));
+        }
+
+        vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+/*
+ * Limit the number of available kernel image capture buffers based on the
+ * number requested, the currently selected image size, and the maximum
+ * amount of memory permitted for kernel capture buffers.
+ */
+static int omap24xxcam_vbq_setup(struct videobuf_queue *vbq, unsigned int *cnt,
+                                 unsigned int *size)
+{
+        struct omap24xxcam_fh *fh = vbq->priv_data;
+
+        if (*cnt <= 0)
+                *cnt = VIDEO_MAX_FRAME; /* supply a default number of buffers */
+
+        if (*cnt > VIDEO_MAX_FRAME)
+                *cnt = VIDEO_MAX_FRAME;
+
+        *size = fh->pix.sizeimage;
+
+        /* accessing fh->cam->capture_mem is ok, it's constant */
+        while (*size * *cnt > fh->cam->capture_mem)
+                (*cnt)--;
+
+        return 0;
+}
+
+static int omap24xxcam_dma_iolock(struct videobuf_queue *vbq,
+                                  struct videobuf_dmabuf *dma)
+{
+        int err = 0;
+
+        dma->direction = PCI_DMA_FROMDEVICE;
+        if (!dma_map_sg(vbq->dev, dma->sglist, dma->sglen, dma->direction)) {
+                kfree(dma->sglist);
+                dma->sglist = NULL;
+                dma->sglen = 0;
+                err = -EIO;
+        }
+
+        return err;
+}
+
+static int omap24xxcam_vbq_prepare(struct videobuf_queue *vbq,
+                                   struct videobuf_buffer *vb,
+                                   enum v4l2_field field)
+{
+        struct omap24xxcam_fh *fh = vbq->priv_data;
+        int err = 0;
+
+        /*
+         * Accessing pix here is okay since it's constant while
+         * streaming is on (and we only get called then).
+         */
+        if (vb->baddr) {
+                /* This is a userspace buffer. */
+                if (fh->pix.sizeimage > vb->bsize) {
+                        /* The buffer isn't big enough. */
+                        err = -EINVAL;
+                } else
+                        vb->size = fh->pix.sizeimage;
+        } else {
+                if (vb->state != VIDEOBUF_NEEDS_INIT) {
+                        /*
+                         * We have a kernel bounce buffer that has
+                         * already been allocated.
+                         */
+                        if (fh->pix.sizeimage > vb->size) {
+                                /*
+                                 * The image size has been changed to
+                                 * a larger size since this buffer was
+                                 * allocated, so we need to free and
+                                 * reallocate it.
+                                 */
+                                omap24xxcam_vbq_release(vbq, vb);
+                                vb->size = fh->pix.sizeimage;
+                        }
+                } else {
+                        /* We need to allocate a new kernel bounce buffer. */
+                        vb->size = fh->pix.sizeimage;
+                }
+        }
+
+        if (err)
+                return err;
+
+        vb->width = fh->pix.width;
+        vb->height = fh->pix.height;
+        vb->field = field;
+
+        if (vb->state == VIDEOBUF_NEEDS_INIT) {
+                if (vb->memory == V4L2_MEMORY_MMAP)
+                        /*
+                         * we have built the scatter-gather list by ourself so
+                         * do the scatter-gather mapping as well
+                         */
+                        err = omap24xxcam_dma_iolock(vbq, videobuf_to_dma(vb));
+                else
+                        err = videobuf_iolock(vbq, vb, NULL);
+        }
+
+        if (!err)
+                vb->state = VIDEOBUF_PREPARED;
+        else
+                omap24xxcam_vbq_release(vbq, vb);
+
+        return err;
+}
+
+static void omap24xxcam_vbq_queue(struct videobuf_queue *vbq,
+                                  struct videobuf_buffer *vb)
+{
+        struct omap24xxcam_fh *fh = vbq->priv_data;
+        struct omap24xxcam_device *cam = fh->cam;
+        enum videobuf_state state = vb->state;
+        unsigned long flags;
+        int err;
+
+        /*
+         * FIXME: We're marking the buffer active since we have no
+         * pretty way of marking it active exactly when the
+         * scatter-gather transfer starts.
+         */
+        vb->state = VIDEOBUF_ACTIVE;
+
+        err = omap24xxcam_sgdma_queue(&fh->cam->sgdma,
+                                      videobuf_to_dma(vb)->sglist,
+                                      videobuf_to_dma(vb)->sglen, vb->size,
+                                      omap24xxcam_vbq_complete, vb);
+
+        if (!err) {
+                spin_lock_irqsave(&cam->core_enable_disable_lock, flags);
+                if (++cam->sgdma_in_queue == 1
+                    && !atomic_read(&cam->in_reset))
+                        omap24xxcam_core_enable(cam);
+                spin_unlock_irqrestore(&cam->core_enable_disable_lock, flags);
+        } else {
+                /*
+                 * Oops. We're not supposed to get any errors here.
+                 * The only way we could get an error is if we ran out
+                 * of scatter-gather DMA slots, but we are supposed to
+                 * have at least as many scatter-gather DMA slots as
+                 * video buffers so that can't happen.
+                 */
+                dev_err(cam->dev, "failed to queue a video buffer for dma!\n");
+                dev_err(cam->dev, "likely a bug in the driver!\n");
+                vb->state = state;
+        }
+}
+
+static struct videobuf_queue_ops omap24xxcam_vbq_ops = {
+        .buf_setup   = omap24xxcam_vbq_setup,
+        .buf_prepare = omap24xxcam_vbq_prepare,
+        .buf_queue   = omap24xxcam_vbq_queue,
+        .buf_release = omap24xxcam_vbq_release,
+};
+
+/*
+ *
+ * OMAP main camera system
+ *
+ */
+
+/*
+ * Reset camera block to power-on state.
+ */
+static void omap24xxcam_poweron_reset(struct omap24xxcam_device *cam)
+{
+        int max_loop = RESET_TIMEOUT_NS;
+
+        /* Reset whole camera subsystem */
+        omap24xxcam_reg_out(cam->mmio_base,
+                            CAM_SYSCONFIG,
+                            CAM_SYSCONFIG_SOFTRESET);
+
+        /* Wait till it's finished */
+        while (!(omap24xxcam_reg_in(cam->mmio_base, CAM_SYSSTATUS)
+                 & CAM_SYSSTATUS_RESETDONE)
+               && --max_loop) {
+                ndelay(1);
+        }
+
+        if (!(omap24xxcam_reg_in(cam->mmio_base, CAM_SYSSTATUS)
+              & CAM_SYSSTATUS_RESETDONE))
+                dev_err(cam->dev, "camera soft reset timeout\n");
+}
+
+/*
+ * (Re)initialise the camera block.
+ */
+static void omap24xxcam_hwinit(struct omap24xxcam_device *cam)
+{
+        omap24xxcam_poweron_reset(cam);
+
+        /* set the camera subsystem autoidle bit */
+        omap24xxcam_reg_out(cam->mmio_base, CAM_SYSCONFIG,
+                            CAM_SYSCONFIG_AUTOIDLE);
+
+        /* set the camera MMU autoidle bit */
+        omap24xxcam_reg_out(cam->mmio_base,
+                            CAMMMU_REG_OFFSET + CAMMMU_SYSCONFIG,
+                            CAMMMU_SYSCONFIG_AUTOIDLE);
+
+        omap24xxcam_core_hwinit(cam);
+
+        omap24xxcam_dma_hwinit(&cam->sgdma.dma);
+}
+
+/*
+ * Callback for dma transfer stalling.
+ */
+static void omap24xxcam_stalled_dma_reset(unsigned long data)
+{
+        struct omap24xxcam_device *cam = (struct omap24xxcam_device *)data;
+
+        if (!atomic_read(&cam->in_reset)) {
+                dev_dbg(cam->dev, "dma stalled, resetting camera\n");
+                omap24xxcam_reset(cam);
+        }
+}
+
+/*
+ * Stop capture. Mark we're doing a reset, stop DMA transfers and
+ * core. (No new scatter-gather transfers will be queued whilst
+ * in_reset is non-zero.)
+ *
+ * If omap24xxcam_capture_stop is called from several places at
+ * once, only the first call will have an effect. Similarly, the last
+ * call omap24xxcam_streaming_cont will have effect.
+ *
+ * Serialisation is ensured by using cam->core_enable_disable_lock.
+ */
+static void omap24xxcam_capture_stop(struct omap24xxcam_device *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->core_enable_disable_lock, flags);
+
+        if (atomic_inc_return(&cam->in_reset) != 1) {
+                spin_unlock_irqrestore(&cam->core_enable_disable_lock, flags);
+                return;
+        }
+
+        omap24xxcam_core_disable(cam);
+
+        spin_unlock_irqrestore(&cam->core_enable_disable_lock, flags);
+
+        omap24xxcam_sgdma_sync(&cam->sgdma);
+}
+
+/*
+ * Reset and continue streaming.
+ *
+ * Note: Resetting the camera FIFO via the CC_RST bit in the CC_CTRL
+ * register is supposed to be sufficient to recover from a camera
+ * interface error, but it doesn't seem to be enough. If we only do
+ * that then subsequent image captures are out of sync by either one
+ * or two times DMA_THRESHOLD bytes. Resetting and re-initializing the
+ * entire camera subsystem prevents the problem with frame
+ * synchronization.
+ */
+static void omap24xxcam_capture_cont(struct omap24xxcam_device *cam)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&cam->core_enable_disable_lock, flags);
+
+        if (atomic_read(&cam->in_reset) != 1)
+                goto out;
+
+        omap24xxcam_hwinit(cam);
+
+        omap24xxcam_sensor_if_enable(cam);
+
+        omap24xxcam_sgdma_process(&cam->sgdma);
+
+        if (cam->sgdma_in_queue)
+                omap24xxcam_core_enable(cam);
+
+out:
+        atomic_dec(&cam->in_reset);
+        spin_unlock_irqrestore(&cam->core_enable_disable_lock, flags);
+}
+
+static ssize_t
+omap24xxcam_streaming_show(struct device *dev, struct device_attribute *attr,
+                char *buf)
+{
+        struct omap24xxcam_device *cam = dev_get_drvdata(dev);
+
+        return sprintf(buf, "%s\n", cam->streaming ?  "active" : "inactive");
+}
+static DEVICE_ATTR(streaming, S_IRUGO, omap24xxcam_streaming_show, NULL);
+
+/*
+ * Stop capture and restart it. I.e. reset the camera during use.
+ */
+static void omap24xxcam_reset(struct omap24xxcam_device *cam)
+{
+        omap24xxcam_capture_stop(cam);
+        omap24xxcam_capture_cont(cam);
+}
+
+/*
+ * The main interrupt handler.
+ */
+static irqreturn_t omap24xxcam_isr(int irq, void *arg)
+{
+        struct omap24xxcam_device *cam = (struct omap24xxcam_device *)arg;
+        u32 irqstatus;
+        unsigned int irqhandled = 0;
+
+        irqstatus = omap24xxcam_reg_in(cam->mmio_base, CAM_IRQSTATUS);
+
+        if (irqstatus &
+            (CAM_IRQSTATUS_DMA_IRQ2 | CAM_IRQSTATUS_DMA_IRQ1
+             | CAM_IRQSTATUS_DMA_IRQ0)) {
+                omap24xxcam_dma_isr(&cam->sgdma.dma);
+                irqhandled = 1;
+        }
+        if (irqstatus & CAM_IRQSTATUS_CC_IRQ) {
+                omap24xxcam_core_isr(cam);
+                irqhandled = 1;
+        }
+        if (irqstatus & CAM_IRQSTATUS_MMU_IRQ)
+                dev_err(cam->dev, "unhandled camera MMU interrupt!\n");
+
+        return IRQ_RETVAL(irqhandled);
+}
+
+/*
+ *
+ * Sensor handling.
+ *
+ */
+
+/*
+ * Enable the external sensor interface. Try to negotiate interface
+ * parameters with the sensor and start using the new ones. The calls
+ * to sensor_if_enable and sensor_if_disable need not to be balanced.
+ */
+static int omap24xxcam_sensor_if_enable(struct omap24xxcam_device *cam)
+{
+        int rval;
+        struct v4l2_ifparm p;
+
+        rval = vidioc_int_g_ifparm(cam->sdev, &p);
+        if (rval) {
+                dev_err(cam->dev, "vidioc_int_g_ifparm failed with %d\n", rval);
+                return rval;
+        }
+
+        cam->if_type = p.if_type;
+
+        cam->cc_ctrl = CC_CTRL_CC_EN;
+
+        switch (p.if_type) {
+        case V4L2_IF_TYPE_BT656:
+                if (p.u.bt656.frame_start_on_rising_vs)
+                        cam->cc_ctrl |= CC_CTRL_NOBT_SYNCHRO;
+                if (p.u.bt656.bt_sync_correct)
+                        cam->cc_ctrl |= CC_CTRL_BT_CORRECT;
+                if (p.u.bt656.swap)
+                        cam->cc_ctrl |= CC_CTRL_PAR_ORDERCAM;
+                if (p.u.bt656.latch_clk_inv)
+                        cam->cc_ctrl |= CC_CTRL_PAR_CLK_POL;
+                if (p.u.bt656.nobt_hs_inv)
+                        cam->cc_ctrl |= CC_CTRL_NOBT_HS_POL;
+                if (p.u.bt656.nobt_vs_inv)
+                        cam->cc_ctrl |= CC_CTRL_NOBT_VS_POL;
+
+                switch (p.u.bt656.mode) {
+                case V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT:
+                        cam->cc_ctrl |= CC_CTRL_PAR_MODE_NOBT8;
+                        break;
+                case V4L2_IF_TYPE_BT656_MODE_NOBT_10BIT:
+                        cam->cc_ctrl |= CC_CTRL_PAR_MODE_NOBT10;
+                        break;
+                case V4L2_IF_TYPE_BT656_MODE_NOBT_12BIT:
+                        cam->cc_ctrl |= CC_CTRL_PAR_MODE_NOBT12;
+                        break;
+                case V4L2_IF_TYPE_BT656_MODE_BT_8BIT:
+                        cam->cc_ctrl |= CC_CTRL_PAR_MODE_BT8;
+                        break;
+                case V4L2_IF_TYPE_BT656_MODE_BT_10BIT:
+                        cam->cc_ctrl |= CC_CTRL_PAR_MODE_BT10;
+                        break;
+                default:
+                        dev_err(cam->dev,
+                                "bt656 interface mode %d not supported\n",
+                                p.u.bt656.mode);
+                        return -EINVAL;
+                }
+                /*
+                 * The clock rate that the sensor wants has changed.
+                 * We have to adjust the xclk from OMAP 2 side to
+                 * match the sensor's wish as closely as possible.
+                 */
+                if (p.u.bt656.clock_curr != cam->if_u.bt656.xclk) {
+                        u32 xclk = p.u.bt656.clock_curr;
+                        u32 divisor;
+
+                        if (xclk == 0)
+                                return -EINVAL;
+
+                        if (xclk > CAM_MCLK)
+                                xclk = CAM_MCLK;
+
+                        divisor = CAM_MCLK / xclk;
+                        if (divisor * xclk < CAM_MCLK)
+                                divisor++;
+                        if (CAM_MCLK / divisor < p.u.bt656.clock_min
+                            && divisor > 1)
+                                divisor--;
+                        if (divisor > 30)
+                                divisor = 30;
+
+                        xclk = CAM_MCLK / divisor;
+
+                        if (xclk < p.u.bt656.clock_min
+                            || xclk > p.u.bt656.clock_max)
+                                return -EINVAL;
+
+                        cam->if_u.bt656.xclk = xclk;
+                }
+                omap24xxcam_core_xclk_set(cam, cam->if_u.bt656.xclk);
+                break;
+        default:
+                /* FIXME: how about other interfaces? */
+                dev_err(cam->dev, "interface type %d not supported\n",
+                        p.if_type);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static void omap24xxcam_sensor_if_disable(const struct omap24xxcam_device *cam)
+{
+        switch (cam->if_type) {
+        case V4L2_IF_TYPE_BT656:
+                omap24xxcam_core_xclk_set(cam, 0);
+                break;
+        }
+}
+
+/*
+ * Initialise the sensor hardware.
+ */
+static int omap24xxcam_sensor_init(struct omap24xxcam_device *cam)
+{
+        int err = 0;
+        struct v4l2_int_device *sdev = cam->sdev;
+
+        omap24xxcam_clock_on(cam);
+        err = omap24xxcam_sensor_if_enable(cam);
+        if (err) {
+                dev_err(cam->dev, "sensor interface could not be enabled at "
+                        "initialisation, %d\n", err);
+                cam->sdev = NULL;
+                goto out;
+        }
+
+        /* power up sensor during sensor initialization */
+        vidioc_int_s_power(sdev, 1);
+
+        err = vidioc_int_dev_init(sdev);
+        if (err) {
+                dev_err(cam->dev, "cannot initialize sensor, error %d\n", err);
+                /* Sensor init failed --- it's nonexistent to us! */
+                cam->sdev = NULL;
+                goto out;
+        }
+
+        dev_info(cam->dev, "sensor is %s\n", sdev->name);
+
+out:
+        omap24xxcam_sensor_if_disable(cam);
+        omap24xxcam_clock_off(cam);
+
+        vidioc_int_s_power(sdev, 0);
+
+        return err;
+}
+
+static void omap24xxcam_sensor_exit(struct omap24xxcam_device *cam)
+{
+        if (cam->sdev)
+                vidioc_int_dev_exit(cam->sdev);
+}
+
+static void omap24xxcam_sensor_disable(struct omap24xxcam_device *cam)
+{
+        omap24xxcam_sensor_if_disable(cam);
+        omap24xxcam_clock_off(cam);
+        vidioc_int_s_power(cam->sdev, 0);
+}
+
+/*
+ * Power-up and configure camera sensor. It's ready for capturing now.
+ */
+static int omap24xxcam_sensor_enable(struct omap24xxcam_device *cam)
+{
+        int rval;
+
+        omap24xxcam_clock_on(cam);
+
+        omap24xxcam_sensor_if_enable(cam);
+
+        rval = vidioc_int_s_power(cam->sdev, 1);
+        if (rval)
+                goto out;
+
+        rval = vidioc_int_init(cam->sdev);
+        if (rval)
+                goto out;
+
+        return 0;
+
+out:
+        omap24xxcam_sensor_disable(cam);
+
+        return rval;
+}
+
+static void omap24xxcam_sensor_reset_work(struct work_struct *work)
+{
+        struct omap24xxcam_device *cam =
+                container_of(work, struct omap24xxcam_device,
+                             sensor_reset_work);
+
+        if (atomic_read(&cam->reset_disable))
+                return;
+
+        omap24xxcam_capture_stop(cam);
+
+        if (vidioc_int_reset(cam->sdev) == 0) {
+                vidioc_int_init(cam->sdev);
+        } else {
+                /* Can't reset it by vidioc_int_reset. */
+                omap24xxcam_sensor_disable(cam);
+                omap24xxcam_sensor_enable(cam);
+        }
+
+        omap24xxcam_capture_cont(cam);
+}
+
+/*
+ *
+ * IOCTL interface.
+ *
+ */
+
+static int vidioc_querycap(struct file *file, void *fh,
+                           struct v4l2_capability *cap)
+{
+        struct omap24xxcam_fh *ofh = fh;
+        struct omap24xxcam_device *cam = ofh->cam;
+
+        strlcpy(cap->driver, CAM_NAME, sizeof(cap->driver));
+        strlcpy(cap->card, cam->vfd->name, sizeof(cap->card));
+        cap->version = OMAP24XXCAM_VERSION;
+        cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+
+        return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *fh,
+                                   struct v4l2_fmtdesc *f)
+{
+        struct omap24xxcam_fh *ofh = fh;
+        struct omap24xxcam_device *cam = ofh->cam;
+        int rval;
+
+        rval = vidioc_int_enum_fmt_cap(cam->sdev, f);
+
+        return rval;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *fh,
+                                struct v4l2_format *f)
+{
+        struct omap24xxcam_fh *ofh = fh;
+        struct omap24xxcam_device *cam = ofh->cam;
+        int rval;
+
+        mutex_lock(&cam->mutex);
+        rval = vidioc_int_g_fmt_cap(cam->sdev, f);
+        mutex_unlock(&cam->mutex);
+
+        return rval;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *fh,
+                                struct v4l2_format *f)
+{
+        struct omap24xxcam_fh *ofh = fh;
+        struct omap24xxcam_device *cam = ofh->cam;
+        int rval;
+
+        mutex_lock(&cam->mutex);
+        if (cam->streaming) {
+                rval = -EBUSY;
+                goto out;
+        }
+
+        rval = vidioc_int_s_fmt_cap(cam->sdev, f);
+
+out:
+        mutex_unlock(&cam->mutex);
+
+        if (!rval) {
+                mutex_lock(&ofh->vbq.vb_lock);
+                ofh->pix = f->fmt.pix;
+                mutex_unlock(&ofh->vbq.vb_lock);
+        }
+
+        memset(f, 0, sizeof(*f));
+        vidioc_g_fmt_vid_cap(file, fh, f);
+
+        return rval;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *fh,
+                                  struct v4l2_format *f)
+{
+        struct omap24xxcam_fh *ofh = fh;
+        struct omap24xxcam_device *cam = ofh->cam;
+        int rval;
+
+        mutex_lock(&cam->mutex);
+        rval = vidioc_int_try_fmt_cap(cam->sdev, f);
+        mutex_unlock(&cam->mutex);
+
+        return rval;
+}
+
+static int vidioc_reqbufs(struct file *file, void *fh,
+                          struct v4l2_requestbuffers *b)
+{
+        struct omap24xxcam_fh *ofh = fh;
+        struct omap24xxcam_device *cam = ofh->cam;
+        int rval;
+
+        mutex_lock(&cam->mutex);
+        if (cam->streaming) {
+                mutex_unlock(&cam->mutex);
+                return -EBUSY;
+        }
+
+        omap24xxcam_vbq_free_mmap_buffers(&ofh->vbq);
+        mutex_unlock(&cam->mutex);
+