mxc_v4l2_output.c

LINUX下的ov2640驱动程序

/*
 * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
 */

/*
 * The code contained herein is licensed under the GNU General Public
 * License. You may obtain a copy of the GNU General Public License
 * Version 2 or later at the following locations:
 *
 * http://www.opensource.org/licenses/gpl-license.html
 * http://www.gnu.org/copyleft/gpl.html
 */

/*!
 * @file drivers/media/video/mxc/output/mxc_v4l2_output.c
 *
 * @brief MXC V4L2 Video Output Driver
 *
 * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
 *
 * @ingroup MXC_V4L2_OUTPUT
 */
#include <linux/module.h>
#include <linux/init.h>
#include <linux/fs.h>
#include <linux/delay.h>
#include <linux/platform_device.h>
#include <asm/io.h>
#include <asm/semaphore.h>
#include <linux/dma-mapping.h>

#include <asm/arch/mxcfb.h>
#include "mxc_v4l2_output.h"

vout_data *g_vout;
#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565

struct v4l2_output mxc_outputs[2] = {
	{
	 .index = MXC_V4L2_OUT_2_SDC,
	 .name = "DISP3 Video Out",
	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
						   but no other choice */
	 .audioset = 0,
	 .modulator = 0,
	 .std = V4L2_STD_UNKNOWN},
	{
	 .index = MXC_V4L2_OUT_2_ADC,
	 .name = "DISPx Video Out",
	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
						   but no other choice */
	 .audioset = 0,
	 .modulator = 0,
	 .std = V4L2_STD_UNKNOWN}
};

static int video_nr = 16;
static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;

/* debug counters */
uint32_t g_irq_cnt;
uint32_t g_buf_output_cnt;
uint32_t g_buf_q_cnt;
uint32_t g_buf_dq_cnt;

static int dq_intr_cnt=0;
static int dq_timeout_cnt=0;

#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
static __inline int queue_size(v4l_queue * q)
{
	if (q->tail >= q->head)
		return (q->tail - q->head);
	else
		return ((q->tail + QUEUE_SIZE) - q->head);
}

static __inline int queue_buf(v4l_queue * q, int idx)
{
	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
		return -1;	/* queue full */
	q->list[q->tail] = idx;
	q->tail = (q->tail + 1) % QUEUE_SIZE;
	return 0;
}

static __inline int dequeue_buf(v4l_queue * q)
{
	int ret;
	if (q->tail == q->head)
		return -1;	/* queue empty */
	ret = q->list[q->head];
	q->head = (q->head + 1) % QUEUE_SIZE;
	return ret;
}

static __inline int peek_next_buf(v4l_queue * q)
{
	if (q->tail == q->head)
		return -1;	/* queue empty */
	return q->list[q->head];
}

static __inline unsigned long get_jiffies(struct timeval *t)
{
	struct timeval cur;

	if (t->tv_usec >= 1000000) {
		t->tv_sec += t->tv_usec / 1000000;
		t->tv_usec = t->tv_usec % 1000000;
	}

	do_gettimeofday(&cur);
	if ((t->tv_sec < cur.tv_sec)
	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
		return jiffies;

	if (t->tv_usec < cur.tv_usec) {
		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
	} else {
		cur.tv_sec = t->tv_sec - cur.tv_sec;
		cur.tv_usec = t->tv_usec - cur.tv_usec;
	}

	return jiffies + timeval_to_jiffies(&cur);
}

/*!
 * Private function to free buffers
 *
 * @param bufs_paddr	Array of physical address of buffers to be freed
 *
 * @param bufs_vaddr	Array of virtual address of buffers to be freed
 *
 * @param num_buf	Number of buffers to be freed
 *
 * @param size		Size for each buffer to be free
 *
 * @return status  0 success.
 */
static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
			    int num_buf, int size)
{
	int i;

	for (i = 0; i < num_buf; i++) {
		if (bufs_vaddr[i] != 0) {
			dma_free_coherent(0, size, bufs_vaddr[i],
					  bufs_paddr[i]);
			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
			bufs_paddr[i] = 0;
			bufs_vaddr[i] = NULL;
		}
	}
	return 0;
}

/*!
 * Private function to allocate buffers
 *
 * @param bufs_paddr	Output array of physical address of buffers allocated
 *
 * @param bufs_vaddr	Output array of virtual address of buffers allocated
 *
 * @param num_buf	Input number of buffers to allocate
 *
 * @param size		Input size for each buffer to allocate
 *
 * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
 */
