diff --git a/Makefile b/Makefile
index 242f87a..6ba2837 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-bcwc_pcie-objs := bcwc_ddr.o bcwc_hw.o bcwc_drv.o bcwc_ringbuf.o bcwc_isp.o bcwc_v4l2.o
+bcwc_pcie-objs := bcwc_ddr.o bcwc_hw.o bcwc_drv.o bcwc_ringbuf.o bcwc_isp.o bcwc_v4l2.o bcwc_buffer.o
 obj-m := bcwc_pcie.o
 
 KVERSION := $(shell uname -r)
diff --git a/bcwc_drv.c b/bcwc_drv.c
index c964824..627774b 100644
--- a/bcwc_drv.c
+++ b/bcwc_drv.c
@@ -24,12 +24,15 @@
 #include <linux/io.h>
 #include <linux/interrupt.h>
 #include <linux/workqueue.h>
+#include <linux/spinlock.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
+#include <linux/videodev2.h>
 #include "bcwc_drv.h"
 #include "bcwc_hw.h"
 #include "bcwc_isp.h"
 #include "bcwc_ringbuf.h"
+#include "bcwc_buffer.h"
 #include "bcwc_v4l2.h"
 
 static int bcwc_pci_reserve_mem(struct bcwc_private *dev_priv)
@@ -91,54 +94,112 @@ static int bcwc_pci_reserve_mem(struct bcwc_private *dev_priv)
 
 static void bcwc_irq_disable(struct bcwc_private *dev_priv)
 {
+	//bcwc_hw_irq_disable(dev_priv);
 	free_irq(dev_priv->pdev->irq, dev_priv);
 }
 
+static void sharedmalloc_handler(struct bcwc_private *dev_priv,
+				 struct fw_channel *chan,
+				 struct bcwc_ringbuf_entry *entry)
+{
+	u32 request_size, response_size, address;
+	struct isp_mem_obj *obj, **p;
+
+	request_size = entry->request_size;
+	response_size = entry->response_size;
+	address = entry->address_flags & ~ 3;
+
+	if (address) {
+		pr_debug("Firmware wants to free memory at %08x\n", address);
+		p = dev_priv->s2_mem + address - 64;
+		isp_mem_destroy(*p);
+
+		bcwc_channel_ringbuf_mark_entry_available(dev_priv, chan);
+		bcwc_channel_ringbuf_send(dev_priv, chan, 0, 0, 0);
+	} else {
+		if (!request_size)
+			return;
+		obj = isp_mem_create(dev_priv, FTHD_MEM_SHAREDMALLOC, request_size + 64);
+		if (!obj)
+			return;
+
+		pr_debug("Firmware allocated %d bytes at %08lx (tag %c%c%c%c)\n", request_size, obj->offset,
+			 response_size >> 24,response_size >> 16,
+			 response_size >> 8, response_size);
+		p = dev_priv->s2_mem + obj->offset;
+		*p = obj;
+		bcwc_channel_ringbuf_send(dev_priv, chan, obj->offset + 64, 0, 0);
+	}
+
+}
+
+
+static void terminal_handler(struct bcwc_private *dev_priv,
+				 struct fw_channel *chan,
+				 struct bcwc_ringbuf_entry *entry)
+{
+	u32 request_size, response_size, address;
+
+	request_size = entry->request_size;
+	response_size = entry->response_size;
+	address = entry->address_flags & ~ 3;
+
+	if (!address || !request_size)
+		return;
+
+	pr_info("FWMSG: %.*s", request_size, (char *)(dev_priv->s2_mem + address));
+
+}
+
+static void buf_t2h_handler(struct bcwc_private *dev_priv,
+				 struct fw_channel *chan,
+				 struct bcwc_ringbuf_entry *entry)
+{
+	u32 request_size, response_size, address;
+
+	request_size = entry->request_size;
+	response_size = entry->response_size;
+	address = entry->address_flags & ~ 3;
+
+	if (entry->address_flags & 1)
+		return;
+
+	bcwc_buffer_return_handler(dev_priv, dev_priv->s2_mem + address, request_size);
+	bcwc_channel_ringbuf_send(dev_priv, chan, (response_size & 0x10000000) ? address : 0,
+				  0, 0x80000000);
+}
+
+static void io_t2h_handler(struct bcwc_private *dev_priv,
+				 struct fw_channel *chan,
+				 struct bcwc_ringbuf_entry *entry)
+{
+	bcwc_channel_ringbuf_send(dev_priv, chan, 0, 0, 0);
+}
+
 static void bcwc_handle_irq(struct bcwc_private *dev_priv, struct fw_channel *chan)
 {
-	struct isp_mem_obj *obj;
 	struct bcwc_ringbuf_entry *entry;
-	int request_size;
-	u32 address;
+	int i = 0;
+//	pr_debug("Interrupt from channel source %d, type %d [%s]\n", chan->source, chan->type, chan->name);
 
-	pr_debug("Interrupt from channel source %d, type %d [%s]\n", chan->source, chan->type, chan->name);
-	bcwc_channel_ringbuf_dump(dev_priv, chan);
-	if (chan == dev_priv->channel_io) {
-		pr_debug("command ready\n");
-		wake_up_interruptible(&dev_priv->wq);
-	}
+//	if (strcmp(chan->name, "TERMINAL"))
+//		bcwc_channel_ringbuf_dump(dev_priv, chan);
 
-	while(bcwc_channel_ringbuf_entry_available(dev_priv, chan)) {
+	while(bcwc_channel_ringbuf_entry_available(dev_priv, chan) && i++ < 500) {
 		entry = bcwc_channel_ringbuf_get_entry(dev_priv, chan);
-		if (!entry)
-			goto next;
-
-		request_size = entry->request_size;
-		address = entry->address_flags & ~ 3;
 
 		if (chan == dev_priv->channel_shared_malloc) {
-			if (address) {
-				pr_debug("Firmware wants to free memory at %08x\n", address);
-			} else {
-				if (!request_size)
-					goto next;
-				obj = isp_mem_create(dev_priv, FTHD_MEM_SHAREDMALLOC, request_size);
-				if (!obj)
-					goto next; /* FIXME */
-				pr_debug("Firmware allocated %d bytes at %08lx\n", request_size, obj->offset);
-				bcwc_channel_ringbuf_send(dev_priv, chan, obj->offset, 0, 0);
-			}
+			sharedmalloc_handler(dev_priv, chan, entry);
 		} else if (chan == dev_priv->channel_terminal) {
-			if (!address) {
-				dev_err(&dev_priv->pdev->dev, "%s: no address\n", __FUNCTION__);
-				goto next;
-			}
-			if (!request_size)
-				goto next;
-			pr_debug("FWMSG: %.*s", request_size, (char *)(dev_priv->s2_mem + address));
-		}
-	next:
+			terminal_handler(dev_priv, chan, entry);
 			bcwc_channel_ringbuf_mark_entry_available(dev_priv, chan);
+		} else if (chan == dev_priv->channel_buf_t2h) {
+			buf_t2h_handler(dev_priv, chan, entry);
+		} else if (chan == dev_priv->channel_io) {
+			wake_up_interruptible(&dev_priv->wq);
+		} else if (chan == dev_priv->channel_io_t2h) {
+			io_t2h_handler(dev_priv, chan, entry);
+		}
 	}
 }
 
@@ -150,19 +211,20 @@ static void bcwc_irq_work(struct work_struct *work)
 	u32 pending;
 	int i = 0;
 
