vivi.c

V4l driver for DVB HD

	dprintk(1,"%s dma_q=0x%08lx\n",__FUNCTION__,(unsigned long)dma_q);

	add_wait_queue(&dma_q->wq, &wait);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
	if (!(q->rmmod || signal_pending(current))) {
#else
	if (!kthread_should_stop()) {
#endif
		dma_q->frame++;

		/* Calculate time to wake up */
		timeout=dma_q->ini_jiffies+msecs_to_jiffies((dma_q->frame*WAKE_NUMERATOR*1000)/WAKE_DENOMINATOR)-jiffies;

		if (timeout <= 0) {
			int old=dma_q->frame;
			dma_q->frame=(jiffies_to_msecs(jiffies-dma_q->ini_jiffies)*WAKE_DENOMINATOR)/(WAKE_NUMERATOR*1000)+1;

			timeout=dma_q->ini_jiffies+msecs_to_jiffies((dma_q->frame*WAKE_NUMERATOR*1000)/WAKE_DENOMINATOR)-jiffies;

			dprintk(1,"underrun, losed %d frames. "
				  "Now, frame is %d. Waking on %d jiffies\n",
					dma_q->frame-old,dma_q->frame,timeout);
		} else
			dprintk(1,"will sleep for %i jiffies\n",timeout);

		vivi_thread_tick(dma_q);

#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,13)
		schedule_timeout_interruptible (timeout);
#else
		set_current_state(TASK_INTERRUPTIBLE);
		schedule_timeout(timeout);
#endif
	}
#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,12)) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
	if (current->flags & PF_FREEZE) {
		refrigerator (PF_FREEZE);
	}
#endif

	remove_wait_queue(&dma_q->wq, &wait);
#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,12)
	try_to_freeze();
#endif
}

int vivi_thread(void *data)
{
	struct vivi_dmaqueue  *dma_q=data;

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
	daemonize();
	exit_files(current);
	reparent_to_init();

	spin_lock_irq(SIGMASK_LOCK(current));
	sigfillset(&current->blocked);
	spin_unlock_irq(SIGMASK_LOCK(current));
	strcpy(current->comm, "vivi");

	dma_q->kthread = current;
	if (dma_q->notify != NULL)
		up(dma_q->notify);
#endif
	dprintk(1,"thread started\n");

	for (;;) {
		vivi_sleep(dma_q);

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
		if (dma_q->rmmod || signal_pending(current))
#else
		if (kthread_should_stop())
#endif
			break;
	}
	dprintk(1, "thread: exit\n");
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
	dma_q->kthread = NULL;

	if (dma_q->notify != NULL)
		up(dma_q->notify);
#endif
	return 0;
}

int vivi_start_thread(struct vivi_dmaqueue  *dma_q)
{
	dma_q->frame=0;
	dma_q->ini_jiffies=jiffies;

	dprintk(1,"%s\n",__FUNCTION__);
	init_waitqueue_head(&dma_q->wq);

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
	dma_q->kthread = kthread_run(vivi_thread, dma_q, "vivi");

	if (dma_q->kthread == NULL) {
		printk(KERN_ERR "vivi: kernel_thread() failed\n");
		return -EINVAL;
	}
#else
	DECLARE_MUTEX_LOCKED(sem);

	dma_q->kthread = NULL;
	dma_q->notify = &sem;
	kernel_thread(vivi_thread, dma_q, 0);
	down(&sem);
	dma_q->notify = NULL;
#endif
	dprintk(1,"returning from %s\n",__FUNCTION__);
	return 0;
}

void vivi_stop_thread(struct vivi_dmaqueue  *dma_q)
{
	dprintk(1,"%s\n",__FUNCTION__);
	/* shutdown control thread */
	if (dma_q->kthread) {
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
		DECLARE_MUTEX_LOCKED(sem);
#endif
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
		/* shutdown control thread */
		dma_q->notify = &sem;
		dma_q->rmmod = 1;
		wake_up_interruptible(&dma_q->wq);
		down(&sem);
		dma_q->notify = NULL;
#else
		kthread_stop(dma_q->kthread);
#endif
		dma_q->kthread=NULL;
	}
}