static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
				int num_buf, int size)
{
	int i;

	for (i = 0; i < num_buf; i++) {
		bufs_vaddr[i] = dma_alloc_coherent(0, size,
						   &bufs_paddr[i],
						   GFP_DMA | GFP_KERNEL);

		if (bufs_vaddr[i] == 0) {
			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
			printk(KERN_ERR "dma_alloc_coherent failed.\n");
			return -ENOBUFS;
		}
		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
			 (u32) bufs_paddr[i], size);
	}

	return 0;
}

static void mxc_v4l2out_timer_handler(unsigned long arg)
{
	int index;
	unsigned long timeout;
	u32 lock_flags = 0;
	vout_data *vout = (vout_data *) arg;

	dev_dbg(vout->video_dev->dev, "timer handler: %lu\n", jiffies);

	spin_lock_irqsave(&g_lock, lock_flags);

	/*
	 * If timer occurs before IPU h/w is ready, then set the state to
	 * paused and the timer will be set again when next buffer is queued
	 * or PP comletes
	 */
	if (vout->ipu_buf[vout->next_rdy_ipu_buf] != -1) {
		dev_dbg(vout->video_dev->dev, "IPU buffer busy\n");
		vout->state = STATE_STREAM_PAUSED;
		goto exit0;
	}

	/* Dequeue buffer and pass to IPU */
	index = dequeue_buf(&vout->ready_q);
	if (index == -1) {	/* no buffers ready, should never occur */
		dev_err(vout->video_dev->dev,
			"mxc_v4l2out: timer - no queued buffers ready\n");
		goto exit0;
	}

	g_buf_dq_cnt++;
	vout->frame_count++;
	vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
				      vout->next_rdy_ipu_buf,
				      vout->v4l2_bufs[index].m.offset) < 0) {
		dev_err(vout->video_dev->dev,
			"unable to update buffer %d address\n",
			vout->next_rdy_ipu_buf);
		goto exit0;
	}
	if (ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
			      vout->next_rdy_ipu_buf) < 0) {
		dev_err(vout->video_dev->dev,
			"unable to set IPU buffer ready\n");
	}
	vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;

	/* Setup timer for next buffer */
	index = peek_next_buf(&vout->ready_q);
	if (index != -1) {
		/* if timestamp is 0, then default to 30fps */
		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
			timeout =
			    vout->start_jiffies + vout->frame_count * HZ / 30;
		else
			timeout =
			    get_jiffies(&vout->v4l2_bufs[index].timestamp);

		if (jiffies >= timeout) {
			dev_dbg(vout->video_dev->dev,
				"warning: timer timeout already expired.\n");
		}
		if (mod_timer(&vout->output_timer, timeout))
			dev_dbg(vout->video_dev->dev,
				"warning: timer was already set\n");

		dev_dbg(vout->video_dev->dev,
			"timer handler next schedule: %lu\n", timeout);
	} else {
		vout->state = STATE_STREAM_PAUSED;
	}

      exit0:
	spin_unlock_irqrestore(&g_lock, lock_flags);
}

static irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
{
	int last_buf;
	int index;
	unsigned long timeout;
	u32 lock_flags = 0;
	vout_data *vout = dev_id;

	spin_lock_irqsave(&g_lock, lock_flags);

	g_irq_cnt++;

	/* Process previous buffer */
	last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
	if (last_buf != -1) {
		g_buf_output_cnt++;
		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
		queue_buf(&vout->done_q, last_buf);
		vout->ipu_buf[vout->next_done_ipu_buf] = -1;
		wake_up_interruptible(&vout->v4l_bufq);
		/* printk("pp_irq: buf %d done\n", vout->next_done_ipu_buf); */
		vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
	}

	if (vout->state == STATE_STREAM_STOPPING) {
		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
			vout->state = STATE_STREAM_OFF;
		}
	} else if ((vout->state == STATE_STREAM_PAUSED)
		   && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
		/* Setup timer for next buffer, when stream has been paused */
		pr_debug("next index %d\n", index);

		/* if timestamp is 0, then default to 30fps */
		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
			timeout =
			    vout->start_jiffies + vout->frame_count * HZ / 30;
		else
			timeout =
			    get_jiffies(&vout->v4l2_bufs[index].timestamp);

		if (jiffies >= timeout) {
			pr_debug("warning: timer timeout already expired.\n");
		}

		vout->state = STATE_STREAM_ON;

		if (mod_timer(&vout->output_timer, timeout))
			pr_debug("warning: timer was already set\n");

		pr_debug("timer handler next schedule: %lu\n", timeout);
	}

	spin_unlock_irqrestore(&g_lock, lock_flags);

	return IRQ_HANDLED;
}