-
 	while(i++ < 500) {
+		spin_lock_irq(&dev_priv->io_lock);
 		pending = BCWC_ISP_REG_READ(ISP_REG_41000);
-		pr_debug("interrupt: %08x\n", pending);
+		spin_unlock_irq(&dev_priv->io_lock);
 
 		if (!(pending & 0xf0))
 			break;
 
 		pci_write_config_dword(dev_priv->pdev, 0x94, 0);
+		spin_lock_irq(&dev_priv->io_lock);
 		BCWC_ISP_REG_WRITE(pending, ISP_REG_41024);
+		spin_unlock_irq(&dev_priv->io_lock);
 		pci_write_config_dword(dev_priv->pdev, 0x90, 0x200);
 
-		/* FIXME: locking */
 		for(i = 0; i < dev_priv->num_channels; i++) {
 			chan = dev_priv->channels[i];
 
@@ -176,11 +238,8 @@ static void bcwc_irq_work(struct work_struct *work)
 
 	if (i >= 500) {
 		dev_err(&dev_priv->pdev->dev, "irq stuck, disabling\n");
-//		bcwc_hw_irq_disable(dev_priv);
 		bcwc_irq_disable(dev_priv);
-		return;
 	}
-
 	pci_write_config_dword(dev_priv->pdev, 0x94, 0x200);
 }
 
@@ -189,11 +248,11 @@ static irqreturn_t bcwc_irq_handler(int irq, void *arg)
 	struct bcwc_private *dev_priv = arg;
 	u32 pending;
 	unsigned long flags;
-	
+
 	spin_lock_irqsave(&dev_priv->io_lock, flags);
 	pending = BCWC_ISP_REG_READ(ISP_REG_41000);
 	spin_unlock_irqrestore(&dev_priv->io_lock, flags);
-	
+
 	if (!(pending & 0xf0))
 		return IRQ_NONE;
 
@@ -221,7 +280,7 @@ static int bcwc_pci_set_dma_mask(struct bcwc_private *dev_priv,
 {
 	int ret;
 
-	ret = pci_set_dma_mask(dev_priv->pdev, DMA_BIT_MASK(mask));
+	ret = dma_set_mask_and_coherent(&dev_priv->pdev->dev, DMA_BIT_MASK(mask));
 	if (ret) {
 		dev_err(&dev_priv->pdev->dev, "Failed to set %u pci dma mask\n",
 			mask);
@@ -233,6 +292,39 @@ static int bcwc_pci_set_dma_mask(struct bcwc_private *dev_priv,
 	return 0;
 }
 
+static void bcwc_pci_remove(struct pci_dev *pdev)
+{
+	struct bcwc_private *dev_priv;
+
+	dev_priv = pci_get_drvdata(pdev);
+	if (!dev_priv)
+		goto out;
+
+	bcwc_v4l2_unregister(dev_priv);
+	bcwc_isp_cmd_stop(dev_priv);
+	isp_powerdown(dev_priv);
+	bcwc_irq_disable(dev_priv);
+	cancel_work_sync(&dev_priv->irq_work);
+	isp_uninit(dev_priv);
+	bcwc_hw_deinit(dev_priv);
+	bcwc_buffer_exit(dev_priv);
+	pci_disable_msi(pdev);
+
+	if (dev_priv->s2_io)
+		iounmap(dev_priv->s2_io);
+	if (dev_priv->s2_mem)
+		iounmap(dev_priv->s2_mem);
+	if (dev_priv->isp_io)
+		iounmap(dev_priv->isp_io);
+
+	pci_release_region(pdev, BCWC_PCI_S2_IO);
+	pci_release_region(pdev, BCWC_PCI_S2_MEM);
+	pci_release_region(pdev, BCWC_PCI_ISP_IO);
+out:
+	pci_disable_device(pdev);
+}
+
+
 static int bcwc_pci_probe(struct pci_dev *pdev,
 			  const struct pci_device_id *entry)
 {
@@ -248,6 +340,14 @@ static int bcwc_pci_probe(struct pci_dev *pdev,
 		return -ENOMEM;
 	}
 
+	spin_lock_init(&dev_priv->io_lock);
+	spin_lock_init(&dev_priv->rb_lock);
+	mutex_init(&dev_priv->vb2_queue_lock);
+
+	mutex_init(&dev_priv->ioctl_lock);
+	init_waitqueue_head(&dev_priv->wq);
+	INIT_LIST_HEAD(&dev_priv->buffer_queue);
+
 	dev_priv->pdev = pdev;
 
 	ret = pci_enable_device(pdev);
@@ -263,7 +363,7 @@ static int bcwc_pci_probe(struct pci_dev *pdev,
 	ret = pci_enable_msi(pdev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to enable MSI\n");
-		goto fail_enable;
+		goto fail_reserve;
 	}
 
 	INIT_WORK(&dev_priv->irq_work, bcwc_irq_work);
@@ -288,78 +388,67 @@ static int bcwc_pci_probe(struct pci_dev *pdev,
 	dev_priv->ddr_model = 4;
 	dev_priv->ddr_speed = 450;
 
-	if (!(ret = bcwc_hw_init(dev_priv))) {
-		mdelay(100);
-		bcwc_isp_cmd_start(dev_priv);
-		bcwc_isp_cmd_print_enable(dev_priv, 1);
-		bcwc_isp_cmd_set_loadfile(dev_priv);
-		bcwc_isp_cmd_channel_info(dev_priv);
-		bcwc_isp_cmd_channel_camera_config(dev_priv);
-		bcwc_isp_cmd_channel_camera_config_select(dev_priv, 0, 0);
-		bcwc_isp_cmd_channel_crop_set(dev_priv, 0, 0, 0, 1280, 720);
-		bcwc_isp_cmd_channel_output_config_set(dev_priv, 0, 1280, 720);
-		bcwc_isp_cmd_channel_recycle_mode(dev_priv, 0, 1);
-		bcwc_isp_cmd_channel_recycle_start(dev_priv, 0);
-		bcwc_isp_cmd_channel_drc_start(dev_priv, 0);
-		bcwc_isp_cmd_channel_tone_curve_adaptation_start(dev_priv, 0);
-		bcwc_isp_cmd_channel_sif_pixel_format(dev_priv, 0, 1, 1);
-		bcwc_isp_cmd_channel_error_handling_config(dev_priv, 0, 2, 1);
-		bcwc_isp_cmd_channel_streaming_mode(dev_priv, 0, 0);
-		bcwc_isp_cmd_channel_frame_rate_max(dev_priv, 0, 7672);
-		bcwc_isp_cmd_channel_frame_rate_min(dev_priv, 0, 3072);
-		bcwc_isp_cmd_channel_start(dev_priv);
-		return 0;
-	}
+	ret = bcwc_buffer_init(dev_priv);
+	if (ret)
+		goto fail_irq;
 
-	bcwc_hw_deinit(dev_priv);
-	isp_mem_destroy(dev_priv->firmware);
-	pci_release_region(pdev, BCWC_PCI_S2_IO);
-	pci_release_region(pdev, BCWC_PCI_S2_MEM);
-	pci_release_region(pdev, BCWC_PCI_ISP_IO);
+	ret = bcwc_hw_init(dev_priv);
+	if (ret)
+		goto fail_irq;
 
+	mdelay(1000); /* XXX: should not be needed */
+	bcwc_isp_cmd_start(dev_priv);
+	bcwc_isp_cmd_print_enable(dev_priv, 1);
+	bcwc_isp_cmd_set_loadfile(dev_priv);
+	bcwc_isp_cmd_camera_config(dev_priv);
+	bcwc_isp_cmd_channel_info(dev_priv);
+	bcwc_isp_cmd_channel_camera_config(dev_priv);
+	bcwc_isp_cmd_channel_camera_config_select(dev_priv, 0, 0);
+	bcwc_isp_cmd_channel_crop_set(dev_priv, 0, 0, 0, 1024, 768);
+	bcwc_isp_cmd_channel_output_config_set(dev_priv, 0, 1024, 768, 1);
+	bcwc_isp_cmd_channel_recycle_mode(dev_priv, 0, 1);
+	bcwc_isp_cmd_channel_recycle_start(dev_priv, 0);
+	bcwc_isp_cmd_channel_ae_metering_mode_set(dev_priv, 0, 3);
+	bcwc_isp_cmd_channel_drc_start(dev_priv, 0);
+	bcwc_isp_cmd_channel_tone_curve_adaptation_start(dev_priv, 0);
+	bcwc_isp_cmd_channel_ae_speed_set(dev_priv, 0, 60);
+	bcwc_isp_cmd_channel_ae_stability_set(dev_priv, 0, 75);
+	bcwc_isp_cmd_channel_ae_stability_to_stable_set(dev_priv, 0, 8);
+	bcwc_isp_cmd_channel_sif_pixel_format(dev_priv, 0, 1, 1);
+	bcwc_isp_cmd_channel_error_handling_config(dev_priv, 0, 2, 1);
+	bcwc_isp_cmd_channel_face_detection_enable(dev_priv, 0);
+	bcwc_isp_cmd_channel_face_detection_start(dev_priv, 0);
+	bcwc_isp_cmd_channel_frame_rate_max(dev_priv, 0, 7672);
+	bcwc_isp_cmd_channel_frame_rate_min(dev_priv, 0, 3072);
+	bcwc_isp_cmd_channel_temporal_filter_start(dev_priv, 0);
+	bcwc_isp_cmd_channel_motion_history_start(dev_priv, 0);
+	bcwc_isp_cmd_channel_temporal_filter_enable(dev_priv, 0);
+	bcwc_isp_cmd_channel_streaming_mode(dev_priv, 0, 0);
+	bcwc_isp_cmd_channel_brightness_set(dev_priv, 0, 0x80);
+	bcwc_isp_cmd_channel_contrast_set(dev_priv, 0, 0x80);
+
+	ret = bcwc_v4l2_register(dev_priv);
+	if (ret)
+		goto fail_v4l2;
+
+	return 0;
+fail_v4l2:
+	bcwc_pci_remove(pdev);
 fail_irq:
 	bcwc_irq_disable(dev_priv);
 fail_msi:
 	pci_disable_msi(pdev);
 fail_enable:
 	pci_disable_device(pdev);
+fail_reserve:
+	pci_release_region(pdev, BCWC_PCI_S2_IO);
+	pci_release_region(pdev, BCWC_PCI_S2_MEM);
+	pci_release_region(pdev, BCWC_PCI_ISP_IO);
 fail_free:
 	kfree(dev_priv);
 	return ret;
 }
 
-static void bcwc_pci_remove(struct pci_dev *pdev)
-{
-	struct bcwc_private *dev_priv;
-
-	dev_priv = pci_get_drvdata(pdev);
-	if (dev_priv) {
-		bcwc_isp_cmd_stop(dev_priv);
-		isp_powerdown(dev_priv);
-		bcwc_irq_disable(dev_priv);
-		cancel_work_sync(&dev_priv->irq_work);
-		isp_uninit(dev_priv);
-		bcwc_hw_deinit(dev_priv);
-
-		isp_mem_destroy(dev_priv->firmware);
-		bcwc_buffer_exit(dev_priv);
-		pci_disable_msi(pdev);
-
-		if (dev_priv->s2_io)
-			iounmap(dev_priv->s2_io);
-		if (dev_priv->s2_mem)
-			iounmap(dev_priv->s2_mem);
-		if (dev_priv->isp_io)
-			iounmap(dev_priv->isp_io);
-
-		pci_release_region(pdev, BCWC_PCI_S2_IO);
-		pci_release_region(pdev, BCWC_PCI_S2_MEM);
-		pci_release_region(pdev, BCWC_PCI_ISP_IO);
-	}
-
-	pci_disable_device(pdev);
-}
-
 #ifdef CONFIG_PM
 static int bcwc_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 {
diff --git a/bcwc_drv.h b/bcwc_drv.h
index 1b130e2..d230c1a 100644
--- a/bcwc_drv.h
+++ b/bcwc_drv.h
@@ -21,10 +21,15 @@
 #define _PCWC_PCIE_H
 
 #include <linux/pci.h>
+#include <linux/spinlock.h>
 #include <linux/wait.h>
-
+#include <linux/mutex.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/v4l2-device.h>
 #include "bcwc_reg.h"
 #include "bcwc_ringbuf.h"
+#include "bcwc_buffer.h"
+#include "bcwc_v4l2.h"
 
 #define BCWC_PCI_S2_IO  0
 #define BCWC_PCI_S2_MEM 2
@@ -32,6 +37,8 @@
 
 #define MAX(a, b) ((a)>(b)?(a):(b))
 
+#define BCWC_BUFFERS 4
+
 struct bcwc_reg {
 	u32 offset;
 	u32 value;
@@ -50,9 +57,18 @@ struct bcwc_private {
 	struct pci_dev *pdev;
 	unsigned int dma_mask;
 
+	struct v4l2_device v4l2_dev;
+	struct video_device *videodev;
+	struct mutex ioctl_lock;
+	int users;
+	/* lock for synchronizing with irq/workqueue */
+	spinlock_t io_lock;
+	/* lock for ringbuffer synchronization */
+	spinlock_t rb_lock;
+
 	/* waitqueue for signaling command completion */
 	wait_queue_head_t wq;
-	
+
 	/* Mapped PCI resources */
 	void *s2_io;
 	u32 s2_io_len;
@@ -77,7 +93,8 @@ struct bcwc_private {
 
 	/* Root resource for memory management */
 	struct resource *mem;
-
+	/* Resource for managing IO mmu slots */
+	struct resource *iommu;
 	/* ISP memory objects */
 	struct isp_mem_obj *firmware;
 	struct isp_mem_obj *set_file;
@@ -97,6 +114,15 @@ struct bcwc_private {
 
 	/* camera config */
 	int sensor_count;
+
+	const struct bcwc_fmt *fmt;
+
+	struct vb2_queue vb2_queue;
+	struct mutex vb2_queue_lock;
+	struct list_head buffer_queue;
+	struct vb2_alloc_ctx *alloc_ctx;
+	struct h2t_buf_ctx h2t_bufs[BCWC_BUFFERS];
+
 };
 
 #endif
diff --git a/bcwc_hw.c b/bcwc_hw.c
index f7ecccc..99e0b82 100644
--- a/bcwc_hw.c
+++ b/bcwc_hw.c
@@ -43,11 +43,6 @@ static u32 ddr_phy_reg_map[] = {
 	0x038c, 0x0390, 0x0394, 0x03a0, 0x03a4, 0x03a8, 0x03ac,
 };
 
-static int bcwc_hw_set_core_clk(struct bcwc_private *dev_priv)
-{
-	return 0;
-}
-
 static int bcwc_hw_s2_pll_reset(struct bcwc_private *dev_priv)
 {
 	BCWC_S2_REG_WRITE(0x40, S2_PLL_CTRL_2C);
diff --git a/bcwc_isp.c b/bcwc_isp.c
index 9ac71fd..4e7f305 100644
--- a/bcwc_isp.c
+++ b/bcwc_isp.c
@@ -158,6 +158,7 @@ static int isp_load_firmware(struct bcwc_private *dev_priv)
 			"Misaligned firmware memory object (offset: %lu)\n",
 			dev_priv->firmware->offset);
 		isp_mem_destroy(dev_priv->firmware);
+		dev_priv->firmware = NULL;
 		return -EBUSY;
 	}
 
@@ -287,7 +288,7 @@ int bcwc_isp_cmd(struct bcwc_private *dev_priv, enum bcwc_isp_cmds command, void
 
 	if (request_len)
 		memcpy((char *)cmd + sizeof(struct isp_cmd_hdr), buf, request_len);
-	print_hex_dump_bytes("TXCMD ", DUMP_PREFIX_OFFSET, cmd, len);
+
 	entry = bcwc_channel_ringbuf_send(dev_priv, dev_priv->channel_io,
 		request->offset, request_len + 8, (response_len ? *response_len : 0) + 8);
 
@@ -297,7 +298,7 @@ int bcwc_isp_cmd(struct bcwc_private *dev_priv, enum bcwc_isp_cmds command, void
 		goto out;
 	}
 
-	if (!wait_event_interruptible_timeout(dev_priv->wq, (entry->address_flags & 1), 5 * HZ)) {
+	if (!wait_event_interruptible_timeout(dev_priv->wq, (entry->address_flags & 1), HZ)) {
 		dev_err(&dev_priv->pdev->dev, "timeout wait for command %d\n", cmd->opcode);
 		bcwc_channel_ringbuf_dump(dev_priv, dev_priv->channel_io);
 		if (response_len)
@@ -310,7 +311,7 @@ int bcwc_isp_cmd(struct bcwc_private *dev_priv, enum bcwc_isp_cmds command, void
 	if (response_len && *response_len)
 		memcpy(buf, (entry->address_flags & ~3) + dev_priv->s2_mem + sizeof(struct isp_cmd_hdr), *response_len);
 
-	pr_debug("status %04x, request_len %d response len %d address_flags %x", cmd->status,
+	pr_debug("status %04x, request_len %d response len %d address_flags %x\n", cmd->status,
 		entry->request_size, entry->response_size, entry->address_flags);
 
 	ret = 0;
@@ -329,10 +330,21 @@ int bcwc_isp_cmd_start(struct bcwc_private *dev_priv)
 
 int bcwc_isp_cmd_channel_start(struct bcwc_private *dev_priv)
 {
+	struct isp_cmd_channel_start cmd;
 	pr_debug("sending channel start cmd to firmware\n");
-	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_START, NULL, 0, NULL);
+
+	cmd.channel = 0;
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_START, &cmd, sizeof(cmd), NULL);
 }
 
+int bcwc_isp_cmd_channel_stop(struct bcwc_private *dev_priv)
+{
+	struct isp_cmd_channel_stop cmd;
+
+	cmd.channel = 0;
+	pr_debug("sending channel stop cmd to firmware\n");
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_STOP, &cmd, sizeof(cmd), NULL);
+}
 
 int bcwc_isp_cmd_stop(struct bcwc_private *dev_priv)
 {
@@ -357,14 +369,14 @@ int isp_powerdown(struct bcwc_private *dev_priv)
 	BCWC_ISP_REG_WRITE(0xf7fbdff9, 0xc3000);
 	bcwc_isp_cmd_powerdown(dev_priv);
 
-	for (retries = 0; retries < 1000; retries++) {
+	for (retries = 0; retries < 100; retries++) {
 		reg = BCWC_ISP_REG_READ(0xc3000);
 		if (reg == 0x8042006)
 			break;
 		mdelay(10);
 	}
 
-	if (retries >= 1000) {
+	if (retries >= 100) {
 		dev_info(&dev_priv->pdev->dev, "deinit failed!\n");
 		return -EIO;
 	}
@@ -399,6 +411,7 @@ int isp_uninit(struct bcwc_private *dev_priv)
 	BCWC_ISP_REG_WRITE(0xffffffff, 0x41024);
 	isp_free_channel_info(dev_priv);
 	isp_free_set_file(dev_priv);
+	isp_mem_destroy(dev_priv->firmware);
 	kfree(dev_priv->mem);
 	return 0;
 }
@@ -465,11 +478,27 @@ int bcwc_isp_cmd_channel_info(struct bcwc_private *dev_priv)
 	return ret;
 }
 
+int bcwc_isp_cmd_camera_config(struct bcwc_private *dev_priv)
+{
+	struct isp_cmd_config cmd;
+	int ret, len;
+
+	pr_debug("sending camera config\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+
+	len = sizeof(cmd);
+	ret = bcwc_isp_cmd(dev_priv, CISP_CMD_CONFIG_GET, &cmd, sizeof(cmd), &len);
+	if (!ret)
+		print_hex_dump_bytes("CAMINFO ", DUMP_PREFIX_OFFSET, &cmd, sizeof(cmd));
+	return ret;
+}
+
 int bcwc_isp_cmd_channel_camera_config(struct bcwc_private *dev_priv)
 {
 	struct isp_cmd_channel_camera_config cmd;
 	int ret, len, i;
-
+	char prefix[16];
 	pr_debug("sending ch camera config\n");
 
 	memset(&cmd, 0, sizeof(cmd));
@@ -480,7 +509,8 @@ int bcwc_isp_cmd_channel_camera_config(struct bcwc_private *dev_priv)
 		ret = bcwc_isp_cmd(dev_priv, CISP_CMD_CH_CAMERA_CONFIG_GET, &cmd, sizeof(cmd), &len);
 		if (ret)
 			break;
-		print_hex_dump_bytes("CHINFO ", DUMP_PREFIX_OFFSET, &cmd, sizeof(cmd));
+		snprintf(prefix, sizeof(prefix)-1, "CAMCONF%d ", i);
+		print_hex_dump_bytes(prefix, DUMP_PREFIX_OFFSET, &cmd, sizeof(cmd));
 	}
 	return ret;
 }
@@ -517,7 +547,7 @@ int bcwc_isp_cmd_channel_crop_set(struct bcwc_private *dev_priv, int channel,
 	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_CROP_SET, &cmd, sizeof(cmd), &len);
 }
 
-int bcwc_isp_cmd_channel_output_config_set(struct bcwc_private *dev_priv, int channel, int x, int y)
+int bcwc_isp_cmd_channel_output_config_set(struct bcwc_private *dev_priv, int channel, int x, int y, int pixelformat)
 {
 	struct isp_cmd_channel_output_config cmd;
 	int len;
@@ -525,11 +555,19 @@ int bcwc_isp_cmd_channel_output_config_set(struct bcwc_private *dev_priv, int ch
 	pr_debug("output config: [%d, %d]\n", x, y);
 
 	memset(&cmd, 0, sizeof(cmd));
-	//	cmd.channel = channel;
-	cmd.x1 = x;
-	cmd.x2 = x;
+	cmd.channel = channel;
+	cmd.x1 = x; /* Y size */
+	cmd.x2 = x * 2; /* Chroma size? */
 	cmd.x3 = x;
 	cmd.y1 = y;
+
+	/* pixel formats:
+	 * 0 - plane 0 Y plane 1 UV
+	   1 - YUYV
+	   2 - YVYU
+	*/
+	cmd.pixelformat = pixelformat;
+	cmd.unknown3 = 0;
 	cmd.unknown5 = 0x7ff;
 	len = sizeof(cmd);
 	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_OUTPUT_CONFIG_SET, &cmd, sizeof(cmd), &len);
@@ -549,12 +587,25 @@ int bcwc_isp_cmd_channel_recycle_mode(struct bcwc_private *dev_priv, int channel
 	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_BUFFER_RECYCLE_MODE_SET, &cmd, sizeof(cmd), &len);
 }
 
+int bcwc_isp_cmd_channel_buffer_return(struct bcwc_private *dev_priv, int channel)
+{
+	struct isp_cmd_channel_buffer_return cmd;
+	int len;
+
+	pr_debug("buffer return\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_BUFFER_RETURN, &cmd, sizeof(cmd), &len);
+}
+
 int bcwc_isp_cmd_channel_recycle_start(struct bcwc_private *dev_priv, int channel)
 {
 	struct isp_cmd_channel_recycle_mode cmd;
 	int len;
 
-	pr_debug("start recycle");
+	pr_debug("start recycle\n");
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -567,7 +618,7 @@ int bcwc_isp_cmd_channel_drc_start(struct bcwc_private *dev_priv, int channel)
 	struct isp_cmd_channel_drc_start cmd;
 	int len;
 
-	pr_debug("start drc");
+	pr_debug("start drc\n");
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -580,7 +631,7 @@ int bcwc_isp_cmd_channel_tone_curve_adaptation_start(struct bcwc_private *dev_pr
 	struct isp_cmd_channel_tone_curve_adaptation_start cmd;
 	int len;
 
-	pr_debug("start drc");
+	pr_debug("tone curve adaptation start\n");
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -593,7 +644,7 @@ int bcwc_isp_cmd_channel_sif_pixel_format(struct bcwc_private *dev_priv, int cha
 	struct isp_cmd_channel_sif_format_set cmd;
 	int len;
 
-	pr_debug("set pixel format %d, %d", param1, param2);
+	pr_debug("set pixel format %d, %d\n", param1, param2);
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -608,7 +659,7 @@ int bcwc_isp_cmd_channel_error_handling_config(struct bcwc_private *dev_priv, in
 	struct isp_cmd_channel_camera_err_handle_config cmd;
 	int len;
 
-	pr_debug("set error handling config %d, %d", param1, param2);
+	pr_debug("set error handling config %d, %d\n", param1, param2);
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -623,7 +674,7 @@ int bcwc_isp_cmd_channel_streaming_mode(struct bcwc_private *dev_priv, int chann
 	struct isp_cmd_channel_streaming_mode cmd;
 	int len;
 
-	pr_debug("set streaming mode %d", mode);
+	pr_debug("set streaming mode %d\n", mode);
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -637,7 +688,7 @@ int bcwc_isp_cmd_channel_frame_rate_min(struct bcwc_private *dev_priv, int chann
 	struct isp_cmd_channel_frame_rate_set cmd;
 	int len;
 
-	pr_debug("set ae frame rate min %d", rate);
+	pr_debug("set ae frame rate min %d\n", rate);
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -651,7 +702,7 @@ int bcwc_isp_cmd_channel_frame_rate_max(struct bcwc_private *dev_priv, int chann
 	struct isp_cmd_channel_frame_rate_set cmd;
 	int len;
 
-	pr_debug("set ae frame rate max %d", rate);
+	pr_debug("set ae frame rate max %d\n", rate);
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = channel;
@@ -660,6 +711,155 @@ int bcwc_isp_cmd_channel_frame_rate_max(struct bcwc_private *dev_priv, int chann
 	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_AE_FRAME_RATE_MAX_SET, &cmd, sizeof(cmd), &len);
 }
 
+int bcwc_isp_cmd_channel_ae_speed_set(struct bcwc_private *dev_priv, int channel, int speed)
+{
+	struct isp_cmd_channel_ae_speed_set cmd;
+	int len;
+
+	pr_debug("set ae speed %d\n", speed);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	cmd.speed = speed;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_AE_SPEED_SET, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_ae_stability_set(struct bcwc_private *dev_priv, int channel, int stability)
+{
+	struct isp_cmd_channel_ae_stability_set cmd;
+	int len;
+
+	pr_debug("set ae stability %d\n", stability);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	cmd.stability = stability;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_AE_STABILITY_SET, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_ae_stability_to_stable_set(struct bcwc_private *dev_priv, int channel, int value)
+{
+	struct isp_cmd_channel_ae_stability_to_stable_set cmd;
+	int len;
+
+	pr_debug("set ae stability to stable %d\n", value);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	cmd.value = value;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_AE_STABILITY_TO_STABLE_SET, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_face_detection_start(struct bcwc_private *dev_priv, int channel)
+{
+	struct isp_cmd_channel_face_detection_start cmd;
+	int len;
+
+	pr_debug("face detection start\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_FACE_DETECTION_START, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_face_detection_enable(struct bcwc_private *dev_priv, int channel)
+{
+	struct isp_cmd_channel_face_detection_enable cmd;
+	int len;
+
+	pr_debug("face detection enable\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_FACE_DETECTION_ENABLE, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_temporal_filter_start(struct bcwc_private *dev_priv, int channel)
+{
+	struct isp_cmd_channel_temporal_filter_start cmd;
+	int len;
+
+	pr_debug("temporal filter start\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_APPLE_CH_TEMPORAL_FILTER_START, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_temporal_filter_enable(struct bcwc_private *dev_priv, int channel)
+{
+	struct isp_cmd_channel_temporal_filter_enable cmd;
+	int len;
+
+	pr_debug("temporal filter enable\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_APPLE_CH_TEMPORAL_FILTER_ENABLE, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_motion_history_start(struct bcwc_private *dev_priv, int channel)
+{
+	struct isp_cmd_channel_motion_history_start cmd;
+	int len;
+
+	pr_debug("motion history start\n");
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_APPLE_CH_MOTION_HISTORY_START, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_ae_metering_mode_set(struct bcwc_private *dev_priv, int channel, int mode)
+{
+	struct isp_cmd_channel_ae_metering_mode_set cmd;
+	int len;
+
+	pr_debug("ae metering mode %d\n", mode);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	cmd.mode = mode;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_APPLE_CH_AE_METERING_MODE_SET, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_brightness_set(struct bcwc_private *dev_priv, int channel, int brightness)
+{
+	struct isp_cmd_channel_brightness_set cmd;
+	int len;
+
+	pr_debug("set brightness %d\n", brightness);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	cmd.brightness = brightness;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_SCALER_BRIGHTNESS_SET, &cmd, sizeof(cmd), &len);
+}
+
+int bcwc_isp_cmd_channel_contrast_set(struct bcwc_private *dev_priv, int channel, int contrast)
+{
+	struct isp_cmd_channel_contrast_set cmd;
+	int len;
+
+	pr_debug("set contrast %d\n", contrast);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.channel = channel;
+	cmd.contrast = contrast;
+	len = sizeof(cmd);
+	return bcwc_isp_cmd(dev_priv, CISP_CMD_CH_SCALER_CONTRAST_SET, &cmd, sizeof(cmd), &len);
+}
+
 int isp_init(struct bcwc_private *dev_priv)
 {
 	struct isp_mem_obj *ipc_queue, *heap, *fw_args;
diff --git a/bcwc_isp.h b/bcwc_isp.h
index 8408bf2..7399a30 100644
--- a/bcwc_isp.h
+++ b/bcwc_isp.h
@@ -28,6 +28,7 @@
 #define FTHD_MEM_CMD            5
 #define FTHD_MEM_SHAREDMALLOC   6
 #define FTHD_MEM_SET_FILE       7
+#define FTHD_MEM_BUFFER         8
 
 #define FTHD_MEM_SIZE		0x8000000	/* 128mb */
 #define FTHD_MEM_FW_SIZE	0x800000	/* 8mb */
@@ -478,6 +479,17 @@ struct isp_cmd_print_enable {
 	u32 enable;
 } __attribute__((packed));
 
+struct isp_cmd_config {
+	u32 field0;
+	u32 field4;
+	u32 field8;
+	u32 fieldc;
+	u32 field10;
+	u32 field14;
+	u32 field18;
+	u32 field1c;
+} __attribute__((packed));
+
 struct isp_cmd_set_loadfile {
 	u32 unknown;
 	u32 addr;
@@ -507,11 +519,11 @@ struct isp_cmd_channel_set_crop {
 };
 
 struct isp_cmd_channel_output_config {
-	u32 unknown0;
+	u32 channel;
 	u32 x1;
 	u32 y1;
 	u32 unknown3;
-	u32 unknown4;
+	u32 pixelformat;
 	u32 x2;
 	u32 x3;
 	u32 unknown5;
@@ -560,6 +572,68 @@ struct isp_cmd_channel_frame_rate_set {
 	u16 rate;
 };
 
+struct isp_cmd_channel_ae_speed_set {
+	u32 channel;
+	u16 speed;
+};
+
+struct isp_cmd_channel_ae_stability_set {
+	u32 channel;
+	u16 stability;
+};
+
+struct isp_cmd_channel_ae_stability_to_stable_set {
+	u32 channel;
+	u16 value;
+};
+
+struct isp_cmd_channel_face_detection_start {
+	u32 channel;
+};
+
+struct isp_cmd_channel_face_detection_enable {
+	u32 channel;
+};
+
+struct isp_cmd_channel_temporal_filter_start {
+	u32 channel;
+};
+
+struct isp_cmd_channel_temporal_filter_enable {
+	u32 channel;
+};
+
+struct isp_cmd_channel_motion_history_start {
+	u32 channel;
+};
+
+struct isp_cmd_channel_ae_metering_mode_set {
+	u32 channel;
+	u32 mode;
+};
+
+struct isp_cmd_channel_start {
+	u32 channel;
+};
+
+struct isp_cmd_channel_stop {
+	u32 channel;
+};
+
+struct isp_cmd_channel_brightness_set {
+	u32 channel;
+	u32 brightness;
+};
+
+struct isp_cmd_channel_contrast_set {
+	u32 channel;
+	u32 contrast;
+};
+
+struct isp_cmd_channel_buffer_return {
+	u32 channel;
+};
+
 #define to_isp_mem_obj(x) container_of((x), struct isp_mem_obj, base)
 
 extern int isp_init(struct bcwc_private *dev_priv);
@@ -577,10 +651,11 @@ extern int bcwc_isp_cmd_print_enable(struct bcwc_private *dev_priv, int enable);
 extern int bcwc_isp_cmd_set_loadfile(struct bcwc_private *dev_priv);
 extern int bcwc_isp_cmd_channel_info(struct bcwc_private *dev_priv);
 extern int bcwc_isp_cmd_channel_start(struct bcwc_private *dev_priv);
+extern int bcwc_isp_cmd_channel_stop(struct bcwc_private *dev_priv);
 extern int bcwc_isp_cmd_channel_camera_config(struct bcwc_private *dev_priv);
 extern int bcwc_isp_cmd_channel_crop_set(struct bcwc_private *dev_priv, int channel,
 					 int x1, int y1, int x2, int y2);
-extern int bcwc_isp_cmd_channel_output_config_set(struct bcwc_private *dev_priv, int channel, int x, int y);
+extern int bcwc_isp_cmd_channel_output_config_set(struct bcwc_private *dev_priv, int channel, int x, int y, int pixelformat);
 extern int bcwc_isp_cmd_channel_recycle_mode(struct bcwc_private *dev_priv, int channel, int mode);
 extern int bcwc_isp_cmd_channel_recycle_start(struct bcwc_private *dev_priv, int channel);
 extern int bcwc_isp_cmd_channel_camera_config_select(struct bcwc_private *dev_priv, int channel, int config);
@@ -591,4 +666,17 @@ extern int bcwc_isp_cmd_channel_error_handling_config(struct bcwc_private *dev_p
 extern int bcwc_isp_cmd_channel_streaming_mode(struct bcwc_private *dev_priv, int channel, int mode);
 extern int bcwc_isp_cmd_channel_frame_rate_min(struct bcwc_private *dev_priv, int channel, int rate);
 extern int bcwc_isp_cmd_channel_frame_rate_max(struct bcwc_private *dev_priv, int channel, int rate);
+extern int bcwc_isp_cmd_camera_config(struct bcwc_private *dev_priv);
+extern int bcwc_isp_cmd_channel_ae_speed_set(struct bcwc_private *dev_priv, int channel, int speed);
+extern int bcwc_isp_cmd_channel_ae_stability_set(struct bcwc_private *dev_priv, int channel, int stability);
+extern int bcwc_isp_cmd_channel_ae_stability_to_stable_set(struct bcwc_private *dev_priv, int channel, int value);
+extern int bcwc_isp_cmd_channel_face_detection_enable(struct bcwc_private *dev_priv, int channel);
+extern int bcwc_isp_cmd_channel_face_detection_start(struct bcwc_private *dev_priv, int channel);
+extern int bcwc_isp_cmd_channel_temporal_filter_start(struct bcwc_private *dev_priv, int channel);
+extern int bcwc_isp_cmd_channel_temporal_filter_enable(struct bcwc_private *dev_priv, int channel);
+extern int bcwc_isp_cmd_channel_motion_history_start(struct bcwc_private *dev_priv, int channel);
+extern int bcwc_isp_cmd_channel_ae_metering_mode_set(struct bcwc_private *dev_priv, int channel, int mode);
+extern int bcwc_isp_cmd_channel_brightness_set(struct bcwc_private *dev_priv, int channel, int brightness);
+extern int bcwc_isp_cmd_channel_contrast_set(struct bcwc_private *dev_priv, int channel, int contrast);
+extern int bcwc_isp_cmd_channel_buffer_return(struct bcwc_private *dev_priv, int channel);
 #endif
diff --git a/bcwc_ringbuf.c b/bcwc_ringbuf.c
index ac677c9..e0c1a8e 100644
--- a/bcwc_ringbuf.c
+++ b/bcwc_ringbuf.c
@@ -40,19 +40,14 @@ void bcwc_channel_ringbuf_dump(struct bcwc_private *dev_priv, struct fw_channel
 	char pos;
 	int i;
 
-	pr_debug("dumping %d [%s]\n", chan->type, chan->name);
 	for( i = 0; i < chan->size; i++) {
-		if (chan->ringbuf.send_idx == i && chan->ringbuf.recv_idx == i)
+		if (chan->ringbuf.idx == i)
 			pos = '*';
-		else if (chan->ringbuf.send_idx == i)
-			pos = 'S';
-		else if (chan->ringbuf.recv_idx == i)
-			pos = 'R';
 		else
 			pos = ' ';
 	    entry = dev_priv->s2_mem + chan->offset + i * sizeof(struct bcwc_ringbuf_entry);
-	    pr_debug("%c%3.3d: ADDRESS %08x REQUEST_SIZE %08x RESPONSE_SIZE %08x\n", pos, i, entry->address_flags,
-			 entry->request_size, entry->response_size);
+	    pr_debug("%s: %c%3.3d: ADDRESS %08x REQUEST_SIZE %08x RESPONSE_SIZE %08x\n", chan->name,
+		     pos, i, entry->address_flags, entry->request_size, entry->response_size);
 
 	}
 }
@@ -62,22 +57,22 @@ void bcwc_channel_ringbuf_init(struct bcwc_private *dev_priv, struct fw_channel
 	struct bcwc_ringbuf_entry *entry;
 	int i;
 
-	chan->ringbuf.recv_idx = 0;
-	chan->ringbuf.send_idx = 0;
-	chan->ringbuf.sent_cnt = 0;
-	chan->ringbuf.recv_cnt = 0;
+	chan->ringbuf.idx = 0;
 	chan->ringbuf.phys_offset = chan->offset;
 	chan->ringbuf.virt_addr = dev_priv->s2_mem + chan->offset;
 
 	if (chan->type == RINGBUF_TYPE_H2T) {
 		entry = (struct bcwc_ringbuf_entry *)chan->ringbuf.virt_addr;
 		pr_debug("clearing ringbuf %s at %p (size %d)\n", chan->name, entry, chan->size);
+
+		spin_lock_irq(&dev_priv->rb_lock);
 		for(i = 0; i < chan->size; i++) {
 			entry->address_flags = 1;
 			entry->request_size = 0;
 			entry->response_size = 0;
 			entry++;
 		}
+		spin_unlock_irq(&dev_priv->rb_lock);
 	}
 }
 
@@ -85,19 +80,27 @@ struct bcwc_ringbuf_entry *bcwc_channel_ringbuf_send(struct bcwc_private *dev_pr
 			      u32 data_offset, u32 request_size, u32 response_size)
 {
 	struct bcwc_ringbuf_entry *entry;
-	pr_debug("%s: TX pos %d\n", chan->name, chan->ringbuf.send_idx);
-	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.send_idx++);
-	if (chan->ringbuf.send_idx >= chan->size) {
-		chan->ringbuf.send_idx = 0;
-	pr_debug("send entry %p offset %08x\n", entry, data_offset);
+	int pos = chan->ringbuf.idx;
+
+	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.idx++);
+	if (chan->ringbuf.idx >= chan->size) {
+		pr_debug("%s: reset tx pointer\n", chan->name);
+		chan->ringbuf.idx = 0;
+	}
+	pr_debug("%s: send entry %p offset %08x pos %d\n", chan->name, entry, data_offset, pos);
+
+	spin_lock_irq(&dev_priv->rb_lock);
 	entry->request_size = request_size;
 	entry->response_size = response_size;
 	entry->address_flags = data_offset | (chan->type == 0 ? 0 : 1);
+	spin_unlock_irq(&dev_priv->rb_lock);
 
-	pr_debug("address_flags %x, request size %x response size %x\n",
-		 entry->address_flags, entry->request_size, entry->response_size);
+	//	pr_debug("address_flags %x, request size %x response size %x\n",
+	//	 entry->address_flags, entry->request_size, entry->response_size);
 	wmb();
+	spin_lock_irq(&dev_priv->io_lock);
 	BCWC_ISP_REG_WRITE(0x10 << chan->source, ISP_REG_41020);
+	spin_unlock_irq(&dev_priv->io_lock);
 	return entry;
 }
 
@@ -106,9 +109,11 @@ struct bcwc_ringbuf_entry *bcwc_channel_ringbuf_get_entry(struct bcwc_private *d
 {
 	struct bcwc_ringbuf_entry *entry;
 
-	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.recv_idx);
-	if (chan->ringbuf.recv_idx > chan->size)
-		chan->ringbuf.recv_idx = 0;
+	spin_lock_irq(&dev_priv->rb_lock);
+	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.idx);
+	if (chan->ringbuf.idx > chan->size)
+		chan->ringbuf.idx = 0;
+	spin_unlock_irq(&dev_priv->rb_lock);
 	return entry;
 }
 
@@ -117,18 +122,23 @@ void bcwc_channel_ringbuf_mark_entry_available(struct bcwc_private *dev_priv,
 {
 	struct bcwc_ringbuf_entry *entry;
 
-	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.recv_idx++);
+	spin_lock_irq(&dev_priv->rb_lock);
+	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.idx++);
 	entry->address_flags = 1;
 	entry->request_size = 0;
 	entry->response_size = 0;
+	spin_unlock_irq(&dev_priv->rb_lock);
 }
 
 int bcwc_channel_ringbuf_entry_available(struct bcwc_private *dev_priv,
 					 struct fw_channel *chan)
 {
 	struct bcwc_ringbuf_entry *entry;
+	int ret;
 
-	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.recv_idx);	
-	return (!(entry->address_flags & 1));
+	spin_lock_irq(&dev_priv->rb_lock);
+	entry = get_entry_addr(dev_priv, chan, chan->ringbuf.idx);
+	ret = !(entry->address_flags & 1) ^ (chan->type == 0);
+	spin_unlock_irq(&dev_priv->rb_lock);
+	return ret;
 }
-
diff --git a/bcwc_ringbuf.h b/bcwc_ringbuf.h
index 8e7eedf..1d8b719 100644
--- a/bcwc_ringbuf.h
+++ b/bcwc_ringbuf.h
@@ -29,10 +29,7 @@ enum ringbuf_type_t {
 struct bcwc_ringbuf {
 	void *doorbell;
 	u32 phys_offset;
-	int recv_idx;
-	int send_idx;
-	int recv_cnt;
-	int sent_cnt;
+	int idx;
         u8 *virt_addr;
 };
 
diff --git a/bcwc_v4l2.c b/bcwc_v4l2.c
index ded15f3..975868d 100644
--- a/bcwc_v4l2.c
+++ b/bcwc_v4l2.c
@@ -1,4 +1,3 @@
-
 /*
  * Broadcom PCIe 1570 webcam driver
  *
@@ -22,139 +21,294 @@
 #include <linux/spinlock.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
+#include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-ioctl.h>
-#include <media/videobuf-dma-sg.h>
+#include <media/videobuf2-dma-sg.h>
 #include "bcwc_drv.h"
 #include "bcwc_hw.h"
 #include "bcwc_isp.h"
 #include "bcwc_ringbuf.h"
 #include "bcwc_buffer.h"
 
-static int bcwc_vb_buf_setup(struct videobuf_queue *q,
-			     unsigned int *count, unsigned int *size)
-{
-	struct bcwc_private *priv = q->priv_data;
-	pr_debug("%s: %d\n", __FUNCTION__, priv->user_format.sizeimage);
-	*size = priv->user_format.sizeimage;
-	if (*count == 0 || *count > 6)
-		*count = 6;
-	return 0;
-}
-
-static int bcwc_vb_buf_prepare(struct videobuf_queue *q,
-				 struct videobuf_buffer *vb, enum v4l2_field field)
-{
-	struct bcwc_private *priv = q->priv_data;
-	int ret;
-
-	pr_debug("%s\n", __FUNCTION__);
-
-	vb->size = priv->user_format.sizeimage;
-	vb->width = priv->user_format.width;
-	vb->height = priv->user_format.height;
-	vb->field = field;
-
-	if (vb->state == VIDEOBUF_NEEDS_INIT) {
-		ret = videobuf_iolock(q, vb, NULL);
-		if (ret)
-			return ret;
+#define BCWC_FMT(_desc, _x, _y, _sizeimage, _planes, _pixfmt, _range, _x1, _y1, _x2, _y2) \
+	{								\
+		.fmt.width = (_x),					\
+		.fmt.height = (_y),					\
+		.fmt.sizeimage = (_sizeimage),				\
+		.fmt.pixelformat = (_pixfmt),				\
+		.planes = (_planes),					\
+		.desc = (_desc),					\
+		.range = (_range),					\
+		.x1 = (_x1),						\
+		.y1 = (_y1),						\
+		.x2 = (_x2),						\
+		.y2 = (_y2)						\
 	}
-	vb->state = VIDEOBUF_PREPARED;
-	return 0;
-}
 
-static void bcwc_vb_buf_queue(struct videobuf_queue *q,
-				struct videobuf_buffer *vb)
-{
-	struct bcwc_private *priv = q->priv_data;
-	pr_debug("%s\n", __FUNCTION__);
-	vb->state = VIDEOBUF_QUEUED;
-	list_add_tail(&vb->queue, &priv->buffer_queue);
-}
-
-static void bcwc_vb_buf_release(struct videobuf_queue *q,
-				  struct videobuf_buffer *vb)
-{
-	struct bcwc_private *priv = q->priv_data;
-	pr_debug("%s\n", __FUNCTION__);
-	videobuf_dma_unmap(&priv->pdev->dev, videobuf_to_dma(vb));
-	videobuf_dma_free(videobuf_to_dma(vb));
-	vb->state = VIDEOBUF_NEEDS_INIT;
-}
-
-static const struct videobuf_queue_ops bcwc_vb_ops = {
-	.buf_setup      = bcwc_vb_buf_setup,
-	.buf_prepare    = bcwc_vb_buf_prepare,
-	.buf_queue      = bcwc_vb_buf_queue,
-	.buf_release    = bcwc_vb_buf_release,
+struct bcwc_fmt bcwc_formats[] = {
+	BCWC_FMT("1280x720 YUYV (4:2:2)", 1280, 720, 1280 * 720 * 2, 1, V4L2_PIX_FMT_YUYV, 0, 0, 0, 1280, 720),
+	BCWC_FMT("1280x720 YVYU (4:2:2)", 1280, 720, 1280 * 720 * 2, 1, V4L2_PIX_FMT_YVYU, 0, 0, 0, 1280, 720),
+	BCWC_FMT("1280x720 NV16", 1280, 720, 1280 * 720, 2, V4L2_PIX_FMT_NV16, 0, 0, 0, 1280, 720),
+	BCWC_FMT("640x480 YUYV (4:2:2)", 640,  480, 640 * 480 * 2, 1, V4L2_PIX_FMT_YUYV, 0, 160, 0, 960, 720),
+	BCWC_FMT("640x480 YVYU (4:2:2)", 640,  480, 640 * 480 * 2, 1, V4L2_PIX_FMT_YVYU, 0, 160, 0, 960, 720),
+	BCWC_FMT("640x480 NV16", 640,  480, 640 * 480, 2, V4L2_PIX_FMT_NV16, 0, 160, 0, 960, 720),
+	BCWC_FMT("320x240 YUYV (4:2:2)", 320,  240, 320 * 240 * 2, 1, V4L2_PIX_FMT_YUYV, 0, 160, 0, 960, 720),
+	BCWC_FMT("320x240 YVYU (4:2:2)", 320,  240, 320 * 240 * 2, 1, V4L2_PIX_FMT_YVYU, 0, 160, 0, 960, 720),
+	BCWC_FMT("320x240 NV16", 320,  240, 320 * 240, 2, V4L2_PIX_FMT_NV16, 0, 160, 0, 960, 720),
 };
 
-static int bcwc_v4l2_open(struct file *filp)
+static int bcwc_buffer_queue_setup(struct vb2_queue *vq,
+				   const struct v4l2_format *fmt,
+				   unsigned int *nbuffers, unsigned int *nplanes,
+				   unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct bcwc_private *priv = video_drvdata(filp);
-	pr_info("%s: %p\n", __FUNCTION__, priv);
+	struct bcwc_private *dev_priv = vb2_get_drv_priv(vq);
+	int i;
+	pr_debug("%s: nbuffers %d, nplanes %d, sizes[0]: %d\n", __FUNCTION__,
+		 *nbuffers, *nplanes, sizes[0]);
 
-	mutex_lock(&priv->ioctl_lock);
-	videobuf_queue_sg_init(&priv->vb_queue, &bcwc_vb_ops,
-			       &priv->pdev->dev, &priv->vb_queue_lock,
-			       V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
-			       sizeof(struct videobuf_buffer), priv, NULL);
+	if (!dev_priv->fmt)
+	  return -EINVAL;
 
-	if (!priv->users++) {
-		bcwc_isp_cmd_channel_start(priv);
+	*nplanes = dev_priv->fmt->planes;
+	for(i = 0; i < *nplanes; i++) {
+		sizes[i] = dev_priv->fmt->fmt.sizeimage;
+		alloc_ctxs[i] = dev_priv->alloc_ctx;
 	}
-
-	mutex_unlock(&priv->ioctl_lock);
+	*nbuffers = BCWC_BUFFERS;
 	return 0;
 }
 
-static int bcwc_v4l2_release(struct file *filp)
+static void bcwc_buffer_cleanup(struct vb2_buffer *vb)
 {
-	struct bcwc_private *priv = video_drvdata(filp);
-	pr_info("%s\n", __FUNCTION__);
-	mutex_lock(&priv->ioctl_lock);
-	if (!--priv->users) {
-		bcwc_isp_cmd_channel_stop(priv);
+	struct bcwc_private *dev_priv = vb2_get_drv_priv(vb->vb2_queue);
+	struct h2t_buf_ctx *ctx = NULL;
+	int i;
+
+	pr_debug("%s: vb %p\n", __FUNCTION__, vb);
+	for(i = 0; i < BCWC_BUFFERS; i++) {
+		if (dev_priv->h2t_bufs[i].vb == vb) {
+			ctx = dev_priv->h2t_bufs + i;
+			break;
+		};
+	}
+	if (!ctx) {
+		pr_err("buffer not found\n");
+		return;
 	}
 
-	videobuf_stop(&priv->vb_queue);
-	videobuf_mmap_free(&priv->vb_queue);
-	mutex_unlock(&priv->ioctl_lock);
+	if (ctx->state == BUF_FREE)
+		return;
+
+	ctx->state = BUF_FREE;
+	ctx->vb = NULL;
+	isp_mem_destroy(ctx->dma_desc_obj);
+	for(i = 0; i < dev_priv->fmt->planes; i++) {
+		iommu_free(dev_priv, ctx->plane[i]);
+		ctx->plane[i] = NULL;
+	}
+	ctx->dma_desc_obj = NULL;
+}
+
+static void bcwc_buffer_queue(struct vb2_buffer *vb)
+{
+	struct bcwc_private *dev_priv = vb2_get_drv_priv(vb->vb2_queue);
+	struct dma_descriptor_list *list;
+	struct h2t_buf_ctx *ctx = NULL;
+
+	int i;
+
+	pr_debug("%s: vb %p\n", __FUNCTION__, vb);
+	for(i = 0; i < BCWC_BUFFERS; i++) {
+		if (dev_priv->h2t_bufs[i].vb == vb) {
+			ctx = dev_priv->h2t_bufs + i;
+			break;
+		};
+	}
+
+	if (!ctx) {
+		pr_err("buffer not found\n");
+		return;
+	}
+
+	if (ctx->state != BUF_ALLOC) {
+		pr_err("buffer busy\n");
+		return;
+	}
+
+	if (!vb->vb2_queue->streaming) {
+		pr_debug("not streaming\n");
+		ctx->state = BUF_DRV_QUEUED;
+	} else {
+		list = ctx->dma_desc_list;
+		list->field0 = 1;
+		ctx->state = BUF_HW_QUEUED;
+		wmb();
+		pr_debug("%d: field0: %d, count %d, pool %d, addr0 0x%08x, addr1 0x%08x tag 0x%08llx vb = %p\n", i, list->field0,
+			 list->desc[i].count, list->desc[i].pool, list->desc[i].addr0, list->desc[i].addr1, list->desc[i].tag, ctx->vb);
+
+		bcwc_channel_ringbuf_send(dev_priv, dev_priv->channel_buf_h2t,
+					  ctx->dma_desc_obj->offset, 0x180, 0x30000000);
+	}
+
+}
+
+static int bcwc_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct bcwc_private *dev_priv = vb2_get_drv_priv(vb->vb2_queue);
+	struct sg_table *sgtable;
+	struct h2t_buf_ctx *ctx = NULL;
+	struct dma_descriptor_list *dma_list;
+	int i;
+
+	pr_debug("vb = %p\n", vb);
+
+	for(i = 0; i < BCWC_BUFFERS; i++) {
+		if (dev_priv->h2t_bufs[i].state == BUF_FREE ||
+		    (dev_priv->h2t_bufs[i].state == BUF_ALLOC && dev_priv->h2t_bufs[i].vb == vb)) {
+			ctx = dev_priv->h2t_bufs + i;
+			break;
+		}
+	}
+
+	if (!ctx)
+		return -ENOBUFS;
+
+	if (ctx->state == BUF_FREE) {
+		pr_debug("allocating new entry\n");
+		ctx->dma_desc_obj = isp_mem_create(dev_priv, FTHD_MEM_BUFFER, 0x180);
+		if (!ctx->dma_desc_obj)
+			return -ENOMEM;
+
+		ctx->dma_desc_list = dev_priv->s2_mem + ctx->dma_desc_obj->offset;
+		ctx->vb = vb;
+		ctx->state = BUF_ALLOC;
+
+		for(i = 0; i < dev_priv->fmt->planes; i++) {
+		  sgtable = vb2_dma_sg_plane_desc(vb, i);
+		  ctx->plane[i] = iommu_allocate_sgtable(dev_priv, sgtable);
+		}
+	}
+
+	vb2_set_plane_payload(vb, 0, dev_priv->fmt->fmt.sizeimage);
+
+	dma_list = ctx->dma_desc_list;
+	memset(dma_list, 0, 0x180);
+
+	dma_list->field0 = 1;
+	dma_list->count = 1;
+	dma_list->desc[0].count = 1;
+	dma_list->desc[0].pool = 0x02;
+	dma_list->desc[0].addr0 = (ctx->plane[0]->offset << 12) | 0xc0000000;
+
+	if (dev_priv->fmt->planes >= 2)
+		dma_list->desc[0].addr1 = (ctx->plane[1]->offset << 12) | 0xc0000000;
+	if (dev_priv->fmt->planes >= 3)
+		dma_list->desc[0].addr2 = (ctx->plane[2]->offset << 12) | 0xc0000000;
+
+	dma_list->desc[0].tag = (u64)ctx;
 	return 0;
 }
 
-static ssize_t bcwc_v4l2_read(struct file *filp, char __user *buffer,
-			   size_t len, loff_t *pos)
+void bcwc_buffer_return_handler(struct bcwc_private *dev_priv, struct dma_descriptor_list *list, int size)
 {
-	struct bcwc_private *priv = video_drvdata(filp);
-	pr_info("%s: buf %p, size %d, pos %p\n", __FUNCTION__, buffer, (int)len, pos);
-	return videobuf_read_stream(&priv->vb_queue, buffer, len, pos, 0,
-				   filp->f_flags & O_NONBLOCK);
+	struct h2t_buf_ctx *ctx;
+	int i;
+
+	for(i = 0; i < list->count; i++) {
+		ctx = (struct h2t_buf_ctx *)list->desc[i].tag;
+		pr_debug("%d: field0: %d, count %d, pool %d, addr0 0x%08x, addr1 0x%08x tag 0x%08llx vb = %p\n", i, list->field0,
+			 list->desc[i].count, list->desc[i].pool, list->desc[i].addr0, list->desc[i].addr1, list->desc[i].tag, ctx->vb);
+
+		if (ctx->state == BUF_HW_QUEUED || ctx->state == BUF_DRV_QUEUED) {
+			ctx->state = BUF_ALLOC;
+			vb2_buffer_done(ctx->vb, VB2_BUF_STATE_DONE);
+		}
+
+	}
 }
 
-static unsigned int bcwc_v4l2_poll(struct file *filp, struct poll_table_struct *pt)
+static int bcwc_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
-	struct bcwc_private *priv = video_drvdata(filp);
-	pr_debug("%s\n", __FUNCTION__);
-	return videobuf_poll_stream(filp, &priv->vb_queue, pt);
+	struct bcwc_private *dev_priv = vb2_get_drv_priv(vq);
+	struct h2t_buf_ctx *ctx;
+	int i, pixelformat;
+
+	pr_debug("%s: %d\n", __FUNCTION__, count);
+
+	bcwc_isp_cmd_channel_crop_set(dev_priv, 0,
+				      dev_priv->fmt->x1,
+				      dev_priv->fmt->y1,
+				      dev_priv->fmt->x2,
+				      dev_priv->fmt->y2);
+
+	switch(dev_priv->fmt->fmt.pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+		pixelformat = 1;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		pixelformat = 2;
+		break;
+	case V4L2_PIX_FMT_NV16:
+		pixelformat = 0;
+		break;
+	default:
+		pixelformat = 1;
+		WARN_ON(1);
+	}
+	bcwc_isp_cmd_channel_output_config_set(dev_priv, 0,
+					       dev_priv->fmt->fmt.width,
+					       dev_priv->fmt->fmt.height,
+					       pixelformat);
+
+	bcwc_isp_cmd_channel_start(dev_priv);
+	mdelay(1000); /* Needed to settle AE */
+	for(i = 0; i < BCWC_BUFFERS && count; i++, count--) {
+		ctx = dev_priv->h2t_bufs + i;
+		if (ctx->state != BUF_DRV_QUEUED)
+			continue;
+
+		ctx->state = BUF_HW_QUEUED;
+		bcwc_channel_ringbuf_send(dev_priv, dev_priv->channel_buf_h2t,
+					  ctx->dma_desc_obj->offset, 0x180, 0x30000000);
+	}
+
+	return 0;
 }
 
-static int bcwc_v4l2_mmap(struct file *filp, struct vm_area_struct *vma)
+static void bcwc_stop_streaming(struct vb2_queue *vq)
 {
-	struct bcwc_private *priv = video_drvdata(filp);
+	struct bcwc_private *dev_priv = vb2_get_drv_priv(vq);
+
 	pr_debug("%s\n", __FUNCTION__);
-	return videobuf_mmap_mapper(&priv->vb_queue, vma);
+
+	bcwc_isp_cmd_channel_buffer_return(dev_priv, 0);
+	pr_debug("waiting for buffers...\n");
+	vb2_wait_for_all_buffers(vq);
+	pr_debug("done\n");
+	bcwc_isp_cmd_channel_stop(dev_priv);
 }
 
+static struct vb2_ops vb2_queue_ops = {
+	.queue_setup            = bcwc_buffer_queue_setup,
+	.buf_prepare            = bcwc_buffer_prepare,
+	.buf_cleanup            = bcwc_buffer_cleanup,
+	.start_streaming        = bcwc_start_streaming,
+	.stop_streaming         = bcwc_stop_streaming,
+	.buf_queue              = bcwc_buffer_queue,
+	.wait_prepare           = vb2_ops_wait_prepare,
+	.wait_finish            = vb2_ops_wait_finish,
+};
+
 static struct v4l2_file_operations bcwc_vdev_fops = {
 	.owner          = THIS_MODULE,
-	.open           = bcwc_v4l2_open,
-	.release        = bcwc_v4l2_release,
-	.read           = bcwc_v4l2_read,
-	.poll           = bcwc_v4l2_poll,
-	.mmap           = bcwc_v4l2_mmap,
+	.open           = v4l2_fh_open,
+
+	.release        = vb2_fop_release,
+	.poll           = vb2_fop_poll,
+	.mmap           = vb2_fop_mmap,
 	.unlocked_ioctl = video_ioctl2
 };
 
@@ -205,42 +359,6 @@ static int bcwc_v4l2_ioctl_querycap(struct file *filp, void *priv,
 	return 0;
 }
 
-static struct bcwc_format {
-	__u8 *desc;
-	__u32 pixelformat;
-	int bpp;   /* Bytes per pixel */
-	u32 mbus_code;
-} bcwc_formats[] = {
-	{
-		.desc           = "YUYV 4:2:2",
-		.pixelformat    = V4L2_PIX_FMT_YUYV,
-		.mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
-		.bpp            = 2,
-	},
-};
-#if 0
-static struct bcwc_format *bcwc_find_format(u32 pixelformat)
-{
-	unsigned i;
-
-	for (i = 0; i < ARRAY_SIZE(bcwc_formats); i++)
-		if (bcwc_formats[i].pixelformat == pixelformat)
-			return bcwc_formats + i;
-	/* Not found? Then return the first format. */
-	return bcwc_formats;
-}
-#endif
-static const struct v4l2_pix_format bcwc_def_pix_format = {
-	.width          = 1280,
-	.height         = 720,
-	.pixelformat    = V4L2_PIX_FMT_YUYV,
-	.field          = V4L2_FIELD_NONE,
-	.bytesperline   = 1280 * 2,
-	.sizeimage      = 1280 * 720 * 2,
-};
-
-static const u32 bcwc_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
-
 static int bcwc_v4l2_ioctl_enum_fmt_vid_cap(struct file *filp, void *priv,
 				   struct v4l2_fmtdesc *fmt)
 {
@@ -248,83 +366,118 @@ static int bcwc_v4l2_ioctl_enum_fmt_vid_cap(struct file *filp, void *priv,
 		return -EINVAL;
 	strlcpy(fmt->description, bcwc_formats[fmt->index].desc,
 		sizeof(fmt->description));
-	fmt->pixelformat = bcwc_formats[fmt->index].pixelformat;
+	fmt->pixelformat = bcwc_formats[fmt->index].fmt.pixelformat;
 	return 0;
 }
 
 static int bcwc_v4l2_ioctl_try_fmt_vid_cap(struct file *filp, void *_priv,
 					   struct v4l2_format *fmt)
 {
-	struct bcwc_private *priv = video_drvdata(filp);
+	struct bcwc_private *dev_priv = video_drvdata(filp);
+	const struct bcwc_fmt *p, *p1;
+	int i, width, height, wanted_width, wanted_height, dist, maxdist;
+
 	pr_info("%s: %dx%d\n", __FUNCTION__, fmt->fmt.pix.width, fmt->fmt.pix.height);
 
-	if (fmt->fmt.pix.height != 720 ||
-	    fmt->fmt.pix.width != 1280 ||
-	    fmt->fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV)
-		return -EINVAL;
+	for(i = 0; i < ARRAY_SIZE(bcwc_formats); i++) {
+		p = bcwc_formats + i;
+		if (p->fmt.height == fmt->fmt.pix.height &&
+		    p->fmt.width == fmt->fmt.pix.width &&
+		    p->fmt.pixelformat == fmt->fmt.pix.pixelformat) {
+			dev_priv->fmt = p;
+			return 0;
+		}
+	}
 
-	priv->user_format.height = fmt->fmt.pix.height;
-	priv->user_format.width = fmt->fmt.pix.width;
-	priv->user_format.sizeimage = priv->user_format.height * priv->user_format.width * 2;
+	wanted_width = fmt->fmt.pix.width;
+	wanted_height = fmt->fmt.pix.height;
+	maxdist = 0x7fffffff;
+	p1 = NULL;
+	for (i = 0; i < ARRAY_SIZE(bcwc_formats); i++) {
+		p = bcwc_formats + i;
+
+		if (p->fmt.pixelformat != fmt->fmt.pix.pixelformat)
+			continue;
+
+		width = p->fmt.width;
+		height = p->fmt.height;
+
+		dist = min(width, wanted_width) * min(height, wanted_height);
+		dist = width * height + wanted_width * wanted_height - 2 * dist;
+
+		pr_debug("%dx%d, dist %d\n", width, height, dist);
+
+		if (dist < maxdist) {
+			maxdist = dist;
+			p1 = p;
+			pr_debug("assign\n");
+		}
+
+		if (maxdist == 0)
+			break;
+	}
+
+	if (!p1) {
+		pr_debug("no assign\n");
+		return -EINVAL;
+	}
+
+	fmt->fmt.pix.width = p1->fmt.width;
+	fmt->fmt.pix.height = p1->fmt.height;
+	fmt->fmt.pix.field = V4L2_FIELD_NONE;
+
+	switch(p1->fmt.pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		fmt->fmt.pix.bytesperline = p1->fmt.width * 2;
+		break;
+	case V4L2_PIX_FMT_NV16:
+		fmt->fmt.pix.bytesperline = p1->fmt.width;
+		break;
+	default:
+		WARN_ON(1);
+	}
 	return 0;
 }
 
 static int bcwc_v4l2_ioctl_g_fmt_vid_cap(struct file *filp, void *priv,
 					 struct v4l2_format *fmt)
 {
-	fmt->fmt.pix.height = 720;
-	fmt->fmt.pix.width = 1280;
-	fmt->fmt.pix.sizeimage = 1280 * 720;
-	fmt->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+	struct bcwc_private *dev_priv = video_drvdata(filp);
+	fmt->fmt.pix = dev_priv->fmt->fmt;
 	return 0;
 }
 
 static int bcwc_v4l2_ioctl_s_fmt_vid_cap(struct file *filp, void *priv,
 					 struct v4l2_format *fmt)
 {
-	pr_info("%s: %dx%d\n", __FUNCTION__, fmt->fmt.pix.width, fmt->fmt.pix.height);
+	struct bcwc_private *dev_priv = video_drvdata(filp);
+	struct bcwc_fmt *p = NULL;
+	int i;
+
+	pr_info("%s: %dx%d %c%c%c%c\n", __FUNCTION__,
+		fmt->fmt.pix.width, fmt->fmt.pix.height,
+		fmt->fmt.pix.pixelformat,
+		fmt->fmt.pix.pixelformat >> 8,
+		fmt->fmt.pix.pixelformat >> 16,
+		fmt->fmt.pix.pixelformat >> 24);
+
+	for(i = 0; i < ARRAY_SIZE(bcwc_formats); i++) {
+		p = bcwc_formats + i;
+		if (p->fmt.width == fmt->fmt.pix.width &&
+		    p->fmt.height == fmt->fmt.pix.height &&
+		    p->fmt.pixelformat == fmt->fmt.pix.pixelformat) {
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(bcwc_formats))
+		return -EINVAL;
+
+	dev_priv->fmt = p;
 	return 0;
 }
 
-static int bcwc_v4l2_ioctl_reqbufs(struct file *filp, void *_priv,
-		struct v4l2_requestbuffers *rb)
-{
-	struct bcwc_private *priv = video_drvdata(filp);
-	return videobuf_reqbufs(&priv->vb_queue, rb);
-}
-
-static int bcwc_v4l2_ioctl_querybuf(struct file *filp, void *_priv,
-		struct v4l2_buffer *buf)
-{
-	struct bcwc_private *priv = video_drvdata(filp);
-	return videobuf_querybuf(&priv->vb_queue, buf);
-}
-
-static int bcwc_v4l2_ioctl_qbuf(struct file *filp, void *_priv,
-				struct v4l2_buffer *buf)
-{
-	struct bcwc_private *priv = video_drvdata(filp);
-	return videobuf_qbuf(&priv->vb_queue, buf);
-}
-
-static int bcwc_v4l2_ioctl_dqbuf(struct file *filp, void *_priv,
-				 struct v4l2_buffer *buf)
-{
-	struct bcwc_private *priv = video_drvdata(filp);
-	return videobuf_dqbuf(&priv->vb_queue, buf, filp->f_flags & O_NONBLOCK);
-}
-
-static int bcwc_v4l2_ioctl_streamon(struct file *filp, void *priv, enum v4l2_buf_type t)
-{
-	pr_debug("%s\n", __FUNCTION__);
-	return -ENODEV;
-}
-
-static int bcwc_v4l2_ioctl_streamoff(struct file *filp, void *priv, enum v4l2_buf_type t)
-{
-	pr_debug("%s\n", __FUNCTION__);
-	return -ENODEV;
-}
 
 static int bcwc_v4l2_ioctl_g_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parm)
@@ -343,8 +496,32 @@ static int bcwc_v4l2_ioctl_s_parm(struct file *filp, void *priv,
 static int bcwc_v4l2_ioctl_enum_framesizes(struct file *filp, void *priv,
 		struct v4l2_frmsizeenum *sizes)
 {
-	pr_debug("%s\n", __FUNCTION__);
-	return -ENODEV;
+	struct bcwc_fmt *p = NULL;
+	int i, j;
+	pr_debug("format %c%c%c%c, index %d\n",
+		 sizes->pixel_format >> 24,
+		 sizes->pixel_format >> 16,
+		 sizes->pixel_format >> 8,
+		 sizes->pixel_format,
+		 sizes->index);
+
+	for(i = 0, j = 0; i < ARRAY_SIZE(bcwc_formats); i++) {
+		if (bcwc_formats[i].fmt.pixelformat == sizes->pixel_format) {
+			if (j++ == sizes->index) {
+				p = bcwc_formats + i;
+				break;
+			}
+		}
+	}
+
+	if (!p)
+		return -EINVAL;
+
+	sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	sizes->discrete.width = p->fmt.width;
+	sizes->discrete.height = p->fmt.height;
+	pr_debug("%dx%d\n", sizes->discrete.width, sizes->discrete.height);
+	return 0;
 }
 
 static int bcwc_v4l2_ioctl_enum_frameintervals(struct file *filp, void *priv,
@@ -366,14 +543,17 @@ static struct v4l2_ioctl_ops bcwc_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap   = bcwc_v4l2_ioctl_g_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap   = bcwc_v4l2_ioctl_s_fmt_vid_cap,
 	.vidioc_querycap        = bcwc_v4l2_ioctl_querycap,
-	.vidioc_reqbufs         = bcwc_v4l2_ioctl_reqbufs,
 
-	.vidioc_querybuf        = bcwc_v4l2_ioctl_querybuf,
-	.vidioc_qbuf            = bcwc_v4l2_ioctl_qbuf,
-	.vidioc_dqbuf           = bcwc_v4l2_ioctl_dqbuf,
 
-	.vidioc_streamon        = bcwc_v4l2_ioctl_streamon,
-	.vidioc_streamoff       = bcwc_v4l2_ioctl_streamoff,
+        .vidioc_reqbufs         = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs     = vb2_ioctl_create_bufs,
+	.vidioc_querybuf        = vb2_ioctl_querybuf,
+	.vidioc_qbuf            = vb2_ioctl_qbuf,
+	.vidioc_dqbuf           = vb2_ioctl_dqbuf,
+	.vidioc_expbuf          = vb2_ioctl_expbuf,
+	.vidioc_streamon        = vb2_ioctl_streamon,
+	.vidioc_streamoff       = vb2_ioctl_streamoff,
+
 	.vidioc_g_parm          = bcwc_v4l2_ioctl_g_parm,
 	.vidioc_s_parm          = bcwc_v4l2_ioctl_s_parm,
 	.vidioc_enum_framesizes = bcwc_v4l2_ioctl_enum_framesizes,
@@ -384,6 +564,7 @@ int bcwc_v4l2_register(struct bcwc_private *dev_priv)
 {
 	struct v4l2_device *v4l2_dev = &dev_priv->v4l2_dev;
 	struct video_device *vdev;
+	struct vb2_queue *q;
 	int ret;
 
 	ret = v4l2_device_register(&dev_priv->pdev->dev, v4l2_dev);
@@ -399,13 +580,31 @@ int bcwc_v4l2_register(struct bcwc_private *dev_priv)
 	}
 	dev_priv->videodev = vdev;
 
+	q = &dev_priv->vb2_queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	q->drv_priv = dev_priv;
+	q->ops = &vb2_queue_ops;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->buf_struct_size = 0;//sizeof(struct vpif_cap_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->min_buffers_needed = 1;
+	q->lock = &dev_priv->vb2_queue_lock;
+
+	ret = vb2_queue_init(q);
+	if (ret)
+		goto fail;
+
+	dev_priv->alloc_ctx = vb2_dma_sg_init_ctx(&dev_priv->pdev->dev);
+	dev_priv->fmt = bcwc_formats;
 	vdev->v4l2_dev = v4l2_dev;
 	strcpy(vdev->name, "Apple Facetime HD"); // XXX: Length?
 	vdev->vfl_dir = VFL_DIR_RX;
 	vdev->fops = &bcwc_vdev_fops;
 	vdev->ioctl_ops = &bcwc_ioctl_ops;
+	vdev->queue = q;
 	vdev->release = video_device_release;
-	pr_info("%s: dev_priv %p\n", __FUNCTION__, dev_priv);
+
 	video_set_drvdata(vdev, dev_priv);
 	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 	if (ret) {
@@ -420,6 +619,8 @@ fail:
 
 void bcwc_v4l2_unregister(struct bcwc_private *dev_priv)
 {
+
+	vb2_dma_sg_cleanup_ctx(dev_priv->alloc_ctx);
 	video_unregister_device(dev_priv->videodev);
 	v4l2_device_unregister(&dev_priv->v4l2_dev);
 }
diff --git a/bcwc_v4l2.h b/bcwc_v4l2.h
index ef4e44a..0b70d9f 100644
--- a/bcwc_v4l2.h
+++ b/bcwc_v4l2.h
@@ -17,8 +17,8 @@
  *
  */
 
-#ifndef _BCWC_PCIE_H
-#define _PCWC_PCIE_H
+#ifndef _BCWC_V4L2_H
+#define _BCWC_V4L2_H
 
 #include <linux/pci.h>
 #include <linux/spinlock.h>
@@ -27,6 +27,17 @@
 #include <media/videobuf-dma-sg.h>
 #include <media/v4l2-device.h>
 
+struct bcwc_fmt {
+	struct v4l2_pix_format fmt;
+	const char *desc;
+	int range; /* CISP_COMMAND_CH_OUTPUT_CONFIG_SET */
+	int planes;
+	int x1; /* for CISP_CMD_CH_CROP_SET */
+	int y1;
+	int x2;
+	int y2;
+};
+
 struct bcwc_private;
 extern int bcwc_v4l2_register(struct bcwc_private *dev_priv);
 extern void bcwc_v4l2_unregister(struct bcwc_private *dev_priv);