static int restart_video_queue(struct vivi_dmaqueue *dma_q)
{
	struct vivi_buffer *buf, *prev;
	struct list_head *item;

	dprintk(1,"%s dma_q=0x%08lx\n",__FUNCTION__,(unsigned long)dma_q);

	if (!list_empty(&dma_q->active)) {
		buf = list_entry(dma_q->active.next, struct vivi_buffer, vb.queue);
		dprintk(2,"restart_queue [%p/%d]: restart dma\n",
			buf, buf->vb.i);

		dprintk(1,"Restarting video dma\n");
		vivi_stop_thread(dma_q);
//		vivi_start_thread(dma_q);

		/* cancel all outstanding capture / vbi requests */
		list_for_each(item,&dma_q->active) {
			buf = list_entry(item, struct vivi_buffer, vb.queue);

			list_del(&buf->vb.queue);
			buf->vb.state = STATE_ERROR;
			wake_up(&buf->vb.done);
		}
		mod_timer(&dma_q->timeout, jiffies+BUFFER_TIMEOUT);

		return 0;
	}

	prev = NULL;
	for (;;) {
		if (list_empty(&dma_q->queued))
			return 0;
		buf = list_entry(dma_q->queued.next, struct vivi_buffer, vb.queue);
		if (NULL == prev) {
			list_del(&buf->vb.queue);
			list_add_tail(&buf->vb.queue,&dma_q->active);

			dprintk(1,"Restarting video dma\n");
			vivi_stop_thread(dma_q);
			vivi_start_thread(dma_q);

			buf->vb.state = STATE_ACTIVE;
			mod_timer(&dma_q->timeout, jiffies+BUFFER_TIMEOUT);
			dprintk(2,"[%p/%d] restart_queue - first active\n",
				buf,buf->vb.i);

		} else if (prev->vb.width  == buf->vb.width  &&
			   prev->vb.height == buf->vb.height &&
			   prev->fmt       == buf->fmt) {
			list_del(&buf->vb.queue);
			list_add_tail(&buf->vb.queue,&dma_q->active);
			buf->vb.state = STATE_ACTIVE;
			dprintk(2,"[%p/%d] restart_queue - move to active\n",
				buf,buf->vb.i);
		} else {
			return 0;
		}
		prev = buf;
	}
}

static void vivi_vid_timeout(unsigned long data)
{
	struct vivi_dev      *dev  = (struct vivi_dev*)data;
	struct vivi_dmaqueue *vidq = &dev->vidq;
	struct vivi_buffer   *buf;

	while (!list_empty(&vidq->active)) {
		buf = list_entry(vidq->active.next, struct vivi_buffer, vb.queue);
		list_del(&buf->vb.queue);
		buf->vb.state = STATE_ERROR;
		wake_up(&buf->vb.done);
		printk("vivi/0: [%p/%d] timeout\n", buf, buf->vb.i);
	}

	restart_video_queue(vidq);
}

/* ------------------------------------------------------------------
	Videobuf operations
   ------------------------------------------------------------------*/
static int
buffer_setup(struct videobuf_queue *vq, unsigned int *count, unsigned int *size)
{
	struct vivi_fh *fh = vq->priv_data;

	*size = fh->width*fh->height*2;

	if (0 == *count)
		*count = 32;
	while (*size * *count > vid_limit * 1024 * 1024)
		(*count)--;
	return 0;
}

void
free_buffer(struct videobuf_queue *vq, struct vivi_buffer *buf)
{
	dprintk(1,"%s\n",__FUNCTION__);

	if (in_interrupt())
		BUG();

	/*FIXME: Maybe a spinlock is required here */
	kfree(buf->to_addr);
	buf->to_addr=NULL;

	videobuf_waiton(&buf->vb,0,0);
	videobuf_dma_unmap(vq, &buf->vb.dma);
	videobuf_dma_free(&buf->vb.dma);
	buf->vb.state = STATE_NEEDS_INIT;
}

#define norm_maxw() 1024
#define norm_maxh() 768
static int
buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
						enum v4l2_field field)
{
	struct vivi_fh     *fh  = vq->priv_data;
	struct vivi_buffer *buf = container_of(vb,struct vivi_buffer,vb);
	int rc, init_buffer = 0;

//	dprintk(1,"%s, field=%d\n",__FUNCTION__,field);