/*!
 * Start the output stream
 *
 * @param vout      structure vout_data *
 *
 * @return status  0 Success
 */
static int mxc_v4l2out_streamon(vout_data * vout)
{
	struct device *dev = vout->video_dev->dev;
	ipu_channel_params_t params;
	int pp_in_buf[2];
	u16 out_width;
	u16 out_height;
	ipu_channel_t display_input_ch = MEM_PP_MEM;
	bool use_direct_adc = false;

	if (!vout)
		return -EINVAL;

	if (vout->state != STATE_STREAM_OFF)
		return -EBUSY;

	if (queue_size(&vout->ready_q) < 2) {
		dev_err(dev, "2 buffers not been queued yet!\n");
		return -EINVAL;
	}

	out_width = vout->crop_current.width;
	out_height = vout->crop_current.height;

	vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
	vout->ipu_buf[0] = pp_in_buf[0] = dequeue_buf(&vout->ready_q);
	vout->ipu_buf[1] = pp_in_buf[1] = dequeue_buf(&vout->ready_q);
	vout->frame_count = 2;

	ipu_enable_irq(IPU_IRQ_PP_IN_EOF);

	/* Init Display Channel */
#ifdef CONFIG_FB_MXC_ASYNC_PANEL
	if (vout->cur_disp_output != DISP3) {
		int fbnum = vout->output_fb_num[vout->cur_disp_output];
		mxcfb_set_refresh_mode(registered_fb[fbnum], MXCFB_REFRESH_OFF,
				       0);
		if (vout->rotate < IPU_ROTATE_90_RIGHT) {
			dev_dbg(dev, "Using PP direct to ADC channel\n");
			use_direct_adc = true;
			vout->display_ch = MEM_PP_ADC;
			vout->post_proc_ch = MEM_PP_ADC;

			memset(&params, 0, sizeof(params));
			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
			params.mem_pp_adc.in_pixel_fmt =
			    vout->v2f.fmt.pix.pixelformat;
			params.mem_pp_adc.out_width = out_width;
			params.mem_pp_adc.out_height = out_height;
			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
#ifdef CONFIG_FB_MXC_EPSON_PANEL
			params.mem_pp_adc.out_left =
			    2 + vout->crop_current.left;
#else
			params.mem_pp_adc.out_left =
			    12 + vout->crop_current.left;
#endif
			params.mem_pp_adc.out_top = vout->crop_current.top;
			if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
				dev_err(dev, "Error initializing PP chan\n");
				return -EINVAL;
			}

			if (ipu_init_channel_buffer(vout->post_proc_ch,
						    IPU_INPUT_BUFFER,
						    params.mem_pp_adc.
						    in_pixel_fmt,
						    params.mem_pp_adc.in_width,
						    params.mem_pp_adc.in_height,
						    vout->v2f.fmt.pix.
						    bytesperline /
						    bytes_per_pixel(params.
								    mem_pp_adc.
								    in_pixel_fmt),
						    vout->rotate,
						    vout->
						    v4l2_bufs[pp_in_buf[0]].m.
						    offset,
						    vout->
						    v4l2_bufs[pp_in_buf[1]].m.
						    offset,
						    vout->offset.u_offset,
						    vout->offset.v_offset) !=
			    0) {
				dev_err(dev, "Error initializing PP in buf\n");
				return -EINVAL;
			}

			if (ipu_init_channel_buffer(vout->post_proc_ch,
						    IPU_OUTPUT_BUFFER,
						    params.mem_pp_adc.
						    out_pixel_fmt, out_width,
						    out_height, out_width,
						    vout->rotate, 0, 0, 0,
						    0) != 0) {
				dev_err(dev,
					"Error initializing PP output buffer\n");
				return -EINVAL;
			}

		} else {
			dev_dbg(dev, "Using ADC SYS2 channel\n");
			vout->display_ch = ADC_SYS2;
			vout->post_proc_ch = MEM_PP_MEM;
			memset(&params, 0, sizeof(params));
			params.adc_sys2.disp = vout->cur_disp_output;
			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
#ifdef CONFIG_FB_MXC_EPSON_PANEL
			params.adc_sys2.out_left = 2 + vout->crop_current.left;
#else
			params.adc_sys2.out_left = 12 + vout->crop_current.left;
#endif
			params.adc_sys2.out_top = vout->crop_current.top;
			if (ipu_init_channel(ADC_SYS2, &params) < 0)
				return -EINVAL;

			if (ipu_init_channel_buffer(vout->display_ch,
						    IPU_INPUT_BUFFER,
						    SDC_FG_FB_FORMAT,
						    out_width, out_height,
						    out_width, IPU_ROTATE_NONE,
						    vout->display_bufs[0],
						    vout->display_bufs[1], 0,
						    0) != 0) {
				dev_err(dev,
					"Error initializing SDC FG buffer\n");
				return -EINVAL;
			}
		}
	} else
