From patchwork Fri Sep 28 08:50:54 2012 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Anatolij Gustschin X-Patchwork-Id: 1517821 Return-Path: X-Original-To: patchwork-linux-media@patchwork.kernel.org Delivered-To: patchwork-process-083081@patchwork1.kernel.org Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by patchwork1.kernel.org (Postfix) with ESMTP id 059A33FC71 for ; Fri, 28 Sep 2012 08:51:17 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1756892Ab2I1IvO (ORCPT ); Fri, 28 Sep 2012 04:51:14 -0400 Received: from mail-out.m-online.net ([212.18.0.9]:56903 "EHLO mail-out.m-online.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1754924Ab2I1IvJ (ORCPT ); Fri, 28 Sep 2012 04:51:09 -0400 Received: from frontend1.mail.m-online.net (unknown [192.168.8.180]) by mail-out.m-online.net (Postfix) with ESMTP id 3XSmnl2tS2z4KK6q; Fri, 28 Sep 2012 10:51:07 +0200 (CEST) X-Auth-Info: sY7VVtfsNh9M26Yhhe7xKKixXGujeWHDeUTBr26VULM= Received: from localhost (pD9E2F498.dip.t-dialin.net [217.226.244.152]) (using TLSv1 with cipher DHE-RSA-AES128-SHA (128/128 bits)) (No client certificate requested) by mail.mnet-online.de (Postfix) with ESMTPSA id 3XSmnj4NJMzbcjL; Fri, 28 Sep 2012 10:51:05 +0200 (CEST) From: Anatolij Gustschin To: linux-media@vger.kernel.org Cc: Guennadi Liakhovetski , Mauro Carvalho Chehab Subject: [PATCH 1/2] V4L: soc_camera: add driver for IFM camera sensor interface on mpc5200 Date: Fri, 28 Sep 2012 10:50:54 +0200 Message-Id: <1348822255-30875-1-git-send-email-agust@denx.de> X-Mailer: git-send-email 1.7.1 Sender: linux-media-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: linux-media@vger.kernel.org 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 --- 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, + * + * Copyright (C) 2012 + * Anatolij Gustschin , 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#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@denx.de"); +MODULE_DESCRIPTION("o2d camera sensor interface driver"); +MODULE_LICENSE("GPL");