TW686x driver

176 lines not on this page · raw message

103 lines not on this page · raw message

776 lines not on this page · raw message

117 lines not on this page · raw message

index aa873c5..f22f485 100644
@@ -1,13 +1,13 @@
 /*
-  Copyright (C) 2015 Industrial Research Institute for Automation
-  and Measurements PIAP
-
-  Written by Krzysztof Hałasa.
-
-  This program is free software; you can redistribute it and/or modify it
-  under the terms of version 2 of the GNU General Public License
-  as published by the Free Software Foundation.
-*/
+ * Copyright (C) 2015 Industrial Research Institute for Automation
+ * and Measurements PIAP
+ *
+ * Written by Krzysztof Hałasa.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
 
 #include <linux/init.h>
 #include <linux/interrupt.h>

@@ -68,15 +68,13 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 		goto disable;
 	}
 
-	if (!request_mem_region(pci_resource_start(pci_dev, 0),
-				pci_resource_len(pci_dev, 0), dev->name)) {
+	err = pci_request_regions(pci_dev, dev->name);
+	if (err < 0) {
 		pr_err("%s: Unable to get MMIO region\n", dev->name);
-		err = -EBUSY;
 		goto disable;
 	}
 
-	dev->mmio = ioremap_nocache(pci_resource_start(pci_dev, 0),
-				    pci_resource_len(pci_dev, 0));
+	dev->mmio = pci_ioremap_bar(pci_dev, 0);
 	if (!dev->mmio) {
 		pr_err("%s: Unable to remap MMIO region\n", dev->name);
 		err = -EIO;

@@ -158,19 +156,8 @@ static struct pci_driver tw686x_pci_driver = {
 	.remove = tw686x_remove,
 };
 
-static int tw686x_init(void)
-{
-	return pci_register_driver(&tw686x_pci_driver);
-}
-
-static void tw686x_exit(void)
-{
-	pci_unregister_driver(&tw686x_pci_driver);
-}
-
 MODULE_DESCRIPTION("Driver for video frame grabber cards based on Intersil/Techwell TW686[4589]");
 MODULE_AUTHOR("Krzysztof Halasa");
 MODULE_LICENSE("GPL v2");
 MODULE_DEVICE_TABLE(pci, tw686x_pci_tbl);
-module_init(tw686x_init);
-module_exit(tw686x_exit);
+module_pci_driver(tw686x_pci_driver);

index f9ac413..33b492b 100644
@@ -16,10 +16,10 @@
 						0xD6, 0xD8, 0xDA, 0xDC})
 #define DMA_PAGE_TABLE1_ADDR	((const u16[8]){0x09, 0xD1, 0xD3, 0xD5,	\
 						0xD7, 0xD9, 0xDB, 0xDD})
-#define DMA_CHANNEL_ENABLE	0x0a
-#define DMA_CONFIG		0x0b
-#define DMA_TIMER_INTERVAL	0x0c
-#define DMA_CHANNEL_TIMEOUT	0x0d
+#define DMA_CHANNEL_ENABLE	0x0A
+#define DMA_CONFIG		0x0B
+#define DMA_TIMER_INTERVAL	0x0C
+#define DMA_CHANNEL_TIMEOUT	0x0D
 #define VDMA_CHANNEL_CONFIG	REG8_1(0x10)
 #define ADMA_P_ADDR		REG8_2(0x18)
 #define ADMA_B_ADDR		REG8_2(0x19)

index bf4f12e..5a1b9ab 100644
@@ -1,13 +1,13 @@
 /*
-  Copyright (C) 2015 Industrial Research Institute for Automation
-  and Measurements PIAP
-
-  Written by Krzysztof Hałasa.
-
-  This program is free software; you can redistribute it and/or modify it
-  under the terms of version 2 of the GNU General Public License
-  as published by the Free Software Foundation.
-*/
+ * Copyright (C) 2015 Industrial Research Institute for Automation
+ * and Measurements PIAP
+ *
+ * Written by Krzysztof Hałasa.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
 
 #include <linux/init.h>
 #include <linux/list.h>