#endif
	{			/* Use SDC */
		dev_dbg(dev, "Using SDC channel\n");
		vout->display_ch = MEM_SDC_FG;
		vout->post_proc_ch = MEM_PP_MEM;
		ipu_init_channel(MEM_SDC_FG, NULL);
		ipu_sdc_set_window_pos(MEM_SDC_FG, vout->crop_current.left,
				       vout->crop_current.top);
		if (ipu_init_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
					    SDC_FG_FB_FORMAT,
					    out_width, out_height, out_width,
					    IPU_ROTATE_NONE,
					    vout->display_bufs[0],
					    vout->display_bufs[1], 0, 0) != 0) {
			dev_err(dev, "Error initializing SDC FG buffer\n");
			return -EINVAL;
		}
	}

	/* Init PP */
	if (use_direct_adc == false) {
		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
			out_width = vout->crop_current.height;
			out_height = vout->crop_current.width;
		}
		memset(&params, 0, sizeof(params));
		params.mem_pp_mem.in_width = vout->v2f.fmt.pix.width;
		params.mem_pp_mem.in_height = vout->v2f.fmt.pix.height;
		params.mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
		params.mem_pp_mem.out_width = out_width;
		params.mem_pp_mem.out_height = out_height;
		params.mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
		if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
			dev_err(dev, "Error initializing PP channel\n");
			return -EINVAL;
		}

		if (ipu_init_channel_buffer(vout->post_proc_ch,
					    IPU_INPUT_BUFFER,
					    params.mem_pp_mem.in_pixel_fmt,
					    params.mem_pp_mem.in_width,
					    params.mem_pp_mem.in_height,
					    vout->v2f.fmt.pix.bytesperline /
					    bytes_per_pixel(params.mem_pp_mem.
							    in_pixel_fmt),
					    IPU_ROTATE_NONE,
					    vout->v4l2_bufs[pp_in_buf[0]].m.
					    offset,
					    vout->v4l2_bufs[pp_in_buf[1]].m.
					    offset, vout->offset.u_offset,
					    vout->offset.v_offset) != 0) {
			dev_err(dev, "Error initializing PP input buffer\n");
			return -EINVAL;
		}

		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
			if (vout->rot_pp_bufs[0]) {
				mxc_free_buffers(vout->rot_pp_bufs,
						 vout->rot_pp_bufs_vaddr, 2,
						 vout->sdc_fg_buf_size);
			}
			if (mxc_allocate_buffers
			    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
			     vout->sdc_fg_buf_size) < 0) {
				return -ENOBUFS;
			}

			if (ipu_init_channel_buffer(vout->post_proc_ch,
						    IPU_OUTPUT_BUFFER,
						    params.mem_pp_mem.
						    out_pixel_fmt, out_width,
						    out_height, out_width,
						    IPU_ROTATE_NONE,
						    vout->rot_pp_bufs[0],
						    vout->rot_pp_bufs[1], 0,
						    0) != 0) {
				dev_err(dev,
					"Error initializing PP output buffer\n");
				return -EINVAL;
			}

			if (ipu_init_channel(MEM_ROT_PP_MEM, NULL) != 0) {
				dev_err(dev,
					"Error initializing PP ROT channel\n");
				return -EINVAL;