PageRenderTime 65ms CodeModel.GetById 4ms app.highlight 50ms RepoModel.GetById 0ms app.codeStats 1ms

/drivers/media/video/omap1_camera.c

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