	BUG_ON(NULL == fh->fmt);
	if (fh->width  < 48 || fh->width  > norm_maxw() ||
	    fh->height < 32 || fh->height > norm_maxh())
		return -EINVAL;
	buf->vb.size = fh->width*fh->height*2;
	if (0 != buf->vb.baddr  &&  buf->vb.bsize < buf->vb.size)
		return -EINVAL;

	if (buf->fmt       != fh->fmt    ||
	    buf->vb.width  != fh->width  ||
	    buf->vb.height != fh->height ||
	buf->vb.field  != field) {
		buf->fmt       = fh->fmt;
		buf->vb.width  = fh->width;
		buf->vb.height = fh->height;
		buf->vb.field  = field;
		init_buffer = 1;
	}

	if (STATE_NEEDS_INIT == buf->vb.state) {
		if (0 != (rc = videobuf_iolock(vq,&buf->vb,NULL)))
			goto fail;
	}

	buf->vb.state = STATE_PREPARED;

	if (NULL == (buf->to_addr = kmalloc(sizeof(*buf->to_addr) * vb->dma.nr_pages,GFP_KERNEL))) {
		rc=-ENOMEM;
		goto fail;
	}

	return 0;

fail:
	free_buffer(vq,buf);
	return rc;
}

static void
buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
{
	struct vivi_buffer    *buf     = container_of(vb,struct vivi_buffer,vb);
	struct vivi_fh        *fh      = vq->priv_data;
	struct vivi_dev       *dev     = fh->dev;
	struct vivi_dmaqueue  *vidq    = &dev->vidq;
	struct vivi_buffer    *prev;

	if (!list_empty(&vidq->queued)) {
		dprintk(1,"adding vb queue=0x%08lx\n",(unsigned long)&buf->vb.queue);
		list_add_tail(&buf->vb.queue,&vidq->queued);
		buf->vb.state = STATE_QUEUED;
		dprintk(2,"[%p/%d] buffer_queue - append to queued\n",
			buf, buf->vb.i);
	} else if (list_empty(&vidq->active)) {
		list_add_tail(&buf->vb.queue,&vidq->active);

		buf->vb.state = STATE_ACTIVE;
		mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
		dprintk(2,"[%p/%d] buffer_queue - first active\n",
			buf, buf->vb.i);

		vivi_start_thread(vidq);
	} else {
		prev = list_entry(vidq->active.prev, struct vivi_buffer, vb.queue);
		if (prev->vb.width  == buf->vb.width  &&
		    prev->vb.height == buf->vb.height &&
		    prev->fmt       == buf->fmt) {
			list_add_tail(&buf->vb.queue,&vidq->active);
			buf->vb.state = STATE_ACTIVE;
			dprintk(2,"[%p/%d] buffer_queue - append to active\n",
				buf, buf->vb.i);

		} else {
			list_add_tail(&buf->vb.queue,&vidq->queued);
			buf->vb.state = STATE_QUEUED;
			dprintk(2,"[%p/%d] buffer_queue - first queued\n",
				buf, buf->vb.i);
		}
	}
}

static void buffer_release(struct videobuf_queue *vq, struct videobuf_buffer *vb)
{
	struct vivi_buffer   *buf  = container_of(vb,struct vivi_buffer,vb);
	struct vivi_fh       *fh   = vq->priv_data;
	struct vivi_dev      *dev  = (struct vivi_dev*)fh->dev;
	struct vivi_dmaqueue *vidq = &dev->vidq;

	dprintk(1,"%s\n",__FUNCTION__);

	vivi_stop_thread(vidq);

	free_buffer(vq,buf);
}

int vivi_map_sg (void *dev, struct scatterlist *sg, int nents,
	   int direction)
{
	int i;

	dprintk(1,"%s, number of pages=%d\n",__FUNCTION__,nents);
	BUG_ON(direction == DMA_NONE);

	for (i = 0; i < nents; i++ ) {
		BUG_ON(!sg[i].page);

		sg[i].dma_address = page_to_phys(sg[i].page) + sg[i].offset;
	}

	return nents;
}

int vivi_unmap_sg(void *dev,struct scatterlist *sglist,int nr_pages,
					int direction)
{
	dprintk(1,"%s\n",__FUNCTION__);
	return 0;
}

