IFM O2D cameras use special sensor bus interface glue-logic to
connect camera sensors to mpc5200 LocalPlus bus. Add camera
sensor driver for this mpc5200 camera interface.
Signed-off-by: Anatolij Gustschin <agust@xxxxxxx>
---
drivers/media/platform/soc_camera/Kconfig | 7 +
drivers/media/platform/soc_camera/Makefile | 1 +
.../media/platform/soc_camera/mpc5200-csi-camera.c | 1212 ++++++++++++++++++++
3 files changed, 1220 insertions(+), 0 deletions(-)
create mode 100644 drivers/media/platform/soc_camera/mpc5200-csi-camera.c
diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 9afe1e7..4d6c74d 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -85,3 +85,10 @@ config VIDEO_ATMEL_ISI
This module makes the ATMEL Image Sensor Interface available
as a v4l2 device.
+config VIDEO_MPC52xx_CSI
+ tristate "IFM MPC5200 Camera Sensor Interface driver"
+ depends on SOC_CAMERA && PPC_MPC52xx
+ select PPC_BESTCOMM_GEN_BD
+ select VIDEOBUF2_DMA_CONTIG
+ ---help---
+ This is a driver for IFM camera sensor interface on mpc5200.
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
index 136b7f8..79e6339 100644
--- a/drivers/media/platform/soc_camera/Makefile
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_SOC_CAMERA_PLATFORM) += soc_camera_platform.o
# soc-camera host drivers have to be linked after camera drivers
obj-$(CONFIG_VIDEO_ATMEL_ISI) += atmel-isi.o
+obj-$(CONFIG_VIDEO_MPC52xx_CSI) += mpc5200-csi-camera.o
obj-$(CONFIG_VIDEO_MX1) += mx1_camera.o
obj-$(CONFIG_VIDEO_MX2) += mx2_camera.o
obj-$(CONFIG_VIDEO_MX3) += mx3_camera.o
diff --git a/drivers/media/platform/soc_camera/mpc5200-csi-camera.c b/drivers/media/platform/soc_camera/mpc5200-csi-camera.c
new file mode 100644
index 0000000..84746c1
--- /dev/null
+++ b/drivers/media/platform/soc_camera/mpc5200-csi-camera.c
@@ -0,0 +1,1212 @@
+/*
+ * Driver for image sensor/fpga interface on mpc5200 LPB found
+ * on IFM o2d based boards
+ *
+ * Code base taken from i.MX3x camera host driver
+ * Copyright (C) 2008
+ * Guennadi Liakhovetski, DENX Software Engineering, <lg@xxxxxxx>
+ *
+ * Copyright (C) 2012
+ * Anatolij Gustschin <agust@xxxxxxx>, DENX Software Engineering
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/unistd.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/dma-mapping.h>
+#include <asm/mpc52xx.h>
+
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <linux/sched.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+
+#define O2XXX_CAM_DRV_NAME "o2d_csi"
+
+#define MAX_VIDEO_MEM 2 /* Video memory limit in megabytes */
+
+#define DEFAULT_IMAGE_WIDTH 688 /* Default image width */
+#define DEFAULT_IMAGE_HEIGHT 480 /* Default image height */
+#define DEFAULT_IMAGE_LENGTH (DEFAULT_IMAGE_WIDTH * DEFAULT_IMAGE_HEIGHT)
+
+enum o2xxx_buffer_state {
+ BUF_NEEDS_INIT,
+ BUF_PREPARED,
+};
+
+struct o2xxx_camera_buffer {
+ /* v4l2_buffer must be first */
+ struct vb2_buffer vb;
+ dma_addr_t paddr;
+ size_t bsize;
+ enum o2xxx_buffer_state state;
+ struct list_head queue;
+};
+
+struct o2d_csi_priv {
+ /* sensor control gpios */
+ int imag_capture;
+ int imag_reset;
+ int imag_master_en;
+
+ struct device_node *gpt_np;
+ struct mpc52xx_gpt __iomem *gpt;
+ struct mpc52xx_intr __iomem *intr;
+ int irq;
+ int chip_select;
+
+ /* LocalPlus Bus camera CS phys addr */
+ resource_size_t lpb_cs_phys;
+
+ resource_size_t lpbfifo_regs_phys;
+ void __iomem *lpbfifo_regs;
+
+ /* soc_camera and v4l2 interface */
+ struct platform_device *soc_cam_pdev;
+ struct soc_camera_device *icd;
+ struct soc_camera_host soc_host;
+ size_t buf_total;
+ int sequence;
+ struct vb2_alloc_ctx *alloc_ctx;
+
+ unsigned long image_length;
+
+ struct o2xxx_camera_buffer *active;
+ spinlock_t lock; /* Protects video buffer lists */
+ struct list_head capture;
+ int streaming;
+ int smode;
+ int overrun;
+
+ struct i2c_client *client;
+ struct v4l2_subdev *fsubdev;
+
+ /* for sclpc fifo bestcomm */
+ struct mpc52xx_lpbfifo_request req;
+ int dmareq_queued;
+ int dma_running;
+};
+
+static int o2xxx_queue_dma_request(struct o2d_csi_priv *priv);
+static void o2xxx_bus_master_clock(struct o2d_csi_priv *priv, int onoff);
+
+static struct o2xxx_camera_buffer *to_o2xxx_vb(struct vb2_buffer *vb)
+{
+ return container_of(vb, struct o2xxx_camera_buffer, vb);
+}
+
+static struct i2c_board_info o2xxx_i2c_camera[] = {
+ {
+ I2C_BOARD_INFO("mt9v022", 0x5c),
+ },
+};
+
+static struct i2c_board_info o2xxx_ifm_camera[] = {
+ {
+ I2C_BOARD_INFO("ifm-fpga", 0x48),
+ },
+};
+
+static unsigned long o2xxx_query_bus_param(struct soc_camera_link *icl)
+{
+ pr_debug("%s()\n", __func__);
+
+ return SOCAM_DATAWIDTH_8;
+}
+
+static int o2xxx_add_subdevice(struct soc_camera_device *icd)
+{
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct soc_camera_link *icl = to_soc_camera_link(icd);
+ struct i2c_adapter *adap = i2c_get_adapter(icl->i2c_adapter_id);
+ struct o2d_csi_priv *priv = ici->priv;
+ struct i2c_client *client;
+ struct v4l2_subdev *subdev;
+
+ dev_dbg(icd->parent, "%s()\n", __func__);
+
+ if (!adap) {
+ dev_err(icd->pdev, "Cannot get I2C adapter #%d. No driver?\n",
+ icl->i2c_adapter_id);
+ goto ei2cga;
+ }
+
+ o2xxx_i2c_camera[0].platform_data = icl;
+
+ subdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
+ &o2xxx_i2c_camera[0], NULL);
+ if (!subdev)
+ goto ei2cnd;
+
+ client = v4l2_get_subdevdata(subdev);
+
+ /* Use to_i2c_client(dev) to recover the i2c client */
+ icd->control = &client->dev;
+
+ o2xxx_ifm_camera[0].platform_data = icl;
+ priv->fsubdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
+ &o2xxx_ifm_camera[0], NULL);
+ if (priv->fsubdev) {
+ if (v4l2_ctrl_add_handler(&icd->ctrl_handler,
+ priv->fsubdev->ctrl_handler)) {
+ dev_err(icd->parent, "fpga subdev ctrl hdl failed\n");
+ v4l2_device_unregister_subdev(priv->fsubdev);
+ i2c_unregister_device(v4l2_get_subdevdata(
+ priv->fsubdev));
+ goto ei2cnd;
+ }
+ } else {
+ dev_dbg(icd->parent, "fpga subdev not used\n");
+ }
+
+ dev_dbg(icd->parent, "%s() done\n", __func__);
+ return 0;
+ei2cnd:
+ i2c_put_adapter(adap);
+ei2cga:
+ return -ENODEV;
+}
+
+static void o2xxx_del_subdevice(struct soc_camera_device *icd)
+{
+ struct i2c_client *client =
+ to_i2c_client(to_soc_camera_control(icd));
+ struct i2c_adapter *adap = client->adapter;
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+
+ dev_dbg(icd->parent, "%s()\n", __func__);
+
+ if (priv->fsubdev) {
+ v4l2_device_unregister_subdev(priv->fsubdev);
+ i2c_unregister_device(v4l2_get_subdevdata(priv->fsubdev));
+ }
+
+ icd->control = NULL;
+ v4l2_device_unregister_subdev(i2c_get_clientdata(client));
+ i2c_unregister_device(client);
+ i2c_put_adapter(adap);
+}
+
+static struct soc_camera_link iclink_mt9v022 = {
+ .bus_id = 0, /* Must match with the camera ID */
+ .i2c_adapter_id = 0,
+ .query_bus_param = o2xxx_query_bus_param,
+ .add_device = o2xxx_add_subdevice,
+ .del_device = o2xxx_del_subdevice,
+};
+
+static void o2xxx_sensor_reset(struct o2d_csi_priv *priv)
+{
+ gpio_set_value(priv->imag_reset, 0);
+ udelay(10);
+ gpio_set_value(priv->imag_reset, 1);
+}
+
+static void __maybe_unused o2xxx_sensor_capture(struct o2d_csi_priv *priv)
+{
+ udelay(3);
+ gpio_set_value(priv->imag_capture, 1);
+ udelay(3);
+ gpio_set_value(priv->imag_capture, 0);
+}
+
+static void o2xxx_dma_done_callback(struct mpc52xx_lpbfifo_request *req)
+{
+ struct o2d_csi_priv *priv = req->priv;
+ unsigned long flags;
+
+ spin_lock_irqsave(&priv->lock, flags);
+
+ dev_dbg(priv->icd->parent, "%s(): tb %lu\n", __func__, get_tbl());
+
+ priv->dma_running = 0;
+ priv->sequence++;
+
+ if (list_is_singular(&priv->capture)) {
+ /* not enough buffers in the queue, reuse last buffer */
+ o2xxx_queue_dma_request(priv);
+ priv->overrun++;
+ goto done;
+ }
+
+ if (list_empty(&priv->capture))
+ goto done;
+
+ if (priv->active) {
+ struct vb2_buffer *vb = &priv->active->vb;
+ struct o2xxx_camera_buffer *buf = priv->active;
+
+ list_del_init(&buf->queue);
+ do_gettimeofday(&vb->v4l2_buf.timestamp);
+ vb->v4l2_buf.sequence = priv->sequence;
+ vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+ }
+
+ /* queue next buffer */
+ priv->active = list_entry(priv->capture.next,
+ struct o2xxx_camera_buffer, queue);
+ o2xxx_queue_dma_request(priv);
+
+done:
+ spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int o2xxx_queue_dma_request(struct o2d_csi_priv *priv)
+{
+ struct vb2_buffer *vb = &priv->active->vb;
+ struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+ int ret;
+
+ priv->image_length = vb2_plane_size(vb, 0);
+
+ priv->req.cs = priv->chip_select;
+ priv->req.offset = 0;
+ priv->req.data_phys = buf->paddr;
+ priv->req.data = vb2_plane_vaddr(vb, 0);
+ priv->req.size = priv->image_length;
+ priv->req.pos = 0;
+ priv->req.defer_xfer_start = 1;
+ priv->req.flags = MPC52XX_LPBFIFO_FLAG_READ;
+ priv->req.callback = o2xxx_dma_done_callback;
+ priv->req.priv = priv;
+
+ ret = mpc52xx_lpbfifo_submit(&priv->req);
+ if (ret < 0) {
+ dev_err(priv->icd->parent,
+ "%s: submitting request failed: %d\n", __func__, ret);
+ return ret;
+ }
+
+ dev_dbg(priv->icd->parent, "%s: buf 0x%08x, sz %d\n",
+ __func__, (u32)priv->active->paddr, priv->req.size);
+ priv->dma_running = 0;
+ priv->dmareq_queued = 1;
+
+ return 0;
+}
+
+static int o2xxx_start_dma(struct o2d_csi_priv *priv)
+{
+ int ret;
+
+ /* start fifo transfer */
+ ret = mpc52xx_lpbfifo_start_xfer(&priv->req);
+ if (ret)
+ dev_err(priv->icd->parent, "can't start dma: %d\n", ret);
+
+ priv->dma_running = 1;
+
+ dev_dbg(priv->icd->parent, "%s: buf 0x%08x, tb %lu\n",
+ __func__, (u32)priv->active->paddr, get_tbl());
+ return 0;
+}
+
+/* sets the sysclk clock frequency of the image sensor */
+static int o2xxx_set_sysclk(struct o2d_csi_priv *priv, int int_speed, int on)
+{
+ u32 half_speed;
+ u32 reg;
+
+ /* SYSCLK (TIMER7), 132 / speed = SYSCLK MHz */
+ /* prescaler 1, int_speed counts */
+ half_speed = int_speed / 2;
+ iowrite32be(0x00010000 | (int_speed & 0xff), &priv->gpt->count);
+ iowrite32be(half_speed << 16, &priv->gpt->pwm);
+ iowrite32be((half_speed << 16) + 1, &priv->gpt->pwm); /* load */
+ if (on)
+ reg = 0x00000003; /* module enable, PWM mode */
+ else
+ reg = 0x00000001;
+
+ iowrite32be(reg, &priv->gpt->mode);
+
+ return 0;
+}
+
+static int o2xxx_camera_add_device(struct soc_camera_device *icd)
+{
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+
+ if (priv->icd)
+ return -EBUSY;
+
+ priv->icd = icd;
+
+ dev_info(icd->parent, "Camera driver attached to camera %d\n",
+ icd->devnum);
+
+ return 0;
+}
+
+static void o2xxx_camera_remove_device(struct soc_camera_device *icd)
+{
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+
+ dev_dbg(icd->parent, "%s()\n", __func__);
+
+ BUG_ON(icd != priv->icd);
+
+ priv->icd = NULL;
+
+ dev_info(icd->parent, "Camera driver detached from camera %d\n",
+ icd->devnum);
+}
+
+static int o2xxx_camera_set_crop(struct soc_camera_device *icd,
+ const struct v4l2_crop *a)
+{
+ struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+ int ret;
+
+ dev_dbg(icd->parent, "%s()\n", __func__);
+
+ ret = v4l2_subdev_call(sd, video, s_crop, a);
+ if (ret < 0)
+ return ret;
+ return 0;
+}
+
+static int o2xxx_camera_set_fmt(struct soc_camera_device *icd,
+ struct v4l2_format *f)
+{
+ struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+ const struct soc_camera_format_xlate *xlate;
+ struct v4l2_pix_format *pix = &f->fmt.pix;
+ struct v4l2_mbus_framefmt mf;
+ int ret;
+
+ dev_dbg(icd->parent, "%s()\n", __func__);
+
+ xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+ if (!xlate) {
+ dev_warn(icd->parent, "Format %x not found\n",
+ pix->pixelformat);
+ return -EINVAL;
+ }
+
+ dev_dbg(icd->parent, "Set format %dx%d\n", pix->width, pix->height);
+
+ mf.width = pix->width;
+ mf.height = pix->height;
+ mf.field = pix->field;
+ mf.colorspace = pix->colorspace;
+ mf.code = xlate->code;
+
+ ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+ if (ret < 0) {
+ dev_err(icd->parent, "sub s_mbus_fmt error\n");
+ return ret;
+ }
+
+ if (mf.code != xlate->code) {
+ dev_err(icd->parent, "code error\n");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int o2xxx_camera_try_fmt(struct soc_camera_device *icd,
+ struct v4l2_format *f)
+{
+ dev_dbg(icd->parent, "%s()\n", __func__);
+ return 0;
+}
+
+static const struct soc_mbus_pixelfmt o2xxx_camera_formats[] = {
+ {
+ .fourcc = V4L2_PIX_FMT_SBGGR8,
+ .name = "Bayer BGGR (sRGB) 8 bit",
+ .bits_per_sample = 8,
+ .packing = SOC_MBUS_PACKING_NONE,
+ .order = SOC_MBUS_ORDER_LE,
+ }, {
+ .fourcc = V4L2_PIX_FMT_GREY,
+ .name = "Monochrome 8 bit",
+ .bits_per_sample = 8,
+ .packing = SOC_MBUS_PACKING_NONE,
+ .order = SOC_MBUS_ORDER_LE,
+ },
+};
+
+static int o2xxx_camera_get_formats(struct soc_camera_device *icd,
+ unsigned int idx, struct soc_camera_format_xlate *xlate)
+{
+ struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+ struct device *dev = icd->parent;
+ int formats = 0, ret;
+ enum v4l2_mbus_pixelcode code;
+ const struct soc_mbus_pixelfmt *fmt;
+
+ dev_dbg(dev, "%s()\n", __func__);
+
+ ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+ if (ret < 0)
+ return 0; /* No more formats */
+
+ fmt = soc_mbus_get_fmtdesc(code);
+ if (!fmt) {
+ dev_warn(icd->parent,
+ "Unsupported format code #%u: %d\n", idx, code);
+ return 0;
+ }
+
+ switch (code) {
+ case V4L2_MBUS_FMT_SBGGR8_1X8:
+ formats++;
+ if (xlate) {
+ xlate->host_fmt = &o2xxx_camera_formats[0];
+ xlate->code = code;
+ xlate++;
+ dev_dbg(dev, "Providing format %s using code %d\n",
+ o2xxx_camera_formats[0].name, code);
+ }
+ break;
+ case V4L2_MBUS_FMT_Y10_1X10:
+ formats++;
+ if (xlate) {
+ xlate->host_fmt = &o2xxx_camera_formats[1];
+ xlate->code = code;
+ xlate++;
+ dev_dbg(dev, "Providing format %s using code %d\n",
+ o2xxx_camera_formats[1].name, code);
+ }
+ break;
+ case V4L2_MBUS_FMT_Y8_1X8:
+ formats++;
+ if (xlate) {
+ xlate->host_fmt = &o2xxx_camera_formats[0];
+ xlate->code = code;
+ xlate++;
+ dev_dbg(dev, "Providing format %s using code %d\n",
+ o2xxx_camera_formats[0].name, code);
+ }
+ break;
+ default:
+ return 0;
+ }
+
+ /* Generic pass-through */
+ formats++;
+ if (xlate) {
+ xlate->host_fmt = fmt;
+ xlate->code = code;
+ dev_dbg(dev, "Providing format %c%c%c%c in pass-through mode\n",
+ (fmt->fourcc >> (0*8)) & 0xFF,
+ (fmt->fourcc >> (1*8)) & 0xFF,
+ (fmt->fourcc >> (2*8)) & 0xFF,
+ (fmt->fourcc >> (3*8)) & 0xFF);
+ xlate++;
+ }
+
+ dev_dbg(icd->parent, "%s() formats %d\n", __func__, formats);
+ return formats;
+}
+
+static int o2xxx_videobuf_setup(struct vb2_queue *vq,
+ const struct v4l2_format *fmt,
+ unsigned int *count, unsigned int *num_planes,
+ unsigned int sizes[], void *alloc_ctxs[])
+{
+ struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+ int bytes_per_line;
+ unsigned int height;
+
+ dev_dbg(icd->parent, "%s(), count %d\n", __func__, *count);
+
+ if (fmt) {
+ const struct soc_camera_format_xlate *xlate =
+ soc_camera_xlate_by_fourcc(icd, fmt->fmt.pix.pixelformat);
+
+ if (!xlate)
+ return -EINVAL;
+ bytes_per_line = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+ xlate->host_fmt);
+ height = fmt->fmt.pix.height;
+ } else {
+ /* Called from VIDIOC_REQBUFS or in compatibility mode */
+ bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+ icd->current_fmt->host_fmt);
+ height = icd->user_height;
+ }
+ if (bytes_per_line < 0)
+ return bytes_per_line;
+
+ sizes[0] = bytes_per_line * height;
+
+ alloc_ctxs[0] = priv->alloc_ctx;
+
+ if (!vq->num_buffers)
+ priv->sequence = 0;
+
+ if (!*count)
+ *count = 6;
+
+ /* If *num_planes != 0, we have already verified *count. */
+ if (!*num_planes &&
+ sizes[0] * *count + priv->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
+ *count = (MAX_VIDEO_MEM * 1024 * 1024 - priv->buf_total) /
+ sizes[0];
+
+ *num_planes = 1;
+
+ return 0;
+}
+
+static void o2xxx_videobuf_queue(struct vb2_buffer *vb)
+{
+ struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+ struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+ unsigned long flags;
+
+ if (vb2_plane_size(vb, 0) < icd->sizeimage) {
+ dev_err(icd->parent, "buffer #%d too small (%lu < %zu)\n",
+ vb->v4l2_buf.index, vb2_plane_size(vb, 0), icd->sizeimage);
+ goto error;
+ }
+
+ if (buf->state == BUF_NEEDS_INIT) {
+ buf->paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+ buf->bsize = icd->sizeimage;
+ buf->state = BUF_PREPARED;
+ }
+
+ vb2_set_plane_payload(vb, 0, icd->sizeimage);
+
+#ifdef DEBUG
+ if (vb2_plane_vaddr(vb, 0))
+ memset(vb2_plane_vaddr(vb, 0), 0xa5,
+ vb2_get_plane_payload(vb, 0));
+#endif
+
+ spin_lock_irqsave(&priv->lock, flags);
+
+ list_add_tail(&buf->queue, &priv->capture);
+
+ if (!priv->active) {
+ priv->active = buf;
+ if (o2xxx_queue_dma_request(priv)) {
+ list_del_init(&buf->queue);
+ priv->active = NULL;
+ spin_unlock_irqrestore(&priv->lock, flags);
+ goto error;
+ }
+ }
+
+ spin_unlock_irqrestore(&priv->lock, flags);
+
+ return;
+
+error:
+ vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static void o2xxx_videobuf_release(struct vb2_buffer *vb)
+{
+ struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+ struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+ unsigned long flags;
+
+ dev_dbg(icd->parent,
+ "release%s buf 0x%08lx, queue %sempty\n",
+ priv->active == buf ? " active" : "", (ulong)buf->paddr,
+ list_empty(&buf->queue) ? "" : "not ");
+
+ spin_lock_irqsave(&priv->lock, flags);
+
+ if (priv->active == buf)
+ priv->active = NULL;
+
+ list_del_init(&buf->queue);
+ buf->state = BUF_NEEDS_INIT;
+ priv->buf_total -= vb2_plane_size(vb, 0);
+
+ spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int o2xxx_videobuf_init(struct vb2_buffer *vb)
+{
+ struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+ struct o2xxx_camera_buffer *buf = to_o2xxx_vb(vb);
+
+ INIT_LIST_HEAD(&buf->queue);
+ buf->state = BUF_NEEDS_INIT;
+ priv->buf_total += vb2_plane_size(vb, 0);
+
+ return 0;
+}
+
+static int o2xxx_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+ struct soc_camera_device *icd = soc_camera_from_vb2q(q);
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+ struct v4l2_subdev *sd;
+ int ret;
+
+ /* snapshot case */
+ priv->smode = 0;
+
+ sd = soc_camera_to_subdev(priv->icd);
+ ret = v4l2_subdev_call(sd, video, s_stream, priv->smode);
+ if (ret < 0) {
+ dev_err(icd->parent, "Can't switch to snapshot mode\n");
+ return ret;
+ }
+
+ priv->streaming = 1;
+
+ /* now use bus clock for sensor readout */
+ o2xxx_bus_master_clock(priv, 1);
+
+ return 0;
+}
+
+static int o2xxx_stop_streaming(struct vb2_queue *q)
+{
+ struct soc_camera_device *icd = soc_camera_from_vb2q(q);
+ struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+ struct o2d_csi_priv *priv = ici->priv;
+ struct o2xxx_camera_buffer *buf, *tmp;
+ unsigned long flags;
+
+ spin_lock_irqsave(&priv->lock, flags);
+
+ priv->active = NULL;
+ priv->streaming = 0;
+ if (priv->dmareq_queued) {
+ mpc52xx_lpbfifo_abort(&priv->req);
+ priv->dmareq_queued = 0;
+ }
+
+ list_for_each_entry_safe(buf, tmp, &priv->capture, queue) {
+ list_del_init(&buf->queue);
+ vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+ }
+
+ spin_unlock_irqrestore(&priv->lock, flags);
+
+ /* Enable clock for every instance */
+ o2xxx_bus_master_clock(priv, 0);
+ return 0;
+}
+
+static struct vb2_ops o2xxx_videobuf_ops = {
+ .queue_setup = o2xxx_videobuf_setup,
+ .buf_init = o2xxx_videobuf_init,
+ .buf_queue = o2xxx_videobuf_queue,
+ .buf_cleanup = o2xxx_videobuf_release,
+ .wait_prepare = soc_camera_unlock,
+ .wait_finish = soc_camera_lock,
+ .start_streaming = o2xxx_start_streaming,
+ .stop_streaming = o2xxx_stop_streaming,
+};
+
+static int o2xxx_camera_init_videobuf(struct vb2_queue *q,
+ struct soc_camera_device *icd)
+{
+ q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ q->io_modes = VB2_MMAP;
+ q->drv_priv = icd;
+ q->ops = &o2xxx_videobuf_ops;
+ q->mem_ops = &vb2_dma_contig_memops;
+ q->buf_struct_size = sizeof(struct o2xxx_camera_buffer);
+
+ dev_dbg(icd->parent, "%s()\n", __func__);
+ return vb2_queue_init(q);
+}
+
+static int o2xxx_camera_reqbufs(struct soc_camera_device *icd,
+ struct v4l2_requestbuffers *p)
+{
+ dev_dbg(icd->parent, "%s()\n", __func__);
+ return 0;
+}
+
+static unsigned int o2xxx_camera_poll(struct file *file, poll_table *pt)
+{
+ struct soc_camera_device *icd = file->private_data;
+
+ return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int o2xxx_camera_querycap(struct soc_camera_host *ici,
+ struct v4l2_capability *cap)
+{
+ strlcpy(cap->card, "o2xxx Camera", sizeof(cap->card));
+ cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+ return 0;
+}
+
+static int o2xxx_camera_set_bus_param(struct soc_camera_device *icd)
+{
+ return 0;
+}
+
+static struct soc_camera_host_ops o2xxx_soc_camera_host_ops = {
+ .owner = THIS_MODULE,
+ .add = o2xxx_camera_add_device,
+ .remove = o2xxx_camera_remove_device,
+ .set_crop = o2xxx_camera_set_crop,
+ .set_fmt = o2xxx_camera_set_fmt,
+ .try_fmt = o2xxx_camera_try_fmt,
+ .get_formats = o2xxx_camera_get_formats,
+ .init_videobuf2 = o2xxx_camera_init_videobuf,
+ .reqbufs = o2xxx_camera_reqbufs,
+ .poll = o2xxx_camera_poll,
+ .querycap = o2xxx_camera_querycap,
+ .set_bus_param = o2xxx_camera_set_bus_param,
+};
+
+static void o2xxx_bus_master_clock(struct o2d_csi_priv *priv, int onoff)
+{
+ pr_debug("%s(): %s\n", __func__, onoff ? "ON" : "OFF");
+ gpio_set_value(priv->imag_master_en, onoff);
+}
+
+static irqreturn_t o2xxx_irq(int irq, void *dev_id)
+{
+ struct o2d_csi_priv *priv = dev_id;
+ static unsigned long cj;
+
+ spin_lock(&priv->lock);
+
+ if (!priv->streaming) {
+ if (printk_timed_ratelimit(&cj, 5000))
+ pr_debug("%s: not streaming\n", __func__);
+ spin_unlock(&priv->lock);
+ return IRQ_HANDLED;
+ }
+
+ if (priv->active && priv->dmareq_queued && !priv->dma_running)
+ o2xxx_start_dma(priv);
+
+ spin_unlock(&priv->lock);
+
+ return IRQ_HANDLED;
+}
+
+static int o2d_csi_sclpc_init(struct platform_device *pdev,
+ struct o2d_csi_priv *priv)
+{
+ struct device_node *sclpc_np;
+ struct resource sclpc_res;
+
+ sclpc_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200-lpbfifo");
+ if (!sclpc_np)
+ return -ENODEV;
+
+ if (of_address_to_resource(sclpc_np, 0, &sclpc_res))
+ return -ENODEV;
+
+ priv->lpbfifo_regs_phys = sclpc_res.start;
+ priv->lpbfifo_regs = of_iomap(sclpc_np, 0);
+ of_node_put(sclpc_np);
+ if (!priv->lpbfifo_regs)
+ return -ENOMEM;
+
+ return 0;
+}
+
+static int o2d_csi_lpb_init(struct platform_device *pdev,
+ struct o2d_csi_priv *priv)
+{
+ struct device_node *np = pdev->dev.of_node;
+ struct mpc52xx_mmap_ctl __iomem *immr_regs;
+ struct device_node *immr_np;
+ const unsigned int *prop;
+ void __iomem *csx_start;
+ void __iomem *csx_stop;
+ struct resource res;
+ int chip_select_shift;
+ int csx_cfg_offs;
+ u32 cs_config;
+ u32 val;
+
+ if (of_address_to_resource(np, 0, &res)) {
+ dev_err(&pdev->dev, "coudn't get mapping\n");
+ return -ENODEV;
+ }
+ priv->lpb_cs_phys = res.start;
+
+ if (of_property_read_u32(np, "ifm,csi-addr-bus-width", &val)) {
+ dev_err(&pdev->dev, "missing addr bus width property\n");
+ return -EINVAL;
+ }
+
+ cs_config = 0x00001001; /* chip enable, read-only */
+
+ switch (val) {
+ case 24:
+ cs_config |= 2 << 10;
+ break;
+ case 16:
+ cs_config |= 1 << 10;
+ break;
+ case 8:
+ /* nop: cs_config |= 0 << 10; */
+ break;
+ default:
+ if (val >= 25)
+ cs_config |= 3 << 10;
+ else {
+ dev_err(&pdev->dev, "invalid addr bus width\n");
+ return -EINVAL;
+ }
+ }
+
+ if (of_property_read_u32(np, "ifm,csi-data-bus-width", &val)) {
+ dev_err(&pdev->dev, "missing data bus width property\n");
+ return -EINVAL;
+ }
+
+ switch (val) {
+ case 32:
+ cs_config |= 3 << 8;
+ break;
+ case 16:
+ cs_config |= 1 << 8;
+ break;
+ case 8:
+ /* nop: cs_config |= 0 << 8;*/
+ break;
+ default:
+ dev_err(&pdev->dev, "invalid data bus width\n");
+ return -EINVAL;
+ }
+
+ if (of_property_read_u32(np, "ifm,csi-wait-cycles", &val)) {
+ dev_err(&pdev->dev, "missing wait cycles property\n");
+ return -EINVAL;
+ }
+
+ cs_config |= (val & 0xff) << 24 | (val & 0xff) << 16;
+
+ if (of_property_read_bool(np, "ifm,csi-byte-swap"))
+ cs_config |= 0x4;
+
+ /* find CS # */
+ prop = of_get_property(np, "reg", &val);
+ if (!prop || val != 12 || !*prop || *prop > 7) {
+ dev_err(&pdev->dev, "invalid reg property\n");
+ return -EINVAL;
+ }
+ priv->chip_select = *prop;
+
+ immr_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200b-immr");
+ if (!immr_np)
+ return -ENODEV;
+
+ immr_regs = of_iomap(immr_np, 0);
+ of_node_put(immr_np);
+ if (!immr_regs)
+ return -ENODEV;
+
+ switch (priv->chip_select) {
+ case 7:
+ chip_select_shift = 27;
+ csx_cfg_offs = 0x0324;
+ csx_start = &immr_regs->cs7_start;
+ csx_stop = &immr_regs->cs7_stop;
+ break;
+ case 6:
+ chip_select_shift = 26;
+ csx_cfg_offs = 0x0320;
+ csx_start = &immr_regs->cs6_start;
+ csx_stop = &immr_regs->cs6_stop;
+ break;
+ default:
+ chip_select_shift = 16 + priv->chip_select;
+ csx_cfg_offs = 0x0300 + priv->chip_select * 4;
+ csx_start = &immr_regs->cs0_start + priv->chip_select * 2;
+ csx_stop = &immr_regs->cs0_stop + priv->chip_select * 2;
+ }
+
+ dev_dbg(&pdev->dev, "CS%d, cfg 0x%x, shift %d, cfg offs 0x%x\n",
+ priv->chip_select, cs_config, chip_select_shift, csx_cfg_offs);
+
+ /* CS base and size */
+ iowrite32be(priv->lpb_cs_phys >> 16, csx_start);
+ iowrite32be((priv->lpb_cs_phys + resource_size(&res)) >> 16, csx_stop);
+
+ iowrite32be(cs_config, (void *)immr_regs + csx_cfg_offs);
+
+ /* set CS deadcycles to 0 */
+ val = ioread32be((void *)immr_regs + 0x032c);
+ val &= ~(0xf << (priv->chip_select * 4));
+ iowrite32be(val, (void *)immr_regs + 0x032c);
+
+ /* CS enable */
+ val = ioread32be(&immr_regs->ipbi_ws_ctrl);
+ val |= 0x1 << chip_select_shift;
+ iowrite32be(val, &immr_regs->ipbi_ws_ctrl);
+
+ iounmap(immr_regs);
+ return 0;
+}
+
+static int mpc52xx_xlb_pldis_bsdis(int pldis, int bsdis)
+{
+ struct mpc52xx_xlb __iomem *xlb_regs;
+ struct device_node *xlb_np;
+
+ xlb_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200-xlb");
+ if (!xlb_np)
+ return -ENODEV;
+ xlb_regs = of_iomap(xlb_np, 0);
+ of_node_put(xlb_np);
+ if (!xlb_regs)
+ return -ENOMEM;
+
+ if (pldis)
+ setbits32(&xlb_regs->config, 0x80000000);
+ else
+ clrbits32(&xlb_regs->config, 0x80000000);
+
+ if (bsdis)
+ setbits32(&xlb_regs->config, 0x00010000);
+ else
+ clrbits32(&xlb_regs->config, 0x00010000);
+
+ iounmap(xlb_regs);
+ return 0;
+}
+
+static int o2d_csi_probe(struct platform_device *pdev)
+{
+ struct device_node *np = pdev->dev.of_node;
+ struct device_node *sdma_np;
+ struct o2d_csi_priv *priv;
+ struct mpc52xx_sdma __iomem *sdma_regs;
+ struct soc_camera_host *soc_host;
+ int ret;
+ int irq;
+
+ irq = irq_of_parse_and_map(np, 0);
+ if (!irq) {
+ dev_err(&pdev->dev, "Coudn't get irq\n");
+ return -ENODEV;
+ }
+
+ if (of_gpio_count(np) < 3) {
+ dev_err(&pdev->dev, "Required gpio cfg. not available\n");
+ return -ENODEV;
+ }
+
+ priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+ if (!priv)
+ return -ENOMEM;
+
+ priv->irq = irq;
+
+ /* list of video-buffers */
+ INIT_LIST_HEAD(&priv->capture);
+ spin_lock_init(&priv->lock);
+
+ /* disable xlb pipelining and bestcomm xlb snooping */
+ ret = mpc52xx_xlb_pldis_bsdis(1, 1);
+ if (ret) {
+ dev_err(&pdev->dev, "Can't configure xlb\n");
+ return ret;
+ }
+
+ sdma_np = of_find_compatible_node(NULL, NULL, "fsl,mpc5200-bestcomm");
+ if (!sdma_np)
+ return -ENODEV;
+ sdma_regs = of_iomap(sdma_np, 0);
+ of_node_put(sdma_np);
+ if (!sdma_regs)
+ return -ENOMEM;
+
+ /* disable prefetch on commbus */
+ setbits16(&sdma_regs->PtdCntrl, 1);
+ iounmap(sdma_regs);
+
+ ret = o2d_csi_sclpc_init(pdev, priv);
+ if (ret)
+ goto err_request3;
+
+ ret = o2d_csi_lpb_init(pdev, priv);
+ if (ret)
+ goto err_request3;
+
+ priv->imag_capture = of_get_gpio(np, 0);
+ priv->imag_reset = of_get_gpio(np, 1);
+ priv->imag_master_en = of_get_gpio(np, 2);
+
+ if (!gpio_is_valid(priv->imag_capture) ||
+ !gpio_is_valid(priv->imag_reset) ||
+ !gpio_is_valid(priv->imag_master_en)) {
+ dev_err(&pdev->dev, "%s: invalid GPIO pins\n", np->full_name);
+ return -ENODEV;
+ }
+
+ priv->gpt_np = of_parse_phandle(np, "ifm,csi-clk-handle", 0);
+ if (!priv->gpt_np)
+ return -ENODEV;
+
+ priv->gpt = of_iomap(priv->gpt_np, 0);
+ if (!priv->gpt) {
+ ret = -ENOMEM;
+ goto err_iomap;
+ }
+
+ ret = gpio_request(priv->imag_capture, "imag_capture");
+ if (ret)
+ goto err_request1;
+ ret = gpio_request(priv->imag_reset, "imag_reset");
+ if (ret)
+ goto err_request2;
+ ret = gpio_request(priv->imag_master_en, "imag_master_en");
+ if (ret)
+ goto err_request3;
+
+ o2xxx_set_sysclk(priv, 6, 1);
+
+ gpio_direction_output(priv->imag_capture, 0);
+ gpio_direction_output(priv->imag_reset, 1);
+ gpio_direction_output(priv->imag_master_en, 0);
+ o2xxx_bus_master_clock(priv, 0);
+
+ o2xxx_sensor_reset(priv);
+ mdelay(20);
+
+ priv->image_length = DEFAULT_IMAGE_LENGTH;
+
+ priv->soc_cam_pdev = platform_device_register_resndata(NULL,
+ "soc-camera-pdrv", 0, NULL, 0,
+ &iclink_mt9v022, sizeof(iclink_mt9v022));
+
+ soc_host = &priv->soc_host;
+ soc_host->drv_name = O2XXX_CAM_DRV_NAME;
+ soc_host->ops = &o2xxx_soc_camera_host_ops;
+ soc_host->priv = priv;
+ soc_host->v4l2_dev.dev = &pdev->dev;
+ soc_host->nr = 0;
+
+ priv->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+ if (IS_ERR(priv->alloc_ctx)) {
+ ret = PTR_ERR(priv->alloc_ctx);
+ goto err_request3;
+ }
+
+ ret = soc_camera_host_register(soc_host);
+ if (ret) {
+ dev_err(&pdev->dev, "Coudn't register host\n");
+ goto err_hostreg;
+ }
+
+ o2xxx_set_sysclk(priv, 6, 1);
+ mdelay(20);
+
+ ret = request_irq(irq, o2xxx_irq, 0, "mpc52xx-csi", priv);
+ if (ret)
+ goto err_hostreg;
+
+ dev_info(&pdev->dev, "initialised\n");
+
+ return 0;
+
+err_hostreg:
+ vb2_dma_contig_cleanup_ctx(priv->alloc_ctx);
+err_request3:
+ gpio_free(priv->imag_reset);
+err_request2:
+ gpio_free(priv->imag_capture);
+err_request1:
+ iounmap(priv->gpt);
+err_iomap:
+ if (priv->gpt_np)
+ of_node_put(priv->gpt_np);
+ return ret;
+}
+
+static int o2d_csi_remove(struct platform_device *pdev)
+{
+ struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+ struct o2d_csi_priv *priv = container_of(soc_host, struct o2d_csi_priv,
+ soc_host);
+
+ dev_info(&pdev->dev, "released\n");
+
+ /* stop sensor clock */
+ o2xxx_set_sysclk(priv, 6, 0);
+
+ free_irq(priv->irq, priv);
+
+ soc_camera_host_unregister(soc_host);
+
+ vb2_dma_contig_cleanup_ctx(priv->alloc_ctx);
+
+ if (priv->gpt_np)
+ of_node_put(priv->gpt_np);
+
+ iounmap(priv->lpbfifo_regs);
+ iounmap(priv->gpt);
+
+ platform_device_unregister(priv->soc_cam_pdev);
+
+ /* hold sensor reset */
+ gpio_set_value(priv->imag_reset, 0);
+ gpio_free(priv->imag_master_en);
+ gpio_free(priv->imag_reset);
+ gpio_free(priv->imag_capture);
+
+ /* xlb pipelining and snooping on */
+ mpc52xx_xlb_pldis_bsdis(0, 0);
+ return 0;
+}
+
+static struct of_device_id __devinitdata o2d_csi_match[] = {
+ { .compatible = "ifm,o2d-csi", },
+ { /* end */ }
+};
+MODULE_DEVICE_TABLE(of, o2d_csi_match);
+
+static struct platform_driver o2d_csi_driver = {
+ .driver = {
+ .name = "o2d_csi",
+ .owner = THIS_MODULE,
+ .of_match_table = o2d_csi_match,
+ },
+ .probe = o2d_csi_probe,
+ .remove = __devexit_p(o2d_csi_remove),
+};
+
+module_platform_driver(o2d_csi_driver);
+
+MODULE_AUTHOR("Anatolij Gustschin, agust@xxxxxxx");
+MODULE_DESCRIPTION("o2d camera sensor interface driver");
+MODULE_LICENSE("GPL");
--
1.7.1
--
To unsubscribe from this list: send the line "unsubscribe linux-media" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