index 6a147d2..8b9d313 100644
@@ -1,13 +1,13 @@
 /*
-  Copyright (C) 2015 Industrial Research Institute for Automation
-  and Measurements PIAP
-
-  Written by Krzysztof Hałasa.
-
-  This program is free software; you can redistribute it and/or modify it
-  under the terms of version 2 of the GNU General Public License
-  as published by the Free Software Foundation.
-*/
+ * Copyright (C) 2015 Industrial Research Institute for Automation
+ * and Measurements PIAP
+ *
+ * Written by Krzysztof Hałasa.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
 
 #include <linux/delay.h>
 #include <linux/freezer.h>

index f22f485..e2dec02 100644
@@ -42,8 +42,9 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 	struct tw686x_dev *dev;
 	int err;
 
-	dev = kzalloc(sizeof(*dev) + (pci_id->driver_data & TYPE_MAX_CHANNELS) *
-		      sizeof(dev->video_channels[0]), GFP_KERNEL);
+	dev = devm_kzalloc(&pci_dev->dev,
+			   sizeof(*dev) + (pci_id->driver_data & TYPE_MAX_CHANNELS) *
+			   sizeof(dev->video_channels[0]), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
 

@@ -55,30 +56,26 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 		(unsigned long)pci_resource_start(pci_dev, 0));
 
 	dev->pci_dev = pci_dev;
-	if (pci_enable_device(pci_dev)) {
-		err = -EIO;
-		goto free_dev;
-	}
+	if (pcim_enable_device(pci_dev))
+		return -EIO;
 
 	pci_set_master(pci_dev);
 
 	if (!pci_dma_supported(pci_dev, DMA_BIT_MASK(32))) {
 		pr_err("%s: 32-bit PCI DMA not supported\n", dev->name);
-		err = -EIO;
-		goto disable;
+		return -EIO;
 	}
 
 	err = pci_request_regions(pci_dev, dev->name);
 	if (err < 0) {
 		pr_err("%s: Unable to get MMIO region\n", dev->name);
-		goto disable;
+		return err;
 	}
 
 	dev->mmio = pci_ioremap_bar(pci_dev, 0);
 	if (!dev->mmio) {
 		pr_err("%s: Unable to remap MMIO region\n", dev->name);
-		err = -EIO;
-		goto free_region;
+		return -EIO;
 	}
 
 	reg_write(dev, SYS_SOFT_RST, 0x0F); /* Reset all subsystems */

@@ -95,32 +92,19 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 
 	spin_lock_init(&dev->irq_lock);
 
-	err = request_irq(pci_dev->irq, tw686x_irq, IRQF_SHARED, dev->name,
-			  dev);
+	err = devm_request_irq(&pci_dev->dev, pci_dev->irq, tw686x_irq,
+			       IRQF_SHARED, dev->name, dev);
 	if (err < 0) {
 		pr_err("%s: Unable to get IRQ\n", dev->name);
-		goto iounmap;
+		return err;
 	}
 
 	err = tw686x_video_init(dev);
 	if (err)
-		goto free_irq;
+		return err;
 
 	pci_set_drvdata(pci_dev, dev);
 	return 0;
-
-free_irq:
-	free_irq(pci_dev->irq, dev);
-iounmap:
-	iounmap(dev->mmio);
-free_region:
-	release_mem_region(pci_resource_start(pci_dev, 0),
-			   pci_resource_len(pci_dev, 0));
-disable:
-	pci_disable_device(pci_dev);
-free_dev:
-	kfree(dev);
-	return err;
 }
 
 static void tw686x_remove(struct pci_dev *pci_dev)

@@ -128,13 +112,6 @@ static void tw686x_remove(struct pci_dev *pci_dev)
 	struct tw686x_dev *dev = pci_get_drvdata(pci_dev);
 
 	tw686x_video_free(dev);