int vivi_dma_sync_sg(void *dev,struct scatterlist *sglist,int nr_pages,
					int direction)
{
//	dprintk(1,"%s\n",__FUNCTION__);

//	flush_write_buffers();
	return 0;
}

static struct videobuf_queue_ops vivi_video_qops = {
	.buf_setup      = buffer_setup,
	.buf_prepare    = buffer_prepare,
	.buf_queue      = buffer_queue,
	.buf_release    = buffer_release,

	/* Non-pci handling routines */
	.vb_map_sg      = vivi_map_sg,
	.vb_dma_sync_sg = vivi_dma_sync_sg,
	.vb_unmap_sg    = vivi_unmap_sg,
};

/* ------------------------------------------------------------------
	IOCTL handling
   ------------------------------------------------------------------*/

static int vivi_try_fmt(struct vivi_dev *dev, struct vivi_fh *fh,
			struct v4l2_format *f)
{
	struct vivi_fmt *fmt;
	enum v4l2_field field;
	unsigned int maxw, maxh;

	if (format.fourcc != f->fmt.pix.pixelformat) {
		dprintk(1,"Fourcc format invalid.\n");
		return -EINVAL;
	}
	fmt=&format;

	field = f->fmt.pix.field;

	if (field == V4L2_FIELD_ANY) {
//		field=V4L2_FIELD_INTERLACED;
		field=V4L2_FIELD_SEQ_TB;
	} else if (V4L2_FIELD_INTERLACED != field) {
		dprintk(1,"Field type invalid.\n");
		return -EINVAL;
	}

	maxw  = norm_maxw();
	maxh  = norm_maxh();

	f->fmt.pix.field = field;
	if (f->fmt.pix.height < 32)
		f->fmt.pix.height = 32;
	if (f->fmt.pix.height > maxh)
		f->fmt.pix.height = maxh;
	if (f->fmt.pix.width < 48)
		f->fmt.pix.width = 48;
	if (f->fmt.pix.width > maxw)
		f->fmt.pix.width = maxw;
	f->fmt.pix.width &= ~0x03;
	f->fmt.pix.bytesperline =
		(f->fmt.pix.width * fmt->depth) >> 3;
	f->fmt.pix.sizeimage =
		f->fmt.pix.height * f->fmt.pix.bytesperline;

	return 0;
}

static int res_get(struct vivi_dev *dev, struct vivi_fh *fh)
{
	/* is it free? */
	down(&dev->lock);
	if (dev->resources) {
		/* no, someone else uses it */
		up(&dev->lock);
		return 0;
	}
	/* it's free, grab it */
	dev->resources =1;
	dprintk(1,"res: get\n");
	up(&dev->lock);
	return 1;
}

static inline int res_locked(struct vivi_dev *dev)
{
	return (dev->resources);
}

static void res_free(struct vivi_dev *dev, struct vivi_fh *fh)
{
	down(&dev->lock);
	dev->resources = 0;
	dprintk(1,"res: put\n");
	up(&dev->lock);
}

static int vivi_do_ioctl(struct inode *inode, struct file *file, unsigned int cmd, void *arg)
{
	struct vivi_fh  *fh     = file->private_data;
	struct vivi_dev *dev    = fh->dev;
	int ret=0;

	if (debug) {
		if (_IOC_DIR(cmd) & _IOC_WRITE)
			v4l_printk_ioctl_arg("vivi(w)",cmd, arg);
		else if (!_IOC_DIR(cmd) & _IOC_READ) {
			v4l_print_ioctl("vivi", cmd);
		}
	}

	switch(cmd) {
	/* --- capabilities ------------------------------------------ */
	case VIDIOC_QUERYCAP:
	{
		struct v4l2_capability *cap = (struct v4l2_capability*)arg;

		memset(cap, 0, sizeof(*cap));

		strcpy(cap->driver, "vivi");
		strcpy(cap->card, "vivi");
		cap->version = VIVI_VERSION;
		cap->capabilities =
					V4L2_CAP_VIDEO_CAPTURE |
					V4L2_CAP_STREAMING     |
					V4L2_CAP_READWRITE;
		break;
	}
	/* --- capture ioctls ---------------------------------------- */
	case VIDIOC_ENUM_FMT:
	{
		struct v4l2_fmtdesc *f = arg;
		enum v4l2_buf_type type;