-
-	free_irq(pci_dev->irq, dev);
-	iounmap(dev->mmio);
-	release_mem_region(pci_resource_start(pci_dev, 0),
-			   pci_resource_len(pci_dev, 0));
-	pci_disable_device(pci_dev);
-	kfree(dev);
 }
 
 /* driver_data is number of A/V channels */

index 5a1b9ab..78f4f55 100644
@@ -416,7 +416,7 @@ static int tw686x_querycap(struct file *file, void *priv,
 
 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
-	struct tw686x_video_channel *vc = priv;
+	struct tw686x_video_channel *vc = video_drvdata(file);
 	unsigned std, count = 0;
 	u32 sdt, std_mask = 0;
 

@@ -437,7 +437,7 @@ static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 
 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 {
-	struct tw686x_video_channel *vc = priv;
+	struct tw686x_video_channel *vc = video_drvdata(file);
 
 	*id = vc->video_standard;
 	return 0;

@@ -457,7 +457,7 @@ static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 static int tw686x_g_parm(struct file *file, void *priv,
 			 struct v4l2_streamparm *sp)
 {
-	struct tw686x_video_channel *vc = priv;
+	struct tw686x_video_channel *vc = video_drvdata(file);
 
 	if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;

index f22f485..e2dec02 100644
@@ -42,8 +42,9 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 	struct tw686x_dev *dev;
 	int err;
 
-	dev = kzalloc(sizeof(*dev) + (pci_id->driver_data & TYPE_MAX_CHANNELS) *
-		      sizeof(dev->video_channels[0]), GFP_KERNEL);
+	dev = devm_kzalloc(&pci_dev->dev,
+			   sizeof(*dev) + (pci_id->driver_data & TYPE_MAX_CHANNELS) *
+			   sizeof(dev->video_channels[0]), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
 

@@ -55,30 +56,26 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 		(unsigned long)pci_resource_start(pci_dev, 0));
 
 	dev->pci_dev = pci_dev;
-	if (pci_enable_device(pci_dev)) {
-		err = -EIO;
-		goto free_dev;
-	}
+	if (pcim_enable_device(pci_dev))
+		return -EIO;
 
 	pci_set_master(pci_dev);
 
 	if (!pci_dma_supported(pci_dev, DMA_BIT_MASK(32))) {
 		pr_err("%s: 32-bit PCI DMA not supported\n", dev->name);
-		err = -EIO;
-		goto disable;
+		return -EIO;
 	}
 
 	err = pci_request_regions(pci_dev, dev->name);
 	if (err < 0) {
 		pr_err("%s: Unable to get MMIO region\n", dev->name);
-		goto disable;
+		return err;
 	}
 
 	dev->mmio = pci_ioremap_bar(pci_dev, 0);
 	if (!dev->mmio) {
 		pr_err("%s: Unable to remap MMIO region\n", dev->name);
-		err = -EIO;
-		goto free_region;
+		return -EIO;
 	}
 
 	reg_write(dev, SYS_SOFT_RST, 0x0F); /* Reset all subsystems */

@@ -95,32 +92,19 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 
 	spin_lock_init(&dev->irq_lock);
 
-	err = request_irq(pci_dev->irq, tw686x_irq, IRQF_SHARED, dev->name,
-			  dev);
+	err = devm_request_irq(&pci_dev->dev, pci_dev->irq, tw686x_irq,
+			       IRQF_SHARED, dev->name, dev);
 	if (err < 0) {
 		pr_err("%s: Unable to get IRQ\n", dev->name);
-		goto iounmap;
+		return err;
 	}
 
 	err = tw686x_video_init(dev);
 	if (err)
-		goto free_irq;
+		return err;
 
 	pci_set_drvdata(pci_dev, dev);
 	return 0;
-
-free_irq:
-	free_irq(pci_dev->irq, dev);
-iounmap:
-	iounmap(dev->mmio);
-free_region:
-	release_mem_region(pci_resource_start(pci_dev, 0),
-			   pci_resource_len(pci_dev, 0));
-disable:
-	pci_disable_device(pci_dev);
-free_dev:
-	kfree(dev);
-	return err;
 }
 
 static void tw686x_remove(struct pci_dev *pci_dev)

@@ -128,13 +112,6 @@ static void tw686x_remove(struct pci_dev *pci_dev)
 	struct tw686x_dev *dev = pci_get_drvdata(pci_dev);
 
 	tw686x_video_free(dev);
-
-	free_irq(pci_dev->irq, dev);
-	iounmap(dev->mmio);
-	release_mem_region(pci_resource_start(pci_dev, 0),
-			   pci_resource_len(pci_dev, 0));
-	pci_disable_device(pci_dev);
-	kfree(dev);
 }
 
 /* driver_data is number of A/V channels */

index 5a1b9ab..78f4f55 100644
@@ -416,7 +416,7 @@ static int tw686x_querycap(struct file *file, void *priv,
 
 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
-	struct tw686x_video_channel *vc = priv;
+	struct tw686x_video_channel *vc = video_drvdata(file);
 	unsigned std, count = 0;
 	u32 sdt, std_mask = 0;
 

@@ -437,7 +437,7 @@ static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 
 static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)
 {
-	struct tw686x_video_channel *vc = priv;
+	struct tw686x_video_channel *vc = video_drvdata(file);
 
 	*id = vc->video_standard;
 	return 0;

@@ -457,7 +457,7 @@ static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,
 static int tw686x_g_parm(struct file *file, void *priv,
 			 struct v4l2_streamparm *sp)
 {
-	struct tw686x_video_channel *vc = priv;
+	struct tw686x_video_channel *vc = video_drvdata(file);
 
 	if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;

index 78f4f55..c5d8f28 100644
@@ -417,18 +417,14 @@ static int tw686x_querycap(struct file *file, void *priv,
 static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
 	struct tw686x_video_channel *vc = video_drvdata(file);
-	unsigned std, count = 0;
-	u32 sdt, std_mask = 0;
-
-	for (std = 0; std > ARRAY_SIZE(video_standards); std++)
-		if (id & video_standards[std]) {
-			sdt = std;
-			std_mask |= 1 << std;
-			count++;
-		}
+	unsigned cnt;
+	u32 sdt = 0; /* default */
 
-	if (count != 1)
-		return -EINVAL; /* must request exactly one standard */
+	for (cnt = 0; cnt < ARRAY_SIZE(video_standards); cnt++)
+		if (id & video_standards[cnt]) {
+			sdt = cnt;
+			break;
+		}
 
 	reg_write(vc->dev, SDT[vc->ch], sdt);
 	vc->video_standard = video_standards[sdt];

@@ -658,12 +654,13 @@ int tw686x_video_init(struct tw686x_dev *dev)
 		vc->dev = dev;
 		vc->ch = ch;
 
-		/* default settings */
+		/* default settings: NTSC */
 		vc->format = &formats[0];
-		vc->video_standard = V4L2_STD_PAL;
-		vc->field = V4L2_FIELD_SEQ_TB;
+		vc->video_standard = V4L2_STD_NTSC;
+		reg_write(vc->dev, SDT[vc->ch], 0);
+		vc->field = V4L2_FIELD_SEQ_BT;
 		vc->width = 704;
-		vc->height = 576;
+		vc->height = 480;
 
 		for (n = 0; n < 2; n++) {
 			void *cpu;

@@ -733,8 +730,7 @@ int tw686x_video_init(struct tw686x_dev *dev)
 		vdev->release = video_device_release;
 		vdev->v4l2_dev = &dev->v4l2_dev;
 		vdev->queue = &vc->vidq;
-		vdev->tvnorms = V4L2_STD_PAL | V4L2_STD_NTSC | V4L2_STD_SECAM |
-			V4L2_STD_PAL_60;
+		vdev->tvnorms = V4L2_STD_ALL;
 		vdev->minor = -1;
 		vdev->lock = &vc->vb_mutex;
 

index c5d8f28..c781b3c 100644
@@ -379,6 +379,7 @@ static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.height = height;
 	f->fmt.pix.bytesperline = f->fmt.pix.width * format->depth / 8;
 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }

index c781b3c..21efa30 100644
@@ -466,6 +466,34 @@ static int tw686x_g_parm(struct file *file, void *priv,
 	return 0;
 }
 
+static int tw686x_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *inp)
+{
+	/* the chip has internal multiplexer, support can be added
+	   if the actual hw uses it */
+	if (inp->index)
+		return -EINVAL;
+
+	snprintf(inp->name, sizeof(inp->name), "Composite");
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+  	inp->std = V4L2_STD_ALL;
+	inp->capabilities = V4L2_IN_CAP_STD;
+	return 0;
+}
+
+static int tw686x_g_input(struct file *file, void *priv, unsigned *v)
+{
+	*v = 0;
+	return 0;
+}
+
+static int tw686x_s_input(struct file *file, void *priv, unsigned v)
+{
+	if (v)
+		return -EINVAL;
+	return 0;
+}
+
 const struct v4l2_file_operations tw686x_video_fops = {
 	.owner		= THIS_MODULE,
 	.open		= v4l2_fh_open,

@@ -492,6 +520,9 @@ const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {
 	.vidioc_g_std			= tw686x_g_std,
 	.vidioc_s_std			= tw686x_s_std,
 	.vidioc_g_parm			= tw686x_g_parm,
+	.vidioc_enum_input		= tw686x_enum_input,
+	.vidioc_g_input			= tw686x_g_input,
+	.vidioc_s_input			= tw686x_s_input,
 	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
 };

index e2dec02..a0e1d2d 100644
@@ -61,7 +61,7 @@ static int tw686x_probe(struct pci_dev *pci_dev,
 
 	pci_set_master(pci_dev);
 
-	if (!pci_dma_supported(pci_dev, DMA_BIT_MASK(32))) {
+	if (pci_set_dma_mask(pci_dev, DMA_BIT_MASK(32))) {
 		pr_err("%s: 32-bit PCI DMA not supported\n", dev->name);
 		return -EIO;
 	}

index 21efa30..12cc108 100644
@@ -84,8 +84,7 @@ static const struct tw686x_format *format_by_fourcc(unsigned fourcc)
 
 /* video queue operations */
 
-static int tw686x_queue_setup(struct vb2_queue *vq,
-			      const struct v4l2_format *v4l_fmt,
+static int tw686x_queue_setup(struct vb2_queue *vq, const void *arg,
 			      unsigned int *nbuffers, unsigned int *nplanes,
 			      unsigned int sizes[], void *alloc_ctxs[])
 {

@@ -103,9 +102,10 @@ static int tw686x_queue_setup(struct vb2_queue *vq,
 static void tw686x_buf_queue(struct vb2_buffer *vb)
 {
 	struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 	struct tw686x_vb2_buf *buf;
 
-	buf = container_of(vb, struct tw686x_vb2_buf, vb);
+	buf = container_of(vbuf, struct tw686x_vb2_buf, vb);
 
 	spin_lock(&vc->qlock);
 	list_add_tail(&buf->list, &vc->vidq_queued);

@@ -128,13 +128,13 @@ loop:
 		list_del(&buf->list);
 
 		buf_len = vc->width * vc->height * vc->format->depth / 8;
-		if (vb2_plane_size(&buf->vb, 0) < buf_len) {
+		if (vb2_plane_size(&buf->vb.vb2_buf, 0) < buf_len) {
 			pr_err("Video buffer size too small\n");
-			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 			goto loop; /* try another */
 		}
 
-		vbuf = vb2_dma_sg_plane_desc(&buf->vb, 0);
+		vbuf = vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0);
 		for_each_sg(vbuf->sgl, sg, vbuf->nents, i) {
 			dma_addr_t phys = sg_dma_address(sg);
 			unsigned len = sg_dma_len(sg);

@@ -145,7 +145,7 @@ loop:
 				entry_len = min(entry_len, buf_len);
 				if (count == MAX_SG_DESC_COUNT) {
 					pr_err("Video buffer size too fragmented\n");
-					vb2_buffer_done(&buf->vb,
+					vb2_buffer_done(&buf->vb.vb2_buf,
 							VB2_BUF_STATE_ERROR);
 					goto loop;
 				}

@@ -167,7 +167,7 @@ loop:
 			descs[count++].flags_length = 0; /* unavailable */
 		}
 
-		buf->vb.state = VB2_BUF_STATE_ACTIVE;
+		buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
 		vc->curr_bufs[n] = buf;
 		return;
 	}

@@ -265,12 +265,12 @@ static void tw686x_stop_streaming(struct vb2_queue *vq)
 		buf = list_entry(vc->vidq_queued.next, struct tw686x_vb2_buf,
 				 list);
 		list_del(&buf->list);
-		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 	}
 
 	for (n = 0; n < 2; n++)
 		if (vc->curr_bufs[n])
-			vb2_buffer_done(&vc->curr_bufs[n]->vb,
+			vb2_buffer_done(&vc->curr_bufs[n]->vb.vb2_buf,
 					VB2_BUF_STATE_ERROR);
 
 	spin_unlock(&vc->qlock);

@@ -581,13 +581,13 @@ static int video_thread(void *arg)
 				spin_lock(&vc->qlock);
 				n = !!(reg_read(dev, PB_STATUS) & (1 << ch));
 				if (vc->curr_bufs[n]) {
-					struct vb2_buffer *vb;
+					struct vb2_v4l2_buffer *vb;
 
 					vb = &vc->curr_bufs[n]->vb;
-					v4l2_get_timestamp(&vb->v4l2_buf.timestamp);
-					vb->v4l2_buf.field = vc->field;
-					vb2_set_plane_payload(vb, 0, vc->width * vc->height * vc->format->depth / 8);
-					vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+					v4l2_get_timestamp(&vb->timestamp);
+					vb->field = vc->field;
+					vb2_set_plane_payload(&vb->vb2_buf, 0, vc->width * vc->height * vc->format->depth / 8);
+					vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
 				}
 				setup_descs(vc, n);
 				spin_unlock(&vc->qlock);

index 8b9d313..a7f1d18 100644
@@ -44,7 +44,7 @@ struct vdma_desc {
 };
 
 struct tw686x_vb2_buf {
-	struct vb2_buffer vb;
+	struct vb2_v4l2_buffer vb;
 	struct list_head list;
 };
 

index 12cc108..cfc15e7 100644
@@ -82,6 +82,50 @@ static const struct tw686x_format *format_by_fourcc(unsigned fourcc)
 	return NULL;
 }
 
+static void tw686x_get_format(struct tw686x_video_channel *vc,
+			      struct v4l2_format *f)
+{
+	const struct tw686x_format *format;
+	unsigned width, height, height_div = 1;
+
+	format = format_by_fourcc(f->fmt.pix.pixelformat);
+	if (!format) {
+		format = &formats[0];
+		f->fmt.pix.pixelformat = format->fourcc;
+	}
+
+	width = 704;
+	if (f->fmt.pix.width < width * 3 / 4 /* halfway */)
+		width /= 2;
+
+	height = (vc->video_standard & V4L2_STD_625_50) ? 576 : 480;
+	if (f->fmt.pix.height < height * 3 / 4 /* halfway */)
+		height_div = 2;
+
+	switch (f->fmt.pix.field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		height_div = 2;
+		break;
+	case V4L2_FIELD_SEQ_BT:
+		if (height_div > 1)
+			f->fmt.pix.field = V4L2_FIELD_BOTTOM;
+		break;
+	default:
+		if (height_div > 1)
+			f->fmt.pix.field = V4L2_FIELD_TOP;
+		else
+			f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
+	}
+	height /= height_div;
+
+	f->fmt.pix.width = width;
+	f->fmt.pix.height = height;
+	f->fmt.pix.bytesperline = f->fmt.pix.width * format->depth / 8;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+}
+
 /* video queue operations */
 
 static int tw686x_queue_setup(struct vb2_queue *vq, const void *arg,

@@ -90,12 +134,17 @@ static int tw686x_queue_setup(struct vb2_queue *vq, const void *arg,
 {
 	struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);
 
-	sizes[0] = vc->width * vc->height * vc->format->depth / 8;
+	if (arg) {
+		struct v4l2_format fmt = *(const struct v4l2_format*)arg;
+		tw686x_get_format(vc, &fmt);
+		sizes[0] = fmt.fmt.pix.sizeimage;
+	} else
+		sizes[0] = vc->width * vc->height * vc->format->depth / 8;
+
 	alloc_ctxs[0] = vc->alloc_ctx;
 	*nplanes = 1;		/* packed formats only */
 	if (*nbuffers < 2)
 		*nbuffers = 2;
-
 	return 0;
 }
 

@@ -340,47 +389,7 @@ static int tw686x_g_fmt_vid_cap(struct file *file, void *priv,
 static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
-	struct tw686x_video_channel *vc = video_drvdata(file);
-	const struct tw686x_format *format;
-	unsigned width, height, height_div = 1;
-
-	format = format_by_fourcc(f->fmt.pix.pixelformat);
-	if (!format) {
-		format = &formats[0];
-		f->fmt.pix.pixelformat = format->fourcc;
-	}
-
-	width = 704;
-	if (f->fmt.pix.width < width * 3 / 4 /* halfway */)
-		width /= 2;
-
-	height = (vc->video_standard & V4L2_STD_625_50) ? 576 : 480;
-	if (f->fmt.pix.height < height * 3 / 4 /* halfway */)
-		height_div = 2;
-
-	switch (f->fmt.pix.field) {
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-		height_div = 2;
-		break;
-	case V4L2_FIELD_SEQ_BT:
-		if (height_div > 1)
-			f->fmt.pix.field = V4L2_FIELD_BOTTOM;
-		break;
-	default:
-		if (height_div > 1)
-			f->fmt.pix.field = V4L2_FIELD_TOP;
-		else
-			f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
-	}
-	height /= height_div;
-
-	f->fmt.pix.width = width;
-	f->fmt.pix.height = height;
-	f->fmt.pix.bytesperline = f->fmt.pix.width * format->depth / 8;
-	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
-	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-
+	tw686x_get_format(video_drvdata(file), f);
 	return 0;
 }
 

@@ -388,12 +397,8 @@ static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	struct tw686x_video_channel *vc = video_drvdata(file);
-	int err;
-
-	err = tw686x_try_fmt_vid_cap(file, priv, f);
-	if (err)
-		return err;
 
+	tw686x_get_format(vc, f);
 	vc->format = format_by_fourcc(f->fmt.pix.pixelformat);
 	vc->field = f->fmt.pix.field;
 	vc->width = f->fmt.pix.width;

index cfc15e7..d09a4b0 100644
@@ -280,6 +280,7 @@ static int tw686x_start_streaming(struct vb2_queue *vq, unsigned int count)
 	spin_unlock(&vc->qlock);
 
 	dev->video_active |= 1 << vc->ch;
+	vc->seq = 0;
 	dma_ch_mask = reg_read(dev, DMA_CHANNEL_ENABLE) | (1 << vc->ch);
 	reg_write(dev, DMA_CHANNEL_ENABLE, dma_ch_mask);
 	reg_write(dev, DMA_CMD, (1 << 31) | dma_ch_mask);

@@ -591,6 +592,10 @@ static int video_thread(void *arg)
 					vb = &vc->curr_bufs[n]->vb;
 					v4l2_get_timestamp(&vb->timestamp);
 					vb->field = vc->field;
+					if (V4L2_FIELD_HAS_BOTH(vc->field))
+						vb->sequence = vc->seq++;
+					else
+						vb->sequence = (vc->seq++) / 2;
 					vb2_set_plane_payload(&vb->vb2_buf, 0, vc->width * vc->height * vc->format->depth / 8);
 					vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
 				}

index a7f1d18..540b0ad 100644
@@ -66,6 +66,7 @@ struct tw686x_video_channel {
 	v4l2_std_id video_standard;
 	unsigned width, height;
 	enum v4l2_field field; /* supported TOP, BOTTOM, SEQ_TB and SEQ_BT */
+	unsigned seq;	       /* video field or frame counter */
 	unsigned ch;
 };
 

index d09a4b0..bb77c1b 100644
@@ -551,7 +551,7 @@ static int video_thread(void *arg)
 		for (ch = 0; ch < max_channels(dev); ch++) {
 			struct tw686x_video_channel *vc;
 			unsigned long flags;
-			u32 request, n;
+			u32 request, n, stat = VB2_BUF_STATE_DONE;
 
 			vc = &dev->video_channels[ch];
 			if (!(dev->video_active & (1 << ch)))

@@ -581,28 +581,29 @@ static int video_thread(void *arg)
 				reg_write(dev, DMA_CMD, reg & ~(1 << ch));
 				reg_write(dev, DMA_CMD, reg);
 				spin_unlock_irqrestore(&dev->irq_lock, flags);
-			} else {
-				/* handle video stream */
-				mutex_lock(&vc->vb_mutex);
-				spin_lock(&vc->qlock);
-				n = !!(reg_read(dev, PB_STATUS) & (1 << ch));
-				if (vc->curr_bufs[n]) {
-					struct vb2_v4l2_buffer *vb;
-
-					vb = &vc->curr_bufs[n]->vb;
-					v4l2_get_timestamp(&vb->timestamp);
-					vb->field = vc->field;
-					if (V4L2_FIELD_HAS_BOTH(vc->field))
-						vb->sequence = vc->seq++;
-					else
-						vb->sequence = (vc->seq++) / 2;
-					vb2_set_plane_payload(&vb->vb2_buf, 0, vc->width * vc->height * vc->format->depth / 8);
-					vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
-				}
-				setup_descs(vc, n);
-				spin_unlock(&vc->qlock);
-				mutex_unlock(&vc->vb_mutex);
+				stat = VB2_BUF_STATE_ERROR;
+			}
+
+			/* handle video stream */
+			mutex_lock(&vc->vb_mutex);
+			spin_lock(&vc->qlock);
+			n = !!(reg_read(dev, PB_STATUS) & (1 << ch));
+			if (vc->curr_bufs[n]) {
+				struct vb2_v4l2_buffer *vb;
+
+				vb = &vc->curr_bufs[n]->vb;
+				v4l2_get_timestamp(&vb->timestamp);
+				vb->field = vc->field;
+				if (V4L2_FIELD_HAS_BOTH(vc->field))
+					vb->sequence = vc->seq++;
+				else
+					vb->sequence = (vc->seq++) / 2;
+				vb2_set_plane_payload(&vb->vb2_buf, 0, vc->width * vc->height * vc->format->depth / 8);
+				vb2_buffer_done(&vb->vb2_buf, stat);
 			}
+			setup_descs(vc, n);
+			spin_unlock(&vc->qlock);
+			mutex_unlock(&vc->vb_mutex);
 		}
 		try_to_freeze();
 	}