[PATCH/RFC v2 0/3] Multi-plane video buffer support for V4L2 API and videobuf

[PATCH/RFC v2 0/3] Multi-plane video buffer support for V4L2 API and videobuf

[Pawel Osciak]

Hello,

this version differs only slightly from the previous one, it fixes some
memory allocation/freeing-related problems of the previous patchset.

As was the case with v1, it is still intended for demonstration/discussion/testing
purposes only.


Changes since v2:
- simplified videobuf_buffer allocation
- fixed some videobuf_buffer memory leaks (missing kfrees)


The series contains:

[PATCH v2 1/3] v4l: Add support for multi-plane buffers to V4L2 API.
[PATCH v2 2/3] v4l: videobuf: Add support for multi-plane buffers.
[PATCH v2 3/3] v4l: vivi: add 2- and 3-planar YCbCr422

Best regards
--
Pawel Osciak
Linux Platform Group
Samsung Poland R&D Center

[PATCH v2 1/3] v4l: Add support for multi-plane buffers to V4L2 API.

[Pawel Osciak]

Current V4L2 API assumes that each video buffer contains exactly one,
contiguous memory buffer for video data. Even in case of planar video
formats, e.g. YCbCr with each component residing in a separate area
of memory, it is specified that each of those planes immediately follows
the previous one in memory.

There exist hardware video devices that handle, or even require, each of
the planes to reside in a separate, arbitrary memory area. Some even
require different planes to be placed in different, physical memory banks.

This patch introduces a backward-compatible extension of V4L2 API, which
allows passing additional, per-plane info in the video buffer structure.

Signed-off-by: Pawel Osciak <p.osciak@samsung.com>
Reviewed-by: Marek Szyprowski <m.szyprowski@samsung.com>
Reviewed-by: Kyungmin Park <kyungmin.park@samsung.com>
---
 drivers/media/video/v4l2-ioctl.c |   97 ++++++++++++++++++++++++++-----------
 include/linux/videodev2.h        |   33 ++++++++++++-
 2 files changed, 99 insertions(+), 31 deletions(-)

diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
index 4b11257..b89b73f 100644
--- a/drivers/media/video/v4l2-ioctl.c
+++ b/drivers/media/video/v4l2-ioctl.c
@@ -172,6 +172,8 @@ static const char *v4l2_memory_names[] = {
 	[V4L2_MEMORY_MMAP]    = "mmap",
 	[V4L2_MEMORY_USERPTR] = "userptr",
 	[V4L2_MEMORY_OVERLAY] = "overlay",
+	[V4L2_MEMORY_MULTI_USERPTR]	= "multi-userptr",
+	[V4L2_MEMORY_MULTI_MMAP]	= "multi-mmap",
 };
 
 #define prt_names(a, arr) ((((a) >= 0) && ((a) < ARRAY_SIZE(arr))) ? \
@@ -1975,7 +1977,7 @@ static unsigned long cmd_input_size(unsigned int cmd)
 	switch (cmd) {
 		CMDINSIZE(ENUM_FMT,		fmtdesc,	type);
 		CMDINSIZE(G_FMT,		format,		type);
-		CMDINSIZE(QUERYBUF,		buffer,		type);
+		CMDINSIZE(QUERYBUF,		buffer,		length);
 		CMDINSIZE(G_PARM,		streamparm,	type);
 		CMDINSIZE(ENUMSTD,		standard,	index);
 		CMDINSIZE(ENUMINPUT,		input,		index);
@@ -2000,6 +2002,46 @@ static unsigned long cmd_input_size(unsigned int cmd)
 	}
 }
 
+static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
+			    void * __user *user_ptr, void ***kernel_ptr)
+{
+	int ret = 0;
+
+	switch(cmd) {
+	case VIDIOC_QUERYBUF:
+	case VIDIOC_QBUF:
+	case VIDIOC_DQBUF: {
+		struct v4l2_buffer *buf = parg;
+
+		if ((buf->memory == V4L2_MEMORY_MULTI_USERPTR
+		     || buf->memory == V4L2_MEMORY_MULTI_MMAP)) {
+			*user_ptr = (void __user *)buf->m.planes;
+			*kernel_ptr = (void **)&buf->m.planes;
+			*array_size = sizeof(struct v4l2_plane) * buf->length;
+			ret = 1;
+		}
+		break;
+	}
+
+	case VIDIOC_S_EXT_CTRLS:
+	case VIDIOC_G_EXT_CTRLS:
+	case VIDIOC_TRY_EXT_CTRLS: {
+		struct v4l2_ext_controls *ctrls = parg;
+
+		if (ctrls->count != 0) {
+			*user_ptr = (void __user *)ctrls->controls;
+			*kernel_ptr = (void **)&ctrls->controls;
+			*array_size = sizeof(struct v4l2_ext_control)
+				    * ctrls->count;
+			ret = 1;
+		}
+		break;
+	}
+	}
+
+	return ret;
+}
+
 long video_ioctl2(struct file *file,
 	       unsigned int cmd, unsigned long arg)
 {
@@ -2007,15 +2049,16 @@ long video_ioctl2(struct file *file,
 	void    *mbuf = NULL;
 	void	*parg = NULL;
 	long	err  = -EINVAL;
-	int     is_ext_ctrl;
-	size_t  ctrls_size = 0;
+	int	has_array_args;
+	size_t  array_size = 0;
 	void __user *user_ptr = NULL;
+	void	**kernel_ptr = NULL;
 
 #ifdef __OLD_VIDIOC_
 	cmd = video_fix_command(cmd);
 #endif
-	is_ext_ctrl = (cmd == VIDIOC_S_EXT_CTRLS || cmd == VIDIOC_G_EXT_CTRLS ||
-		       cmd == VIDIOC_TRY_EXT_CTRLS);
+	/*is_ext_ctrl = (cmd == VIDIOC_S_EXT_CTRLS || cmd == VIDIOC_G_EXT_CTRLS ||
+		       cmd == VIDIOC_TRY_EXT_CTRLS);*/
 
 	/*  Copy arguments into temp kernel buffer  */
 	if (_IOC_DIR(cmd) != _IOC_NONE) {
@@ -2045,43 +2088,39 @@ long video_ioctl2(struct file *file,
 		}
 	}
 
-	if (is_ext_ctrl) {
-		struct v4l2_ext_controls *p = parg;
+	has_array_args = check_array_args(cmd, parg, &array_size,
+					  &user_ptr, &kernel_ptr);
 
-		/* In case of an error, tell the caller that it wasn't
-		   a specific control that caused it. */
-		p->error_idx = p->count;
-		user_ptr = (void __user *)p->controls;
-		if (p->count) {
-			ctrls_size = sizeof(struct v4l2_ext_control) * p->count;
-			/* Note: v4l2_ext_controls fits in sbuf[] so mbuf is still NULL. */
-			mbuf = kmalloc(ctrls_size, GFP_KERNEL);
-			err = -ENOMEM;
-			if (NULL == mbuf)
-				goto out_ext_ctrl;
-			err = -EFAULT;
-			if (copy_from_user(mbuf, user_ptr, ctrls_size))
-				goto out_ext_ctrl;
-			p->controls = mbuf;
-		}
+	if (has_array_args) {
+		/* When adding new types of array args, make sure that the
+		 * parent argument to ioctl, which contains the array, fits into
+		 * sbuf (so that mbuf will still remain unused up to here).
+		 */
+		mbuf = kmalloc(array_size, GFP_KERNEL);
+		err = -ENOMEM;
+		if (NULL == mbuf)
+			goto out_array_args;
+		err = -EFAULT;
+		if (copy_from_user(mbuf, user_ptr, array_size))
+			goto out_array_args;
+		*kernel_ptr = mbuf;
 	}
 
 	/* Handles IOCTL */
 	err = __video_do_ioctl(file, cmd, parg);
 	if (err == -ENOIOCTLCMD)
 		err = -EINVAL;
-	if (is_ext_ctrl) {
-		struct v4l2_ext_controls *p = parg;
 
-		p->controls = (void *)user_ptr;
-		if (p->count && err == 0 && copy_to_user(user_ptr, mbuf, ctrls_size))
+	if (has_array_args) {
+		*kernel_ptr = user_ptr;
+		if (copy_to_user(user_ptr, mbuf, array_size))
 			err = -EFAULT;
-		goto out_ext_ctrl;
+		goto out_array_args;
 	}
 	if (err < 0)
 		goto out;
 
-out_ext_ctrl:
+out_array_args:
 	/*  Copy results into user buffer  */
 	switch (_IOC_DIR(cmd)) {
 	case _IOC_READ:
diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
index d4962a7..bf3f33d 100644
--- a/include/linux/videodev2.h
+++ b/include/linux/videodev2.h
@@ -70,6 +70,7 @@
  * Moved from videodev.h
  */
 #define VIDEO_MAX_FRAME               32
+#define VIDEO_MAX_PLANES		3
 
 #ifndef __KERNEL__
 
@@ -180,6 +181,10 @@ enum v4l2_memory {
 	V4L2_MEMORY_MMAP             = 1,
 	V4L2_MEMORY_USERPTR          = 2,
 	V4L2_MEMORY_OVERLAY          = 3,
+
+	/* Discontiguous buffer types */
+	V4L2_MEMORY_MULTI_USERPTR    = 4,
+	V4L2_MEMORY_MULTI_MMAP       = 5,
 };
 
 /* see also http://vektor.theorem.ca/graphics/ycbcr/ */
@@ -519,6 +524,29 @@ struct v4l2_requestbuffers {
 	__u32			reserved[2];
 };
 
+/* struct v4l2_plane - a multi-plane buffer plane.
+ *
+ * Multi-plane buffers consist of two or more planes, e.g. an YCbCr buffer
+ * with two planes has one plane for Y, and another for interleaved CbCr
+ * components. Each plane can reside in a separate memory buffer, or in
+ * a completely separate memory chip even (e.g. in embedded devices).
+ */
+struct v4l2_plane {
+	__u32			bytesused;
+
+	union {
+		__u32		offset;
+		unsigned long	userptr;
+	} m;
+	__u32			length;
+	__u32			reserved[5];
+};
+
+/* struct v4l2_buffer - a video buffer (frame)
+ * @length:	size of the buffer (not its payload) for single-plane buffers,
+ * 		number of planes (and number of elements in planes array)
+ * 		for multi-plane
+ */
 struct v4l2_buffer {
 	__u32			index;
 	enum v4l2_buf_type      type;
@@ -532,8 +560,9 @@ struct v4l2_buffer {
 	/* memory location */
 	enum v4l2_memory        memory;
 	union {
-		__u32           offset;
-		unsigned long   userptr;
+		__u32			offset;
+		unsigned long		userptr;
+		struct v4l2_plane	*planes;
 	} m;
 	__u32			length;
 	__u32			input;
-- 
1.7.0.31.g1df487

Re: [PATCH v2 1/3] v4l: Add support for multi-plane buffers to V4L2 API.

[Mauro Carvalho Chehab]

Pawel Osciak wrote:
> Current V4L2 API assumes that each video buffer contains exactly one,
> contiguous memory buffer for video data. Even in case of planar video
> formats, e.g. YCbCr with each component residing in a separate area
> of memory, it is specified that each of those planes immediately follows
> the previous one in memory.
> 
> There exist hardware video devices that handle, or even require, each of
> the planes to reside in a separate, arbitrary memory area. Some even
> require different planes to be placed in different, physical memory banks.
> 
> This patch introduces a backward-compatible extension of V4L2 API, which
> allows passing additional, per-plane info in the video buffer structure.
> 
> Signed-off-by: Pawel Osciak <p.osciak@samsung.com>
> Reviewed-by: Marek Szyprowski <m.szyprowski@samsung.com>
> Reviewed-by: Kyungmin Park <kyungmin.park@samsung.com>
> ---
>  drivers/media/video/v4l2-ioctl.c |   97 ++++++++++++++++++++++++++-----------
>  include/linux/videodev2.h        |   33 ++++++++++++-
>  2 files changed, 99 insertions(+), 31 deletions(-)
> 
> diff --git a/drivers/media/video/v4l2-ioctl.c b/drivers/media/video/v4l2-ioctl.c
> index 4b11257..b89b73f 100644
> --- a/drivers/media/video/v4l2-ioctl.c
> +++ b/drivers/media/video/v4l2-ioctl.c
> @@ -172,6 +172,8 @@ static const char *v4l2_memory_names[] = {
>  	[V4L2_MEMORY_MMAP]    = "mmap",
>  	[V4L2_MEMORY_USERPTR] = "userptr",
>  	[V4L2_MEMORY_OVERLAY] = "overlay",
> +	[V4L2_MEMORY_MULTI_USERPTR]	= "multi-userptr",
> +	[V4L2_MEMORY_MULTI_MMAP]	= "multi-mmap",
>  };
>  
>  #define prt_names(a, arr) ((((a) >= 0) && ((a) < ARRAY_SIZE(arr))) ? \
> @@ -1975,7 +1977,7 @@ static unsigned long cmd_input_size(unsigned int cmd)
>  	switch (cmd) {
>  		CMDINSIZE(ENUM_FMT,		fmtdesc,	type);
>  		CMDINSIZE(G_FMT,		format,		type);
> -		CMDINSIZE(QUERYBUF,		buffer,		type);
> +		CMDINSIZE(QUERYBUF,		buffer,		length);
>  		CMDINSIZE(G_PARM,		streamparm,	type);
>  		CMDINSIZE(ENUMSTD,		standard,	index);
>  		CMDINSIZE(ENUMINPUT,		input,		index);
> @@ -2000,6 +2002,46 @@ static unsigned long cmd_input_size(unsigned int cmd)
>  	}
>  }
>  
> +static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
> +			    void * __user *user_ptr, void ***kernel_ptr)
> +{
> +	int ret = 0;
> +
> +	switch(cmd) {
> +	case VIDIOC_QUERYBUF:
> +	case VIDIOC_QBUF:
> +	case VIDIOC_DQBUF: {
> +		struct v4l2_buffer *buf = parg;
> +
> +		if ((buf->memory == V4L2_MEMORY_MULTI_USERPTR
> +		     || buf->memory == V4L2_MEMORY_MULTI_MMAP)) {
> +			*user_ptr = (void __user *)buf->m.planes;
> +			*kernel_ptr = (void **)&buf->m.planes;
> +			*array_size = sizeof(struct v4l2_plane) * buf->length;
> +			ret = 1;
> +		}
> +		break;
> +	}
> +
> +	case VIDIOC_S_EXT_CTRLS:
> +	case VIDIOC_G_EXT_CTRLS:
> +	case VIDIOC_TRY_EXT_CTRLS: {
> +		struct v4l2_ext_controls *ctrls = parg;
> +
> +		if (ctrls->count != 0) {
> +			*user_ptr = (void __user *)ctrls->controls;
> +			*kernel_ptr = (void **)&ctrls->controls;
> +			*array_size = sizeof(struct v4l2_ext_control)
> +				    * ctrls->count;
> +			ret = 1;
> +		}
> +		break;
> +	}
> +	}
> +
> +	return ret;
> +}
> +
>  long video_ioctl2(struct file *file,
>  	       unsigned int cmd, unsigned long arg)
>  {
> @@ -2007,15 +2049,16 @@ long video_ioctl2(struct file *file,
>  	void    *mbuf = NULL;
>  	void	*parg = NULL;
>  	long	err  = -EINVAL;
> -	int     is_ext_ctrl;
> -	size_t  ctrls_size = 0;
> +	int	has_array_args;
> +	size_t  array_size = 0;
>  	void __user *user_ptr = NULL;
> +	void	**kernel_ptr = NULL;
>  
>  #ifdef __OLD_VIDIOC_
>  	cmd = video_fix_command(cmd);
>  #endif
> -	is_ext_ctrl = (cmd == VIDIOC_S_EXT_CTRLS || cmd == VIDIOC_G_EXT_CTRLS ||
> -		       cmd == VIDIOC_TRY_EXT_CTRLS);
> +	/*is_ext_ctrl = (cmd == VIDIOC_S_EXT_CTRLS || cmd == VIDIOC_G_EXT_CTRLS ||
> +		       cmd == VIDIOC_TRY_EXT_CTRLS);*/

Just drop it if not used anymore.

>  
>  	/*  Copy arguments into temp kernel buffer  */
>  	if (_IOC_DIR(cmd) != _IOC_NONE) {
> @@ -2045,43 +2088,39 @@ long video_ioctl2(struct file *file,
>  		}
>  	}
>  
> -	if (is_ext_ctrl) {
> -		struct v4l2_ext_controls *p = parg;
> +	has_array_args = check_array_args(cmd, parg, &array_size,
> +					  &user_ptr, &kernel_ptr);
>  
> -		/* In case of an error, tell the caller that it wasn't
> -		   a specific control that caused it. */
> -		p->error_idx = p->count;
> -		user_ptr = (void __user *)p->controls;
> -		if (p->count) {
> -			ctrls_size = sizeof(struct v4l2_ext_control) * p->count;
> -			/* Note: v4l2_ext_controls fits in sbuf[] so mbuf is still NULL. */
> -			mbuf = kmalloc(ctrls_size, GFP_KERNEL);
> -			err = -ENOMEM;
> -			if (NULL == mbuf)
> -				goto out_ext_ctrl;
> -			err = -EFAULT;
> -			if (copy_from_user(mbuf, user_ptr, ctrls_size))
> -				goto out_ext_ctrl;
> -			p->controls = mbuf;
> -		}
> +	if (has_array_args) {
> +		/* When adding new types of array args, make sure that the
> +		 * parent argument to ioctl, which contains the array, fits into
> +		 * sbuf (so that mbuf will still remain unused up to here).
> +		 */
> +		mbuf = kmalloc(array_size, GFP_KERNEL);
> +		err = -ENOMEM;
> +		if (NULL == mbuf)
> +			goto out_array_args;
> +		err = -EFAULT;
> +		if (copy_from_user(mbuf, user_ptr, array_size))
> +			goto out_array_args;
> +		*kernel_ptr = mbuf;

You probably need something similar to this at v4l2-compat-ioctl32.c.

>  	}
>  
>  	/* Handles IOCTL */
>  	err = __video_do_ioctl(file, cmd, parg);
>  	if (err == -ENOIOCTLCMD)
>  		err = -EINVAL;
> -	if (is_ext_ctrl) {
> -		struct v4l2_ext_controls *p = parg;
>  
> -		p->controls = (void *)user_ptr;
> -		if (p->count && err == 0 && copy_to_user(user_ptr, mbuf, ctrls_size))
> +	if (has_array_args) {
> +		*kernel_ptr = user_ptr;
> +		if (copy_to_user(user_ptr, mbuf, array_size))
>  			err = -EFAULT;
> -		goto out_ext_ctrl;
> +		goto out_array_args;
>  	}
>  	if (err < 0)
>  		goto out;
>  
> -out_ext_ctrl:
> +out_array_args:
>  	/*  Copy results into user buffer  */
>  	switch (_IOC_DIR(cmd)) {
>  	case _IOC_READ:
> diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
> index d4962a7..bf3f33d 100644
> --- a/include/linux/videodev2.h
> +++ b/include/linux/videodev2.h
> @@ -70,6 +70,7 @@
>   * Moved from videodev.h
>   */
>  #define VIDEO_MAX_FRAME               32
> +#define VIDEO_MAX_PLANES		3
>  
>  #ifndef __KERNEL__
>  
> @@ -180,6 +181,10 @@ enum v4l2_memory {
>  	V4L2_MEMORY_MMAP             = 1,
>  	V4L2_MEMORY_USERPTR          = 2,
>  	V4L2_MEMORY_OVERLAY          = 3,
> +
> +	/* Discontiguous buffer types */
> +	V4L2_MEMORY_MULTI_USERPTR    = 4,
> +	V4L2_MEMORY_MULTI_MMAP       = 5,
>  };
>  
>  /* see also http://vektor.theorem.ca/graphics/ycbcr/ */
> @@ -519,6 +524,29 @@ struct v4l2_requestbuffers {
>  	__u32			reserved[2];
>  };
>  
> +/* struct v4l2_plane - a multi-plane buffer plane.
> + *
> + * Multi-plane buffers consist of two or more planes, e.g. an YCbCr buffer
> + * with two planes has one plane for Y, and another for interleaved CbCr
> + * components. Each plane can reside in a separate memory buffer, or in
> + * a completely separate memory chip even (e.g. in embedded devices).
> + */
> +struct v4l2_plane {
> +	__u32			bytesused;
> +
> +	union {
> +		__u32		offset;
> +		unsigned long	userptr;
> +	} m;
> +	__u32			length;
> +	__u32			reserved[5];
> +};
> +
> +/* struct v4l2_buffer - a video buffer (frame)
> + * @length:	size of the buffer (not its payload) for single-plane buffers,
> + * 		number of planes (and number of elements in planes array)
> + * 		for multi-plane
> + */
>  struct v4l2_buffer {
>  	__u32			index;
>  	enum v4l2_buf_type      type;
> @@ -532,8 +560,9 @@ struct v4l2_buffer {
>  	/* memory location */
>  	enum v4l2_memory        memory;
>  	union {
> -		__u32           offset;
> -		unsigned long   userptr;
> +		__u32			offset;
> +		unsigned long		userptr;
> +		struct v4l2_plane	*planes;
>  	} m;
>  	__u32			length;
>  	__u32			input;


-- 

Cheers,
Mauro

[PATCH v2 2/3] v4l: videobuf: Add support for multi-plane buffers.

[Pawel Osciak]

Add support for multiplanar buffers to videobuf core, dma-contig
and vmalloc memory types.

Signed-off-by: Pawel Osciak <p.osciak@samsung.com>
Reviewed-by: Kyungmin Park <kyungmin.park@samsung.com>
---
 drivers/media/video/videobuf-core.c       |  375 ++++++++++++++++++++++++-----
 drivers/media/video/videobuf-dma-contig.c |  297 +++++++++++++++++------
 drivers/media/video/videobuf-vmalloc.c    |  188 ++++++++++++---
 include/media/videobuf-core.h             |   59 +++--
 include/media/videobuf-dma-contig.h       |    3 +
 include/media/videobuf-vmalloc.h          |    2 +
 6 files changed, 722 insertions(+), 202 deletions(-)

diff --git a/drivers/media/video/videobuf-core.c b/drivers/media/video/videobuf-core.c
index bb0a1c8..f54bbc8 100644
--- a/drivers/media/video/videobuf-core.c
+++ b/drivers/media/video/videobuf-core.c
@@ -29,6 +29,10 @@
 	printk(KERN_ERR "magic mismatch: %x (expected %x)\n", is, should); \
 	BUG(); } } while (0)
 
+#define is_multiplane(vb)\
+	(vb->memory == V4L2_MEMORY_MULTI_MMAP\
+	 || vb->memory == V4L2_MEMORY_MULTI_USERPTR)
+
 static int debug;
 module_param(debug, int, 0644);
 
@@ -45,22 +49,24 @@ MODULE_LICENSE("GPL");
 #define CALL(q, f, arg...)						\
 	((q->int_ops->f) ? q->int_ops->f(arg) : 0)
 
-void *videobuf_alloc(struct videobuf_queue *q)
+void *videobuf_alloc(struct videobuf_queue *q, unsigned int num_planes)
 {
 	struct videobuf_buffer *vb;
 
 	BUG_ON(q->msize < sizeof(*vb));
+	BUG_ON(0 == num_planes);
 
 	if (!q->int_ops || !q->int_ops->alloc) {
 		printk(KERN_ERR "No specific ops defined!\n");
 		BUG();
 	}
 
-	vb = q->int_ops->alloc(q->msize);
+	vb = q->int_ops->alloc(q->msize, num_planes);
 
 	if (NULL != vb) {
 		init_waitqueue_head(&vb->done);
 		vb->magic     = MAGIC_BUFFER;
+		vb->num_planes = num_planes;
 	}
 
 	return vb;
@@ -106,6 +112,17 @@ void *videobuf_queue_to_vmalloc (struct videobuf_queue *q,
 }
 EXPORT_SYMBOL_GPL(videobuf_queue_to_vmalloc);
 
+void *videobuf_queue_plane_to_vmalloc(struct videobuf_queue *q,
+				      struct videobuf_buffer *buf,
+				      unsigned int plane)
+{
+	if (q->int_ops->plane_vmalloc)
+		return q->int_ops->plane_vmalloc(buf, plane);
+	else
+		return NULL;
+}
+EXPORT_SYMBOL_GPL(videobuf_queue_plane_to_vmalloc);
+
 /* --------------------------------------------------------------------- */
 
 
@@ -131,7 +148,8 @@ void videobuf_queue_core_init(struct videobuf_queue *q,
 	q->int_ops   = int_ops;
 
 	/* All buffer operations are mandatory */
-	BUG_ON(!q->ops->buf_setup);
+	BUG_ON(!q->ops->buf_negotiate);
+	BUG_ON(!q->ops->buf_setup_plane);
 	BUG_ON(!q->ops->buf_prepare);
 	BUG_ON(!q->ops->buf_queue);
 	BUG_ON(!q->ops->buf_release);
@@ -169,7 +187,7 @@ int videobuf_queue_is_busy(struct videobuf_queue *q)
 	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 		if (NULL == q->bufs[i])
 			continue;
-		if (q->bufs[i]->map) {
+		if (q->bufs[i]->mapped) {
 			dprintk(1, "busy: buffer #%d mapped\n", i);
 			return 1;
 		}
@@ -242,6 +260,8 @@ enum v4l2_field videobuf_next_field(struct videobuf_queue *q)
 static void videobuf_status(struct videobuf_queue *q, struct v4l2_buffer *b,
 			    struct videobuf_buffer *vb, enum v4l2_buf_type type)
 {
+	unsigned int i;
+
 	MAGIC_CHECK(vb->magic, MAGIC_BUFFER);
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
 
@@ -251,20 +271,50 @@ static void videobuf_status(struct videobuf_queue *q, struct v4l2_buffer *b,
 	b->memory   = vb->memory;
 	switch (b->memory) {
 	case V4L2_MEMORY_MMAP:
-		b->m.offset  = vb->boff;
-		b->length    = vb->bsize;
+		b->m.offset  = vb->planes[0].boff;
+		b->length    = vb->planes[0].bsize;
 		break;
 	case V4L2_MEMORY_USERPTR:
-		b->m.userptr = vb->baddr;
-		b->length    = vb->bsize;
+		b->m.userptr = vb->planes[0].baddr;
+		b->length    = vb->planes[0].bsize;
 		break;
 	case V4L2_MEMORY_OVERLAY:
-		b->m.offset  = vb->boff;
+		b->m.offset  = vb->planes[0].boff;
+		break;
+	case V4L2_MEMORY_MULTI_MMAP:
+		if (NULL == b->m.planes) {
+			dprintk(1, "Warning: buffer details not copied back "
+				   "as no planes array has been provided\n");
+			break;
+		}
+
+		BUG_ON(b->length < vb->num_planes);
+		b->length    = vb->num_planes;
+		for (i = 0; i < vb->num_planes; ++i) {
+			b->m.planes[i].m.offset  = vb->planes[i].boff;
+			b->m.planes[i].length    = vb->planes[i].bsize;
+			b->m.planes[i].bytesused = vb->planes[i].size;
+		}
+		break;
+	case V4L2_MEMORY_MULTI_USERPTR:
+		if (NULL == b->m.planes) {
+			dprintk(1, "Warning: buffer details not copied back "
+				   "as no planes array has been provided\n");
+			break;
+		}
+
+		BUG_ON(b->length < vb->num_planes);
+		b->length    = vb->num_planes;
+		for (i = 0; i < vb->num_planes; ++i) {
+			b->m.planes[i].m.userptr = vb->planes[i].baddr;
+			b->m.planes[i].length    = vb->planes[i].bsize;
+			b->m.planes[i].bytesused = vb->planes[i].size;
+		}
 		break;
 	}
 
 	b->flags    = 0;
-	if (vb->map)
+	if (vb->mapped)
 		b->flags |= V4L2_BUF_FLAG_MAPPED;
 
 	switch (vb->state) {
@@ -290,7 +340,8 @@ static void videobuf_status(struct videobuf_queue *q, struct v4l2_buffer *b,
 
 	b->field     = vb->field;
 	b->timestamp = vb->ts;
-	b->bytesused = vb->size;
+	/* TODO what to do with bytesused in the main buffer structure? */
+	b->bytesused = vb->planes[0].size;
 	b->sequence  = vb->field_count >> 1;
 }
 
@@ -305,7 +356,6 @@ static int __videobuf_mmap_free(struct videobuf_queue *q)
 
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
 
-
 	rc  = CALL(q, mmap_free, q);
 
 	q->is_mmapped = 0;
@@ -317,6 +367,8 @@ static int __videobuf_mmap_free(struct videobuf_queue *q)
 		if (NULL == q->bufs[i])
 			continue;
 		q->ops->buf_release(q, q->bufs[i]);
+		kfree(q->bufs[i]->planes);
+		q->bufs[i]->planes = NULL;
 		kfree(q->bufs[i]);
 		q->bufs[i] = NULL;
 	}
@@ -333,23 +385,83 @@ int videobuf_mmap_free(struct videobuf_queue *q)
 	return ret;
 }
 
-/* Locking: Caller holds q->vb_lock */
-int __videobuf_mmap_setup(struct videobuf_queue *q,
-			unsigned int bcount, unsigned int bsize,
-			enum v4l2_memory memory)
+static int buf_setup_planes(struct videobuf_queue *q, unsigned int num_planes,
+			    unsigned int *plane_sizes)
 {
 	unsigned int i;
+	int ret;
+
+	BUG_ON(NULL == plane_sizes);
+
+	dprintk(1, "%s num_planes: %u\n", __func__, num_planes);
+
+	for (i = 0; i < num_planes; ++i) {
+		ret = q->ops->buf_setup_plane(q, i, &plane_sizes[i]);
+		if (ret)
+			return ret;
+		plane_sizes[i] = PAGE_ALIGN(plane_sizes[i]);
+		dprintk(1, "%s %dth plane_size: %d\n",
+			__func__, i, plane_sizes[i]);
+	}
+
+	return 0;
+}
+
+static int verify_planes_array(struct videobuf_queue *q, struct v4l2_buffer *b)
+{
+	if (is_multiplane(q->bufs[b->index])) {
+		if (NULL == b->m.planes) {
+			/* Ioctl logic has not copied planes array
+			 * from userspace */
+			dprintk(1, "Multiplane buffer queried but "
+				   "planes array not provided\n");
+			return -EINVAL;
+		}
+
+		if (b->length != q->bufs[b->index]->num_planes) {
+			dprintk(1, "Incorrect planes array length, "
+				   "expected %d, got %d\n",
+				   q->bufs[b->index]->num_planes, b->length);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/* Locking: Caller holds q->vb_lock */
+int __videobuf_mmap_setup(struct videobuf_queue *q, unsigned int bcount,
+			  unsigned int pcount, enum v4l2_memory memory)
+{
+	unsigned int i = 0;
+	unsigned int j;
 	int err;
+	size_t curr_off = 0;
+	unsigned int *plane_sizes;
 
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
+	BUG_ON(pcount < 1);
+
+	dprintk(1, "mmap setup: bcount: %d, pcount: %d, memory: %d\n",
+		bcount, pcount, memory);
 
 	err = __videobuf_mmap_free(q);
 	if (0 != err)
 		return err;
 
+	plane_sizes = kzalloc(pcount * sizeof(*plane_sizes), GFP_KERNEL);
+	if (NULL == plane_sizes)
+		return -ENOMEM;
+
+	err = buf_setup_planes(q, pcount, plane_sizes);
+	if (err) {
+		dprintk(1, "mmap setup: failed setting up planes\n");
+		goto done;
+	}
+
 	/* Allocate and initialize buffers */
 	for (i = 0; i < bcount; i++) {
-		q->bufs[i] = videobuf_alloc(q);
+		q->bufs[i] = videobuf_alloc(q, pcount);
 
 		if (q->bufs[i] == NULL)
 			break;
@@ -357,12 +469,31 @@ int __videobuf_mmap_setup(struct videobuf_queue *q,
 		q->bufs[i]->i      = i;
 		q->bufs[i]->input  = UNSET;
 		q->bufs[i]->memory = memory;
-		q->bufs[i]->bsize  = bsize;
+		q->bufs[i]->num_planes = pcount;
+
 		switch (memory) {
 		case V4L2_MEMORY_MMAP:
-			q->bufs[i]->boff = PAGE_ALIGN(bsize) * i;
+		case V4L2_MEMORY_MULTI_MMAP:
+			for (j = 0; j < pcount; ++j) {
+				q->bufs[i]->planes[j].boff = curr_off;
+				q->bufs[i]->planes[j].bsize = plane_sizes[j];
+				curr_off += plane_sizes[j];
+				dprintk(1, "%s: buf %d plane: %d: "
+					   "bsize: %d boff: %d\n",
+					   __func__, i, j,
+					   q->bufs[i]->planes[j].bsize,
+					   q->bufs[i]->planes[j].boff);
+			}
 			break;
 		case V4L2_MEMORY_USERPTR:
+		case V4L2_MEMORY_MULTI_USERPTR:
+			for (j = 0; j < pcount; ++j) {
+				q->bufs[i]->planes[j].bsize = plane_sizes[j];
+				dprintk(1, "%s: buf: %d plane: %d bsize: %d\n",
+					   __func__, i, j,
+					   q->bufs[i]->planes[j].bsize);
+			}
+			break;
 		case V4L2_MEMORY_OVERLAY:
 			/* nothing */
 			break;
@@ -370,21 +501,21 @@ int __videobuf_mmap_setup(struct videobuf_queue *q,
 	}
 
 	if (!i)
-		return -ENOMEM;
-
-	dprintk(1, "mmap setup: %d buffers, %d bytes each\n",
-		i, bsize);
+		err = -ENOMEM;
+	else
+		err = i;
 
-	return i;
+done:
+	kfree(plane_sizes);
+	return err;
 }
 
-int videobuf_mmap_setup(struct videobuf_queue *q,
-			unsigned int bcount, unsigned int bsize,
-			enum v4l2_memory memory)
+int videobuf_mmap_setup(struct videobuf_queue *q, unsigned int bcount,
+			unsigned int pcount, enum v4l2_memory memory)
 {
 	int ret;
 	mutex_lock(&q->vb_lock);
-	ret = __videobuf_mmap_setup(q, bcount, bsize, memory);
+	ret = __videobuf_mmap_setup(q, bcount, pcount, memory);
 	mutex_unlock(&q->vb_lock);
 	return ret;
 }
@@ -392,7 +523,7 @@ int videobuf_mmap_setup(struct videobuf_queue *q,
 int videobuf_reqbufs(struct videobuf_queue *q,
 		 struct v4l2_requestbuffers *req)
 {
-	unsigned int size, count;
+	unsigned int buf_count, plane_count;
 	int retval;
 
 	if (req->count < 1) {
@@ -402,7 +533,9 @@ int videobuf_reqbufs(struct videobuf_queue *q,
 
 	if (req->memory != V4L2_MEMORY_MMAP     &&
 	    req->memory != V4L2_MEMORY_USERPTR  &&
-	    req->memory != V4L2_MEMORY_OVERLAY) {
+	    req->memory != V4L2_MEMORY_OVERLAY  &&
+	    req->memory != V4L2_MEMORY_MULTI_MMAP &&
+	    req->memory != V4L2_MEMORY_MULTI_USERPTR) {
 		dprintk(1, "reqbufs: memory type invalid\n");
 		return -EINVAL;
 	}
@@ -425,16 +558,13 @@ int videobuf_reqbufs(struct videobuf_queue *q,
 		goto done;
 	}
 
-	count = req->count;
-	if (count > VIDEO_MAX_FRAME)
-		count = VIDEO_MAX_FRAME;
-	size = 0;
-	q->ops->buf_setup(q, &count, &size);
-	dprintk(1, "reqbufs: bufs=%d, size=0x%x [%u pages total]\n",
-		count, size,
-		(unsigned int)((count*PAGE_ALIGN(size))>>PAGE_SHIFT) );
+	buf_count = req->count;
+	if (buf_count > VIDEO_MAX_FRAME)
+		buf_count = VIDEO_MAX_FRAME;
+	q->ops->buf_negotiate(q, &buf_count, &plane_count);
+	dprintk(1, "reqbufs: bufs=%d, planes=%d\n", buf_count, plane_count);
 
-	retval = __videobuf_mmap_setup(q, count, size, req->memory);
+	retval = __videobuf_mmap_setup(q, buf_count, plane_count, req->memory);
 	if (retval < 0) {
 		dprintk(1, "reqbufs: mmap setup returned %d\n", retval);
 		goto done;
@@ -452,6 +582,7 @@ int videobuf_querybuf(struct videobuf_queue *q, struct v4l2_buffer *b)
 {
 	int ret = -EINVAL;
 
+
 	mutex_lock(&q->vb_lock);
 	if (unlikely(b->type != q->type)) {
 		dprintk(1, "querybuf: Wrong type.\n");
@@ -466,6 +597,10 @@ int videobuf_querybuf(struct videobuf_queue *q, struct v4l2_buffer *b)
 		goto done;
 	}
 
+	ret = verify_planes_array(q, b);
+	if (ret < 0)
+		goto done;
+
 	videobuf_status(q, b, q->bufs[b->index], q->type);
 
 	ret = 0;
@@ -474,6 +609,63 @@ done:
 	return ret;
 }
 
+static int multiusrptr_check_buf(struct videobuf_buffer *vb,
+				 struct v4l2_buffer *buf)
+{
+	unsigned int i;
+
+	BUG_ON(NULL == vb->planes);
+
+	if (buf->length != vb->num_planes) {
+		dprintk(1, "%s: wrong number of planes, should be %d\n",
+			__func__, vb->num_planes);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < buf->length; ++i) {
+		dprintk(1, "%s: plane %d: addr %lu, size %u\n", __func__,
+			i, buf->m.planes[i].m.userptr, buf->m.planes[i].length);
+		if (buf->m.planes[i].length < vb->planes[i].bsize) {
+			dprintk(1, "%s: plane %u too small (%u < %u)\n",
+				__func__, i, buf->m.planes[i].length,
+				vb->planes[i].bsize);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void multiusrptr_set_addr(struct videobuf_buffer *vb,
+				 struct v4l2_buffer *buf)
+{
+	struct v4l2_plane *planes = buf->m.planes;
+	struct videobuf_plane *vb_planes = vb->planes;
+	unsigned int i;
+
+	BUG_ON(NULL == vb_planes);
+
+	for (i = 0; i < vb->num_planes; ++i)
+		vb_planes[i].baddr = planes[i].m.userptr;
+}
+
+static int multiusrptr_addr_match(struct videobuf_buffer *vb,
+				  struct v4l2_buffer *buf)
+{
+	struct v4l2_plane *planes = buf->m.planes;
+	struct videobuf_plane *vb_planes = vb->planes;
+	unsigned int i;
+
+	BUG_ON(NULL == vb_planes);
+
+	for (i = 0; i < vb->num_planes; ++i) {
+		if (vb_planes[i].baddr != planes[i].m.userptr)
+			return 0;
+	}
+
+	return 1;
+}
+
 int videobuf_qbuf(struct videobuf_queue *q,
 	      struct v4l2_buffer *b)
 {
@@ -484,7 +676,8 @@ int videobuf_qbuf(struct videobuf_queue *q,
 
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
 
-	if (b->memory == V4L2_MEMORY_MMAP)
+	if (b->memory == V4L2_MEMORY_MMAP ||
+	    b->memory == V4L2_MEMORY_MULTI_MMAP)
 		down_read(&current->mm->mmap_sem);
 
 	mutex_lock(&q->vb_lock);
@@ -529,24 +722,41 @@ int videobuf_qbuf(struct videobuf_queue *q,
 
 	switch (b->memory) {
 	case V4L2_MEMORY_MMAP:
-		if (0 == buf->baddr) {
-			dprintk(1, "qbuf: mmap requested "
-				   "but buffer addr is zero!\n");
+	case V4L2_MEMORY_MULTI_MMAP:
+		if (!buf->mapped) {
+			dprintk(1, "qbuf: mmap requested, but the buffer "
+				"has not been fully mapped!\n");
 			goto done;
 		}
 		break;
 	case V4L2_MEMORY_USERPTR:
-		if (b->length < buf->bsize) {
+		if (b->length < buf->planes[0].bsize) {
 			dprintk(1, "qbuf: buffer length is not enough\n");
 			goto done;
 		}
 		if (VIDEOBUF_NEEDS_INIT != buf->state &&
-		    buf->baddr != b->m.userptr)
+		    buf->planes[0].baddr != b->m.userptr)
 			q->ops->buf_release(q, buf);
-		buf->baddr = b->m.userptr;
+		buf->planes[0].baddr = b->m.userptr;
+		break;
+	case V4L2_MEMORY_MULTI_USERPTR:
+		/* Verify sizes of each plane */
+		retval = multiusrptr_check_buf(buf, b);
+		if (retval) {
+			dprintk(1, "qbuf: invalid buffer\n");
+			goto done;
+		}
+
+		/* Verify addresses and release if changed */
+		if (VIDEOBUF_NEEDS_INIT != buf->state &&
+		    !multiusrptr_addr_match(buf, b))
+			q->ops->buf_release(q, buf);
+
+		/* Set addresses again */
+		multiusrptr_set_addr(buf, b);
 		break;
 	case V4L2_MEMORY_OVERLAY:
-		buf->boff = b->m.offset;
+		buf->planes[0].boff = b->m.offset;
 		break;
 	default:
 		dprintk(1, "qbuf: wrong memory type\n");
@@ -574,7 +784,8 @@ int videobuf_qbuf(struct videobuf_queue *q,
  done:
 	mutex_unlock(&q->vb_lock);
 
-	if (b->memory == V4L2_MEMORY_MMAP)
+	if (b->memory == V4L2_MEMORY_MMAP ||
+	    b->memory == V4L2_MEMORY_MULTI_MMAP)
 		up_read(&current->mm->mmap_sem);
 
 	return retval;
@@ -651,11 +862,19 @@ int videobuf_dqbuf(struct videobuf_queue *q,
 {
 	struct videobuf_buffer *buf = NULL;
 	int retval;
+	unsigned long len;
+	void __user *usr_addr;
+
 
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
 
 	mutex_lock(&q->vb_lock);
 
+	/* There could be a verify_planes_array() call here to verify
+	 * that userspace has passed the array in the buffer, but it might be
+	 * too much to ask userspace to pass the array in each dqbuf call.
+	 */
+
 	retval = stream_next_buffer(q, &buf, nonblocking);
 	if (retval < 0) {
 		dprintk(1, "dqbuf: next_buffer error: %i\n", retval);
@@ -680,7 +899,16 @@ int videobuf_dqbuf(struct videobuf_queue *q,
 		goto done;
 	}
 	list_del(&buf->stream);
-	memset(b, 0, sizeof(*b));
+
+	if (is_multiplane(buf)) {
+		usr_addr = b->m.planes;
+		len = b->length;
+		memset(b, 0, sizeof(*b));
+		b->m.planes = usr_addr;
+		b->length = len;
+	} else {
+		memset(b, 0, sizeof(*b));
+	}
 	videobuf_status(q, b, buf, q->type);
 
  done:
@@ -747,14 +975,15 @@ static ssize_t videobuf_read_zerocopy(struct videobuf_queue *q,
 
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
 
+	/* TODO has yet to be adapted to mutliplanes */
 	/* setup stuff */
-	q->read_buf = videobuf_alloc(q);
+	q->read_buf = videobuf_alloc(q, 1);
 	if (NULL == q->read_buf)
 		return -ENOMEM;
 
 	q->read_buf->memory = V4L2_MEMORY_USERPTR;
-	q->read_buf->baddr  = (unsigned long)data;
-	q->read_buf->bsize  = count;
+	q->read_buf->planes[0].baddr  = (unsigned long)data;
+	q->read_buf->planes[0].bsize  = count;
 
 	field = videobuf_next_field(q);
 	retval = q->ops->buf_prepare(q, q->read_buf, field);
@@ -771,12 +1000,14 @@ static ssize_t videobuf_read_zerocopy(struct videobuf_queue *q,
 		if (VIDEOBUF_ERROR == q->read_buf->state)
 			retval = -EIO;
 		else
-			retval = q->read_buf->size;
+			retval = q->read_buf->planes[0].size;
 	}
 
  done:
 	/* cleanup */
 	q->ops->buf_release(q, q->read_buf);
+	kfree(q->read_buf->planes);
+	q->read_buf->planes = NULL;
 	kfree(q->read_buf);
 	q->read_buf = NULL;
 	return retval;
@@ -788,14 +1019,17 @@ ssize_t videobuf_read_one(struct videobuf_queue *q,
 {
 	enum v4l2_field field;
 	unsigned long flags = 0;
-	unsigned size = 0, nbufs = 1;
+	unsigned size = 0, nbufs = 1, nplanes;
 	int retval;
 
 	MAGIC_CHECK(q->int_ops->magic, MAGIC_QTYPE_OPS);
 
 	mutex_lock(&q->vb_lock);
 
-	q->ops->buf_setup(q, &nbufs, &size);
+	/* TODO read has yet to be verified for mutliplanes */
+	/*q->ops->buf_setup(q, &nbufs, &size);*/
+	q->ops->buf_negotiate(q, &nbufs, &nplanes);
+	q->ops->buf_setup_plane(q, 0, &size);
 
 	if (NULL == q->read_buf  &&
 	    count >= size        &&
@@ -810,17 +1044,19 @@ ssize_t videobuf_read_one(struct videobuf_queue *q,
 	if (NULL == q->read_buf) {
 		/* need to capture a new frame */
 		retval = -ENOMEM;
-		q->read_buf = videobuf_alloc(q);
+		q->read_buf = videobuf_alloc(q, 1);
 
 		dprintk(1, "video alloc=0x%p\n", q->read_buf);
 		if (NULL == q->read_buf)
 			goto done;
 		q->read_buf->memory = V4L2_MEMORY_USERPTR;
-		q->read_buf->bsize = count; /* preferred size */
+		q->read_buf->planes[0].bsize = count; /* preferred size */
 		field = videobuf_next_field(q);
 		retval = q->ops->buf_prepare(q, q->read_buf, field);
 
 		if (0 != retval) {
+			kfree(q->read_buf->planes);
+			q->read_buf->planes = NULL;
 			kfree(q->read_buf);
 			q->read_buf = NULL;
 			goto done;
@@ -843,6 +1079,8 @@ ssize_t videobuf_read_one(struct videobuf_queue *q,
 	if (VIDEOBUF_ERROR == q->read_buf->state) {
 		/* catch I/O errors */
 		q->ops->buf_release(q, q->read_buf);
+		kfree(q->read_buf->planes);
+		q->read_buf->planes = NULL;
 		kfree(q->read_buf);
 		q->read_buf = NULL;
 		retval = -EIO;
@@ -855,9 +1093,11 @@ ssize_t videobuf_read_one(struct videobuf_queue *q,
 		goto done;
 
 	q->read_off += retval;
-	if (q->read_off == q->read_buf->size) {
+	if (q->read_off == q->read_buf->planes[0].size) {
 		/* all data copied, cleanup */
 		q->ops->buf_release(q, q->read_buf);
+		kfree(q->read_buf->planes);
+		q->read_buf->planes = NULL;
 		kfree(q->read_buf);
 		q->read_buf = NULL;
 	}
@@ -872,17 +1112,20 @@ static int __videobuf_read_start(struct videobuf_queue *q)
 {
 	enum v4l2_field field;
 	unsigned long flags = 0;
-	unsigned int count = 0, size = 0;
+	unsigned int count = 0, size = 0, nplanes;
 	int err, i;
 
-	q->ops->buf_setup(q, &count, &size);
+	/*q->ops->buf_setup(q, &count, &size);*/
+	q->ops->buf_negotiate(q, &count, &nplanes);
+	q->ops->buf_setup_plane(q, 0, &size);
 	if (count < 2)
 		count = 2;
 	if (count > VIDEO_MAX_FRAME)
 		count = VIDEO_MAX_FRAME;
 	size = PAGE_ALIGN(size);
 
-	err = __videobuf_mmap_setup(q, count, size, V4L2_MEMORY_USERPTR);
+	/* TODO multiplane support */
+	err = __videobuf_mmap_setup(q, count, nplanes, V4L2_MEMORY_USERPTR);
 	if (err < 0)
 		return err;
 
@@ -913,6 +1156,8 @@ static void __videobuf_read_stop(struct videobuf_queue *q)
 	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 		if (NULL == q->bufs[i])
 			continue;
+		kfree(q->bufs[i]->planes);
+		q->bufs[i]->planes = NULL;
 		kfree(q->bufs[i]);
 		q->bufs[i] = NULL;
 	}
@@ -1001,13 +1246,13 @@ ssize_t videobuf_read_stream(struct videobuf_queue *q,
 			q->read_off += rc;
 		} else {
 			/* some error */
-			q->read_off = q->read_buf->size;
+			q->read_off = q->read_buf->planes[0].size;
 			if (0 == retval)
 				retval = -EIO;
 		}
 
 		/* requeue buffer when done with copying */
-		if (q->read_off == q->read_buf->size) {
+		if (q->read_off == q->read_buf->planes[0].size) {
 			list_add_tail(&q->read_buf->stream,
 				      &q->stream);
 			spin_lock_irqsave(q->irqlock, flags);
@@ -1098,8 +1343,8 @@ int videobuf_cgmbuf(struct videobuf_queue *q,
 	mbuf->frames = req.count;
 	mbuf->size   = 0;
 	for (i = 0; i < mbuf->frames; i++) {
-		mbuf->offsets[i]  = q->bufs[i]->boff;
-		mbuf->size       += PAGE_ALIGN(q->bufs[i]->bsize);
+		mbuf->offsets[i]  = q->bufs[i]->planes[0].boff;
+		mbuf->size       += q->bufs[i]->planes[0].bsize;
 	}
 
 	return 0;
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index 22c0109..d475571 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -9,6 +9,8 @@
  * Based on videobuf-vmalloc.c,
  * (c) 2007 Mauro Carvalho Chehab, <mchehab@infradead.org>
  *
+ * Adapted for multi-plane buffer support by Pawel Osciak <p.osciak@samsung.com>
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2
@@ -19,7 +21,6 @@
 #include <linux/mm.h>
 #include <linux/pagemap.h>
 #include <linux/dma-mapping.h>
-#include <linux/sched.h>
 #include <media/videobuf-dma-contig.h>
 
 struct videobuf_dma_contig_memory {
@@ -37,6 +38,11 @@ struct videobuf_dma_contig_memory {
 		BUG();							    \
 	}
 
+static int debug;
+#define dprintk(level, fmt, arg...) do {			\
+	if (debug >= level)					\
+	printk(KERN_DEBUG "vbuf: " fmt , ## arg); } while (0)
+
 static void
 videobuf_vm_open(struct vm_area_struct *vma)
 {
@@ -53,6 +59,7 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 	struct videobuf_mapping *map = vma->vm_private_data;
 	struct videobuf_queue *q = map->q;
 	int i;
+	unsigned int j;
 
 	dev_dbg(map->q->dev, "vm_close %p [count=%u,vma=%08lx-%08lx]\n",
 		map, map->count, vma->vm_start, vma->vm_end);
@@ -72,7 +79,12 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 			if (NULL == q->bufs[i])
 				continue;
 
-			if (q->bufs[i]->map != map)
+			for (j = 0; j < q->bufs[i]->num_planes; ++j)
+				if (q->bufs[i]->planes[j].map == map)
+					break;
+
+			if (q->bufs[i]->num_planes == j)
+				/* Not found yet */
 				continue;
 
 			mem = q->bufs[i]->priv;
@@ -83,6 +95,9 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 				   in order to do memory unmap.
 				 */
 
+				/* Mem for j-th plane in the array of
+				 * per-plane privs */
+				mem += j;
 				MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 
 				/* vfree is not atomic - can't be
@@ -96,8 +111,9 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 				mem->vaddr = NULL;
 			}
 
-			q->bufs[i]->map   = NULL;
-			q->bufs[i]->baddr = 0;
+			q->bufs[i]->planes[j].map   = NULL;
+			q->bufs[i]->planes[j].baddr = 0;
+			q->bufs[i]->mapped = 0;
 		}
 
 		kfree(map);
@@ -119,8 +135,8 @@ static const struct vm_operations_struct videobuf_vm_ops = {
  */
 static void videobuf_dma_contig_user_put(struct videobuf_dma_contig_memory *mem)
 {
-	mem->is_userptr = 0;
 	mem->dma_handle = 0;
+	mem->is_userptr = 0;
 	mem->size = 0;
 }
 
@@ -134,90 +150,141 @@ static void videobuf_dma_contig_user_put(struct videobuf_dma_contig_memory *mem)
  *
  * Returns 0 if successful.
  */
-static int videobuf_dma_contig_user_get(struct videobuf_dma_contig_memory *mem,
-					struct videobuf_buffer *vb)
+static int videobuf_dma_contig_user_get(struct videobuf_buffer *vb)
 {
 	struct mm_struct *mm = current->mm;
 	struct vm_area_struct *vma;
+	struct videobuf_dma_contig_memory *mem;
+	struct videobuf_plane *plane;
 	unsigned long prev_pfn, this_pfn;
 	unsigned long pages_done, user_address;
-	unsigned int offset;
+	/*unsigned long baddr;*/
 	int ret;
+	unsigned int i;
 
-	offset = vb->baddr & ~PAGE_MASK;
-	mem->size = PAGE_ALIGN(vb->size + offset);
-	mem->is_userptr = 0;
 	ret = -EINVAL;
+	plane = vb->planes;
 
 	down_read(&mm->mmap_sem);
 
-	vma = find_vma(mm, vb->baddr);
-	if (!vma)
-		goto out_up;
+	mem = vb->priv;
+	for (i = 0; i < vb->num_planes; ++i) {
 
-	if ((vb->baddr + mem->size) > vma->vm_end)
-		goto out_up;
+		if (!plane->baddr)
+			goto out_up;
 
-	pages_done = 0;
-	prev_pfn = 0; /* kill warning */
-	user_address = vb->baddr;
+		mem->is_userptr = 0;
+		mem->size = PAGE_ALIGN(plane->bsize);
 
-	while (pages_done < (mem->size >> PAGE_SHIFT)) {
-		ret = follow_pfn(vma, user_address, &this_pfn);
-		if (ret)
-			break;
+		vma = find_vma(mm, plane->baddr);
+		if (!vma)
+			goto out_up;
 
-		if (pages_done == 0)
-			mem->dma_handle = (this_pfn << PAGE_SHIFT) + offset;
-		else if (this_pfn != (prev_pfn + 1))
-			ret = -EFAULT;
+		if ((plane->baddr + mem->size) > vma->vm_end) {
+			dprintk(1, "Plane %u won't fit into vma "
+				"baddr: %lu, vm_end: %lu\n",
+				i, plane->baddr, vma->vm_end);
+			goto out_up;
+		}
 
-		if (ret)
-			break;
+		pages_done = 0;
+		prev_pfn = 0; /* kill warning */
+		user_address = plane->baddr;
 
-		prev_pfn = this_pfn;
-		user_address += PAGE_SIZE;
-		pages_done++;
-	}
+		while (pages_done < (mem->size >> PAGE_SHIFT)) {
+			ret = follow_pfn(vma, user_address, &this_pfn);
+			if (ret)
+				/*break;*/
+				goto out_up;
+
+			if (pages_done == 0)
+				mem->dma_handle = this_pfn << PAGE_SHIFT;
+			else if (this_pfn != (prev_pfn + 1))
+				ret = -EFAULT;
 
-	if (!ret)
-		mem->is_userptr = 1;
+			if (ret)
+				/*break;*/
+				goto out_up;
+
+			prev_pfn = this_pfn;
+			user_address += PAGE_SIZE;
+			pages_done++;
+		}
 
- out_up:
+		if (!ret)
+			mem->is_userptr = 1;
+
+		++mem;
+		++plane;
+	}
+
+out_up:
 	up_read(&current->mm->mmap_sem);
 
 	return ret;
 }
 
-static void *__videobuf_alloc(size_t size)
+static void *__videobuf_alloc(size_t size, unsigned int num_planes)
 {
 	struct videobuf_dma_contig_memory *mem;
 	struct videobuf_buffer *vb;
+	unsigned int i;
 
-	vb = kzalloc(size + sizeof(*mem), GFP_KERNEL);
-	if (vb) {
-		mem = vb->priv = ((char *)vb) + size;
+	vb = kzalloc(size + sizeof(*mem) * num_planes, GFP_KERNEL);
+	if (!vb)
+		return vb;
+
+	/* At least one plane of memory is always needed */
+	vb->planes = kzalloc(sizeof(struct videobuf_plane) * num_planes,
+			     GFP_KERNEL);
+	if (!vb->planes) {
+		kfree(vb);
+		return NULL;
+	}
+
+	mem = vb->priv = ((char *)vb) + size;
+
+	for (i = 0; i < num_planes; ++i) {
 		mem->magic = MAGIC_DC_MEM;
+		++mem;
 	}
 
 	return vb;
 }
 
-static void *__videobuf_to_vmalloc(struct videobuf_buffer *buf)
+/**
+ * __plane_to_vmalloc() - return vmalloc pointer to a plane
+ * @plane:	Plane number (starting at 0).
+ */
+static void *__plane_to_vmalloc(struct videobuf_buffer *buf, unsigned int plane)
 {
 	struct videobuf_dma_contig_memory *mem = buf->priv;
+	BUG_ON(NULL == buf->planes);
 
-	BUG_ON(!mem);
+	if (plane >= buf->num_planes)
+		return NULL;
+	mem += plane;
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 
 	return mem->vaddr;
 }
 
+/**
+ * __videobuf_to_vmalloc() - return vmalloc pointer to a 1-plane buffer
+ *
+ * Returns vmalloc pointer to the buffer (first plane).
+ */
+static void *__videobuf_to_vmalloc(struct videobuf_buffer *buf)
+{
+	return __plane_to_vmalloc(buf, 0);
+}
+
 static int __videobuf_iolock(struct videobuf_queue *q,
 			     struct videobuf_buffer *vb,
 			     struct v4l2_framebuffer *fbuf)
 {
 	struct videobuf_dma_contig_memory *mem = vb->priv;
+	unsigned int i;
 
 	BUG_ON(!mem);
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
@@ -236,11 +303,13 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 		dev_dbg(q->dev, "%s memory method USERPTR\n", __func__);
 
 		/* handle pointer from user space */
-		if (vb->baddr)
-			return videobuf_dma_contig_user_get(mem, vb);
+		if (vb->planes[0].baddr)
+			/*return videobuf_dma_contig_user_get(mem, vb);*/
+			return videobuf_dma_contig_user_get(vb);
 
+		/* TODO multiplanes */
 		/* allocate memory for the read() method */
-		mem->size = PAGE_ALIGN(vb->size);
+		mem->size = PAGE_ALIGN(vb->planes[0].size);
 		mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
 						&mem->dma_handle, GFP_KERNEL);
 		if (!mem->vaddr) {
@@ -252,6 +321,36 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 		dev_dbg(q->dev, "dma_alloc_coherent data is at %p (%ld)\n",
 			mem->vaddr, mem->size);
 		break;
+	case V4L2_MEMORY_MULTI_MMAP:
+		dev_dbg(q->dev, "%s memory method MULTI_MMAP\n", __func__);
+		BUG_ON(NULL == vb->planes);
+
+		for (i = 0; i < vb->num_planes; ++i) {
+			MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
+			if (!mem->vaddr) {
+				dev_err(q->dev, "plane %d memory is not "
+					"alloced/mmapped\n", i);
+				return -EINVAL;
+			}
+			++mem;
+		}
+		break;
+	case V4L2_MEMORY_MULTI_USERPTR:
+		dev_dbg(q->dev, "%s memory method MULTI_USERPTR\n", __func__);
+		BUG_ON(NULL == vb->planes);
+
+		for (i = 0; i < vb->num_planes; ++i) {
+			MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
+			if (!vb->planes[i].baddr) {
+				dev_err(q->dev, "%s read() not implemented for "
+					"MUTLI_USERPTR\n", __func__);
+				return -EINVAL;
+			}
+			++mem;
+		}
+
+		return videobuf_dma_contig_user_get(vb);
+
 	case V4L2_MEMORY_OVERLAY:
 	default:
 		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n",
@@ -264,12 +363,16 @@ static int __videobuf_iolock(struct videobuf_queue *q,
 
 static int __videobuf_mmap_free(struct videobuf_queue *q)
 {
-	unsigned int i;
+	unsigned int i, j;
 
-	dev_dbg(q->dev, "%s\n", __func__);
 	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
-		if (q->bufs[i] && q->bufs[i]->map)
-			return -EBUSY;
+		if (NULL == q->bufs[i])
+			continue;
+
+		for (j = 0; j < q->bufs[i]->num_planes; ++j) {
+			if (q->bufs[i]->planes[j].map)
+				return -EBUSY;
+		}
 	}
 
 	return 0;
@@ -280,7 +383,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 {
 	struct videobuf_dma_contig_memory *mem;
 	struct videobuf_mapping *map;
-	unsigned int first;
+	unsigned int first, plane;
 	int retval;
 	unsigned long size, offset = vma->vm_pgoff << PAGE_SHIFT;
 
@@ -293,9 +396,16 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 		if (!q->bufs[first])
 			continue;
 
-		if (V4L2_MEMORY_MMAP != q->bufs[first]->memory)
+		if (V4L2_MEMORY_MMAP != q->bufs[first]->memory &&
+		    V4L2_MEMORY_MULTI_MMAP != q->bufs[first]->memory)
 			continue;
-		if (q->bufs[first]->boff == offset)
+
+		for (plane = 0; plane < q->bufs[first]->num_planes; ++plane) {
+			if (q->bufs[first]->planes[plane].boff == offset)
+				break;
+		}
+
+		if (q->bufs[first]->num_planes != plane)
 			break;
 	}
 	if (VIDEO_MAX_FRAME == first) {
@@ -309,18 +419,19 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	if (!map)
 		return -ENOMEM;
 
-	q->bufs[first]->map = map;
+	q->bufs[first]->planes[plane].map = map;
 	map->start = vma->vm_start;
 	map->end = vma->vm_end;
 	map->q = q;
 
-	q->bufs[first]->baddr = vma->vm_start;
+	q->bufs[first]->planes[plane].baddr = vma->vm_start;
 
 	mem = q->bufs[first]->priv;
 	BUG_ON(!mem);
+	mem += plane;
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 
-	mem->size = PAGE_ALIGN(q->bufs[first]->bsize);
+	mem->size = PAGE_ALIGN(q->bufs[first]->planes[plane].bsize);
 	mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
 					&mem->dma_handle, GFP_KERNEL);
 	if (!mem->vaddr) {
@@ -351,13 +462,22 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	vma->vm_flags       |= VM_DONTEXPAND;
 	vma->vm_private_data = map;
 
-	dev_dbg(q->dev, "mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
+	dev_dbg(q->dev,
+		"mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d plane %u\n",
 		map, q, vma->vm_start, vma->vm_end,
-		(long int) q->bufs[first]->bsize,
-		vma->vm_pgoff, first);
+		(long int) q->bufs[first]->planes[plane].bsize,
+		vma->vm_pgoff, first, plane);
 
 	videobuf_vm_open(vma);
 
+	/* Mark vb as mapped after all planes are mapped */
+	for (plane = 0; plane < q->bufs[first]->num_planes; ++plane) {
+		if (0 == q->bufs[first]->planes[plane].map)
+			break;
+	}
+	if (q->bufs[first]->num_planes == plane)
+		q->bufs[first]->mapped = 1;
+
 	return 0;
 
 error:
@@ -376,9 +496,10 @@ static int __videobuf_copy_to_user(struct videobuf_queue *q,
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 	BUG_ON(!mem->vaddr);
 
+	/* TODO multiplanes */
 	/* copy to userspace */
-	if (count > q->read_buf->size - q->read_off)
-		count = q->read_buf->size - q->read_off;
+	if (count > q->read_buf->planes[0].size - q->read_off)
+		count = q->read_buf->planes[0].size - q->read_off;
 
 	vaddr = mem->vaddr;
 
@@ -398,13 +519,14 @@ static int __videobuf_copy_stream(struct videobuf_queue *q,
 	BUG_ON(!mem);
 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
 
+	/* TODO multiplanes */
 	if (vbihack) {
 		/* dirty, undocumented hack -- pass the frame counter
 			* within the last four bytes of each vbi data block.
 			* We need that one to maintain backward compatibility
 			* to all vbi decoding software out there ... */
 		fc = (unsigned int *)mem->vaddr;
-		fc += (q->read_buf->size >> 2) - 1;
+		fc += (q->read_buf->planes[0].size >> 2) - 1;
 		*fc = q->read_buf->field_count >> 1;
 		dev_dbg(q->dev, "vbihack: %d\n", *fc);
 	}
@@ -428,6 +550,7 @@ static struct videobuf_qtype_ops qops = {
 	.video_copy_to_user = __videobuf_copy_to_user,
 	.copy_stream  = __videobuf_copy_stream,
 	.vmalloc      = __videobuf_to_vmalloc,
+	.plane_vmalloc = __plane_to_vmalloc,
 };
 
 void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
@@ -455,34 +578,54 @@ dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf)
 }
 EXPORT_SYMBOL_GPL(videobuf_to_dma_contig);
 
-void videobuf_dma_contig_free(struct videobuf_queue *q,
-			      struct videobuf_buffer *buf)
+dma_addr_t videobuf_plane_to_dma_contig(struct videobuf_buffer *buf,
+					unsigned int plane)
 {
 	struct videobuf_dma_contig_memory *mem = buf->priv;
 
+	BUG_ON(!mem);
+
+	if (plane >= buf->num_planes)
+		return 0;
+
+	mem += plane;
+	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
+
+	return mem->dma_handle;
+}
+EXPORT_SYMBOL_GPL(videobuf_plane_to_dma_contig);
+
+void videobuf_dma_contig_free(struct videobuf_queue *q,
+			      struct videobuf_buffer *vb)
+{
+	struct videobuf_dma_contig_memory *mem = vb->priv;
+	unsigned int i ;
+
 	/* mmapped memory can't be freed here, otherwise mmapped region
 	   would be released, while still needed. In this case, the memory
 	   release should happen inside videobuf_vm_close().
 	   So, it should free memory only if the memory were allocated for
 	   read() operation.
 	 */
-	if (buf->memory != V4L2_MEMORY_USERPTR)
-		return;
-
-	if (!mem)
+	if (vb->memory != V4L2_MEMORY_USERPTR
+	    && vb->memory != V4L2_MEMORY_MULTI_USERPTR)
 		return;
 
-	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
-
-	/* handle user space pointer case */
-	if (buf->baddr) {
-		videobuf_dma_contig_user_put(mem);
-		return;
+	BUG_ON(!mem);
+	BUG_ON(!vb->planes);
+
+	for (i = 0; i < vb->num_planes; ++i) {
+		MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
+		if (vb->planes[i].baddr) {
+			videobuf_dma_contig_user_put(mem);
+		} else {
+			/* read() method */
+			dma_free_coherent(q->dev, mem->size,
+					  mem->vaddr, mem->dma_handle);
+			mem->vaddr = NULL;
+		}
+		++mem;
 	}
-
-	/* read() method */
-	dma_free_coherent(q->dev, mem->size, mem->vaddr, mem->dma_handle);
-	mem->vaddr = NULL;
 }
 EXPORT_SYMBOL_GPL(videobuf_dma_contig_free);
 
diff --git a/drivers/media/video/videobuf-vmalloc.c b/drivers/media/video/videobuf-vmalloc.c
index 136e093..7629735 100644
--- a/drivers/media/video/videobuf-vmalloc.c
+++ b/drivers/media/video/videobuf-vmalloc.c
@@ -8,6 +8,8 @@
  *
  * (c) 2007 Mauro Carvalho Chehab, <mchehab@infradead.org>
  *
+ * Adapted for multi-plane buffer support by Pawel Osciak <p.osciak@samsung.com>
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2
@@ -62,6 +64,7 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 	struct videobuf_mapping *map = vma->vm_private_data;
 	struct videobuf_queue *q = map->q;
 	int i;
+	unsigned int j;
 
 	dprintk(2,"vm_close %p [count=%u,vma=%08lx-%08lx]\n", map,
 		map->count, vma->vm_start, vma->vm_end);
@@ -81,7 +84,12 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 			if (NULL == q->bufs[i])
 				continue;
 
-			if (q->bufs[i]->map != map)
+			for (j = 0; j < q->bufs[i]->num_planes; ++j)
+				if (q->bufs[i]->planes[j].map == map)
+					break;
+
+			if (q->bufs[i]->num_planes == j)
+				/* Not found yet */
 				continue;
 
 			mem = q->bufs[i]->priv;
@@ -92,6 +100,9 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 				   in order to do memory unmap.
 				 */
 
+				/* Mem for j-th plane in the array of
+				 * per-plane privs */
+				mem += j;
 				MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
 
 				/* vfree is not atomic - can't be
@@ -104,8 +115,9 @@ static void videobuf_vm_close(struct vm_area_struct *vma)
 				mem->vmalloc = NULL;
 			}
 
-			q->bufs[i]->map   = NULL;
-			q->bufs[i]->baddr = 0;
+			q->bufs[i]->planes[j].map   = NULL;
+			q->bufs[i]->planes[j].baddr = 0;
+			q->bufs[i]->mapped = 0;
 		}
 
 		kfree(map);
@@ -132,17 +144,30 @@ static const struct vm_operations_struct videobuf_vm_ops =
 	struct videobuf_dma_sg_memory
  */
 
-static void *__videobuf_alloc(size_t size)
+static void *__videobuf_alloc(size_t size, unsigned int num_planes)
 {
 	struct videobuf_vmalloc_memory *mem;
 	struct videobuf_buffer *vb;
+	unsigned int i;
 
-	vb = kzalloc(size+sizeof(*mem),GFP_KERNEL);
+	vb = kzalloc(size + sizeof(*mem) * num_planes, GFP_KERNEL);
 	if (!vb)
 		return vb;
 
-	mem = vb->priv = ((char *)vb)+size;
-	mem->magic=MAGIC_VMAL_MEM;
+	/* At least one plane of memory is always needed */
+	vb->planes = kzalloc(sizeof(struct videobuf_plane) * num_planes,
+			     GFP_KERNEL);
+	if (!vb->planes) {
+		kfree(vb);
+		return NULL;
+	}
+
+	mem = vb->priv = ((char *)vb) + size;
+
+	for (i = 0; i < num_planes; ++i) {
+		mem->magic=MAGIC_VMAL_MEM;
+		++mem;
+	}
 
 	dprintk(1,"%s: allocated at %p(%ld+%ld) & %p(%ld)\n",
 		__func__,vb,(long)sizeof(*vb),(long)size-sizeof(*vb),
@@ -157,6 +182,7 @@ static int __videobuf_iolock (struct videobuf_queue* q,
 {
 	struct videobuf_vmalloc_memory *mem = vb->priv;
 	int pages;
+	unsigned int i;
 
 	BUG_ON(!mem);
 
@@ -173,21 +199,17 @@ static int __videobuf_iolock (struct videobuf_queue* q,
 		}
 		break;
 	case V4L2_MEMORY_USERPTR:
-		pages = PAGE_ALIGN(vb->size);
-
 		dprintk(1, "%s memory method USERPTR\n", __func__);
-
 #if 1
-		if (vb->baddr) {
+		if (vb->planes[0].baddr) {
 			printk(KERN_ERR "USERPTR is currently not supported\n");
 			return -EINVAL;
 		}
 #endif
-
 		/* The only USERPTR currently supported is the one needed for
 		   read() method.
 		 */
-
+		pages = PAGE_ALIGN(vb->planes[0].size);
 		mem->vmalloc = vmalloc_user(pages);
 		if (!mem->vmalloc) {
 			printk(KERN_ERR "vmalloc (%d pages) failed\n", pages);
@@ -216,6 +238,59 @@ static int __videobuf_iolock (struct videobuf_queue* q,
 #endif
 
 		break;
+	case V4L2_MEMORY_MULTI_MMAP:
+		dprintk(1, "%s memory method MULTI_MMAP\n", __func__);
+		BUG_ON(NULL == vb->planes);
+
+		for (i = 0; i < vb->num_planes; ++i) {
+			MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
+			if (!mem->vmalloc) {
+				printk(KERN_ERR
+					"memory is not alloced/mmapped.\n");
+				return -EINVAL;
+			}
+			++mem;
+		}
+		break;
+	case V4L2_MEMORY_MULTI_USERPTR:
+		dprintk(1, "%s memory method MULTI_USERPTR\n", __func__);
+		BUG_ON(NULL == vb->planes);
+
+		for (i = 0; i < vb->num_planes; ++i) {
+			MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
+
+			if (vb->planes[i].baddr) {
+				printk(KERN_ERR
+					"USERPTR is currently not supported\n");
+				return -EINVAL;
+			}
+		}
+
+		/* The only USERPTR currently supported is the one needed for
+		   read() method.
+		 */
+		for (i = 0; i < vb->num_planes; ++i) {
+			pages = PAGE_ALIGN(vb->planes[i].size);
+
+			mem->vmalloc = vmalloc_user(pages);
+			if (!mem->vmalloc) {
+				printk(KERN_ERR
+					"vmalloc for plane %d (%d pages) "
+					"failed\n", i, pages);
+				while (i > 0) {
+					--mem;
+					vfree(mem->vmalloc);
+					mem->vmalloc = NULL;
+					--i;
+				}
+				return -ENOMEM;
+			}
+
+			dprintk(1, "vmalloc is at addr %p (%d pages)\n",
+				mem->vmalloc, pages);
+			++mem;
+		}
+		break;
 	case V4L2_MEMORY_OVERLAY:
 	default:
 		dprintk(1, "%s memory method OVERLAY/unknown\n", __func__);
@@ -236,12 +311,15 @@ static int __videobuf_sync(struct videobuf_queue *q,
 
 static int __videobuf_mmap_free(struct videobuf_queue *q)
 {
-	unsigned int i;
+	unsigned int i, j;
 
 	dprintk(1, "%s\n", __func__);
 	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
-		if (q->bufs[i]) {
-			if (q->bufs[i]->map)
+		if (NULL == q->bufs[i])
+			continue;
+
+		for (j = 0; j < q->bufs[i]->num_planes; ++j) {
+			if (q->bufs[i]->planes[j].map)
 				return -EBUSY;
 		}
 	}
@@ -254,7 +332,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 {
 	struct videobuf_vmalloc_memory *mem;
 	struct videobuf_mapping *map;
-	unsigned int first;
+	unsigned int first, plane;
 	int retval, pages;
 	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
 
@@ -267,9 +345,16 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 		if (NULL == q->bufs[first])
 			continue;
 
-		if (V4L2_MEMORY_MMAP != q->bufs[first]->memory)
+		if (V4L2_MEMORY_MMAP != q->bufs[first]->memory &&
+		    V4L2_MEMORY_MULTI_MMAP != q->bufs[first]->memory)
 			continue;
-		if (q->bufs[first]->boff == offset)
+
+		for (plane = 0; plane < q->bufs[first]->num_planes; ++plane) {
+			if (q->bufs[first]->planes[plane].boff == offset)
+				break;
+		}
+
+		if (q->bufs[first]->num_planes != plane)
 			break;
 	}
 	if (VIDEO_MAX_FRAME == first) {
@@ -283,15 +368,16 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	if (NULL == map)
 		return -ENOMEM;
 
-	q->bufs[first]->map = map;
+	q->bufs[first]->planes[plane].map = map;
 	map->start = vma->vm_start;
 	map->end   = vma->vm_end;
 	map->q     = q;
 
-	q->bufs[first]->baddr = vma->vm_start;
+	q->bufs[first]->planes[plane].baddr = vma->vm_start;
 
 	mem = q->bufs[first]->priv;
 	BUG_ON(!mem);
+	mem += plane;
 	MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
 
 	pages = PAGE_ALIGN(vma->vm_end - vma->vm_start);
@@ -315,13 +401,21 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 	vma->vm_flags       |= VM_DONTEXPAND | VM_RESERVED;
 	vma->vm_private_data = map;
 
-	dprintk(1,"mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
-		map, q, vma->vm_start, vma->vm_end,
-		(long int) q->bufs[first]->bsize,
-		vma->vm_pgoff, first);
+	dprintk(1,"mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx "
+		  "buf %d plane %u\n", map, q, vma->vm_start, vma->vm_end,
+		(long int) q->bufs[first]->planes[plane].bsize,
+		vma->vm_pgoff, first, plane);
 
 	videobuf_vm_open(vma);
 
+	/* Mark vb as mapped after all planes are mapped */
+	for (plane = 0; plane < q->bufs[first]->num_planes; ++plane) {
+		if (0 == q->bufs[first]->planes[plane].map)
+			break;
+	}
+	if (q->bufs[first]->num_planes == plane)
+		q->bufs[first]->mapped = 1;
+
 	return 0;
 
 error:
@@ -340,9 +434,10 @@ static int __videobuf_copy_to_user ( struct videobuf_queue *q,
 
 	BUG_ON (!mem->vmalloc);
 
+	/* TODO multiplanes */
 	/* copy to userspace */
-	if (count > q->read_buf->size - q->read_off)
-		count = q->read_buf->size - q->read_off;
+	if (count > q->read_buf->planes[0].size - q->read_off)
+		count = q->read_buf->planes[0].size - q->read_off;
 
 	if (copy_to_user(data, mem->vmalloc+q->read_off, count))
 		return -EFAULT;
@@ -359,13 +454,14 @@ static int __videobuf_copy_stream ( struct videobuf_queue *q,
 	BUG_ON (!mem);
 	MAGIC_CHECK(mem->magic,MAGIC_VMAL_MEM);
 
+	/* TODO multiplanes */
 	if (vbihack) {
 		/* dirty, undocumented hack -- pass the frame counter
 			* within the last four bytes of each vbi data block.
 			* We need that one to maintain backward compatibility
 			* to all vbi decoding software out there ... */
 		fc  = (unsigned int*)mem->vmalloc;
-		fc += (q->read_buf->size>>2) -1;
+		fc += (q->read_buf->planes[0].size>>2) -1;
 		*fc = q->read_buf->field_count >> 1;
 		dprintk(1,"vbihack: %d\n",*fc);
 	}
@@ -417,9 +513,28 @@ void *videobuf_to_vmalloc (struct videobuf_buffer *buf)
 }
 EXPORT_SYMBOL_GPL(videobuf_to_vmalloc);
 
+void *videobuf_plane_to_vmalloc(struct videobuf_buffer *buf, unsigned int plane)
+{
+	struct videobuf_vmalloc_memory *mem = buf->priv;
+	BUG_ON(!mem);
+
+	if (plane >= buf->num_planes) {
+		dprintk(1, "%s: invalid plane %d\n", __func__, plane);
+		return NULL;
+	}
+
+	mem += plane;
+	MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
+	dprintk(1, "%s: plane %d, vaddr: %p\n", __func__, plane, mem->vmalloc);
+
+	return mem->vmalloc;
+}
+EXPORT_SYMBOL_GPL(videobuf_plane_to_vmalloc);
+
 void videobuf_vmalloc_free (struct videobuf_buffer *buf)
 {
 	struct videobuf_vmalloc_memory *mem = buf->priv;
+	unsigned int i;
 
 	/* mmapped memory can't be freed here, otherwise mmapped region
 	   would be released, while still needed. In this case, the memory
@@ -427,18 +542,21 @@ void videobuf_vmalloc_free (struct videobuf_buffer *buf)
 	   So, it should free memory only if the memory were allocated for
 	   read() operation.
 	 */
-	if ((buf->memory != V4L2_MEMORY_USERPTR) || buf->baddr)
+	if ((buf->memory != V4L2_MEMORY_USERPTR
+	     && buf->memory != V4L2_MEMORY_MULTI_USERPTR)
+	    || buf->planes[0].baddr)
 		return;
 
 	if (!mem)
 		return;
+	BUG_ON(!buf->planes);
 
-	MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
-
-	vfree(mem->vmalloc);
-	mem->vmalloc = NULL;
-
-	return;
+	for (i = 0; i < buf->num_planes; ++i) {
+		MAGIC_CHECK(mem->magic, MAGIC_VMAL_MEM);
+		vfree(mem->vmalloc);
+		mem->vmalloc = NULL;
+		++mem;
+	}
 }
 EXPORT_SYMBOL_GPL(videobuf_vmalloc_free);
 
diff --git a/include/media/videobuf-core.h b/include/media/videobuf-core.h
index 316fdcc..9b617d2 100644
--- a/include/media/videobuf-core.h
+++ b/include/media/videobuf-core.h
@@ -59,6 +59,18 @@ struct videobuf_mapping {
 	struct videobuf_queue *q;
 };
 
+struct videobuf_plane {
+	u32			magic;
+	unsigned long		size;
+	size_t			bsize;
+	size_t			boff;
+	unsigned int		bytesperline;
+	/* For MULTI_USERPTR, userspace address */
+	unsigned long		baddr;
+
+	struct videobuf_mapping	*map;
+};
+
 enum videobuf_state {
 	VIDEOBUF_NEEDS_INIT = 0,
 	VIDEOBUF_PREPARED   = 1,
@@ -76,13 +88,14 @@ struct videobuf_buffer {
 	/* info about the buffer */
 	unsigned int            width;
 	unsigned int            height;
-	unsigned int            bytesperline; /* use only if != 0 */
-	unsigned long           size;
 	unsigned int            input;
 	enum v4l2_field         field;
 	enum videobuf_state     state;
 	struct list_head        stream;  /* QBUF/DQBUF list */
 
+	unsigned int		num_planes;
+	struct videobuf_plane	*planes;
+
 	/* touched by irq handler */
 	struct list_head        queue;
 	wait_queue_head_t       done;
@@ -92,26 +105,19 @@ struct videobuf_buffer {
 	/* Memory type */
 	enum v4l2_memory        memory;
 
-	/* buffer size */
-	size_t                  bsize;
-
-	/* buffer offset (mmap + overlay) */
-	size_t                  boff;
-
-	/* buffer addr (userland ptr!) */
-	unsigned long           baddr;
-
-	/* for mmap'ed buffers */
-	struct videobuf_mapping *map;
+	/* True if all planes are mapped (for MMAP only) */
+	int			mapped;
 
-	/* Private pointer to allow specific methods to store their data */
-	int			privsize;
+	/* Array of per-plane pointers to allow specific methods to store
+	 * their data */
 	void                    *priv;
 };
 
 struct videobuf_queue_ops {
-	int (*buf_setup)(struct videobuf_queue *q,
-			 unsigned int *count, unsigned int *size);
+	int (*buf_negotiate)(struct videobuf_queue *q, unsigned int *buf_count,
+			     unsigned int *plane_count);
+	int (*buf_setup_plane)(struct videobuf_queue *q, unsigned int plane,
+			       unsigned int *plane_size);
 	int (*buf_prepare)(struct videobuf_queue *q,
 			   struct videobuf_buffer *vb,
 			   enum v4l2_field field);
@@ -127,8 +133,10 @@ struct videobuf_queue_ops {
 struct videobuf_qtype_ops {
 	u32                     magic;
 
-	void *(*alloc)		(size_t size);
+	void *(*alloc)		(size_t size, unsigned int num_planes);
 	void *(*vmalloc)	(struct videobuf_buffer *buf);
+	void *(*plane_vmalloc)	(struct videobuf_buffer *buf,
+				 unsigned int plane);
 	int (*iolock)		(struct videobuf_queue* q,
 				 struct videobuf_buffer *vb,
 				 struct v4l2_framebuffer *fbuf);
@@ -188,11 +196,14 @@ int videobuf_waiton(struct videobuf_buffer *vb, int non_blocking, int intr);
 int videobuf_iolock(struct videobuf_queue* q, struct videobuf_buffer *vb,
 		struct v4l2_framebuffer *fbuf);
 
-void *videobuf_alloc(struct videobuf_queue* q);
+void *videobuf_alloc(struct videobuf_queue* q, unsigned int num_planes);
 
 /* Used on videobuf-dvb */
 void *videobuf_queue_to_vmalloc (struct videobuf_queue* q,
 				 struct videobuf_buffer *buf);
+void *videobuf_queue_plane_to_vmalloc(struct videobuf_queue *q,
+				      struct videobuf_buffer *buf,
+				      unsigned int plane);
 
 void videobuf_queue_core_init(struct videobuf_queue *q,
 			 const struct videobuf_queue_ops *ops,
@@ -235,12 +246,10 @@ unsigned int videobuf_poll_stream(struct file *file,
 				  struct videobuf_queue *q,
 				  poll_table *wait);
 
-int videobuf_mmap_setup(struct videobuf_queue *q,
-			unsigned int bcount, unsigned int bsize,
-			enum v4l2_memory memory);
-int __videobuf_mmap_setup(struct videobuf_queue *q,
-			unsigned int bcount, unsigned int bsize,
-			enum v4l2_memory memory);
+int videobuf_mmap_setup(struct videobuf_queue *q, unsigned int bcount,
+			unsigned int pcount, enum v4l2_memory memory);
+int __videobuf_mmap_setup(struct videobuf_queue *q, unsigned int bcount,
+			  unsigned int pcount, enum v4l2_memory memory);
 int videobuf_mmap_free(struct videobuf_queue *q);
 int videobuf_mmap_mapper(struct videobuf_queue *q,
 			 struct vm_area_struct *vma);
diff --git a/include/media/videobuf-dma-contig.h b/include/media/videobuf-dma-contig.h
index ebaa9bc..9dcda78 100644
--- a/include/media/videobuf-dma-contig.h
+++ b/include/media/videobuf-dma-contig.h
@@ -26,6 +26,9 @@ void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
 				    void *priv);
 
 dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf);
+dma_addr_t videobuf_plane_to_dma_contig(struct videobuf_buffer *buf,
+					unsigned int plane);
+
 void videobuf_dma_contig_free(struct videobuf_queue *q,
 			      struct videobuf_buffer *buf);
 
diff --git a/include/media/videobuf-vmalloc.h b/include/media/videobuf-vmalloc.h
index 4b419a2..740aedd 100644
--- a/include/media/videobuf-vmalloc.h
+++ b/include/media/videobuf-vmalloc.h
@@ -39,6 +39,8 @@ void videobuf_queue_vmalloc_init(struct videobuf_queue* q,
 			 void *priv);
 
 void *videobuf_to_vmalloc (struct videobuf_buffer *buf);
+void *videobuf_plane_to_vmalloc(struct videobuf_buffer *buf,
+				unsigned int plane);
 
 void videobuf_vmalloc_free (struct videobuf_buffer *buf);
 
-- 
1.7.0.31.g1df487

[PATCH v2 3/3] v4l: vivi: add 2- and 3-planar YCbCr422

[Pawel Osciak]

Add example 2- and 3- planar YCbCr422 formats for multi-plane
format testing.

Signed-off-by: Pawel Osciak <p.osciak@samsung.com>
Reviewed-by: Kyungmin Park <kyungmin.park@samsung.com>
---
 drivers/media/video/vivi.c |  179 +++++++++++++++++++++++++++++++++++---------
 include/linux/videodev2.h  |    3 +
 2 files changed, 147 insertions(+), 35 deletions(-)

diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index 37632a0..bc1ec0d 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -132,6 +132,9 @@ struct vivi_fmt {
 	char  *name;
 	u32   fourcc;          /* v4l2 format id */
 	int   depth;
+	unsigned int num_planes;
+	unsigned int plane_w_shr;
+	unsigned int plane_h_shr;
 };
 
 static struct vivi_fmt formats[] = {
@@ -139,31 +142,53 @@ static struct vivi_fmt formats[] = {
 		.name     = "4:2:2, packed, YUYV",
 		.fourcc   = V4L2_PIX_FMT_YUYV,
 		.depth    = 16,
+		.num_planes = 1,
 	},
 	{
 		.name     = "4:2:2, packed, UYVY",
 		.fourcc   = V4L2_PIX_FMT_UYVY,
 		.depth    = 16,
+		.num_planes = 1,
 	},
 	{
 		.name     = "RGB565 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
 		.depth    = 16,
+		.num_planes = 1,
 	},
 	{
 		.name     = "RGB565 (BE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
 		.depth    = 16,
+		.num_planes = 1,
 	},
 	{
 		.name     = "RGB555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
 		.depth    = 16,
+		.num_planes = 1,
 	},
 	{
 		.name     = "RGB555 (BE)",
 		.fourcc   = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
 		.depth    = 16,
+		.num_planes = 1,
+	},
+	{
+		.name		= "YUV 4:2:2, 3-planar",
+		.fourcc		= V4L2_PIX_FMT_YUV422PM,
+		.depth		= 16,
+		.num_planes	= 3,
+		.plane_w_shr	= 1,
+		.plane_h_shr	= 0,
+	},
+	{
+		.name		= "YUV 4:2:2, 2-planar",
+		.fourcc		= V4L2_PIX_FMT_NV16M,
+		.depth		= 16,
+		.num_planes	= 2,
+		.plane_w_shr	= 1,
+		.plane_h_shr	= 0,
 	},
 };
 
@@ -361,6 +386,8 @@ static void precalculate_bars(struct vivi_fh *fh)
 		switch (fh->fmt->fourcc) {
 		case V4L2_PIX_FMT_YUYV:
 		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YUV422PM:
+		case V4L2_PIX_FMT_NV16M:
 			is_yuv = 1;
 			break;
 		case V4L2_PIX_FMT_RGB565:
@@ -410,6 +437,8 @@ static void gen_twopix(struct vivi_fh *fh, unsigned char *buf, int colorpos)
 
 		switch (fh->fmt->fourcc) {
 		case V4L2_PIX_FMT_YUYV:
+		case V4L2_PIX_FMT_YUV422PM:
+		case V4L2_PIX_FMT_NV16M:
 			switch (color) {
 			case 0:
 			case 2:
@@ -558,30 +587,58 @@ end:
 static void vivi_fillbuff(struct vivi_fh *fh, struct vivi_buffer *buf)
 {
 	struct vivi_dev *dev = fh->dev;
-	int h , pos = 0;
+	int i, x, h, curr_plane = 0, pos = 0;
 	int hmax  = buf->vb.height;
 	int wmax  = buf->vb.width;
 	struct timeval ts;
-	char *tmpbuf;
-	void *vbuf = videobuf_to_vmalloc(&buf->vb);
+	char *tmpbuf, *p_tmpbuf;
+	char *vbuf[VIDEO_MAX_PLANES];
+
+	for (i = 0; i < fh->fmt->num_planes; ++i) {
+		vbuf[i] = videobuf_plane_to_vmalloc(&buf->vb, i);
+		if (!vbuf[i]) {
+			dprintk(dev, 1, "Failed acquiring vaddr for a plane\n");
+			return;
+		}
+	}
 
-	if (!vbuf)
-		return;
+	if (fh->fmt->num_planes > 1) {
+		tmpbuf = kmalloc(wmax * 2, GFP_ATOMIC);
+		if (!tmpbuf)
+			return;
+
+		for (h = 0; h < hmax; h++) {
+			gen_line(fh, tmpbuf, 0, wmax, hmax, h, dev->mv_count,
+				 dev->timestr);
+			p_tmpbuf = tmpbuf;
+
+			for (x = 0; x < wmax; ++x) {
+				*(vbuf[0]++) = *p_tmpbuf++;
+				*(vbuf[curr_plane + 1]++) = *p_tmpbuf++;
+				if (V4L2_PIX_FMT_YUV422PM == fh->fmt->fourcc)
+					curr_plane = !curr_plane;
+			}
+		}
 
-	tmpbuf = kmalloc(wmax * 2, GFP_ATOMIC);
-	if (!tmpbuf)
-		return;
+		dev->mv_count++;
 
-	for (h = 0; h < hmax; h++) {
-		gen_line(fh, tmpbuf, 0, wmax, hmax, h, dev->mv_count,
-			 dev->timestr);
-		memcpy(vbuf + pos, tmpbuf, wmax * 2);
-		pos += wmax*2;
-	}
+		kfree(tmpbuf);
+	} else {
+		tmpbuf = kmalloc(wmax * 2, GFP_ATOMIC);
+		if (!tmpbuf)
+			return;
+
+		for (h = 0; h < hmax; h++) {
+			gen_line(fh, tmpbuf, 0, wmax, hmax, h, dev->mv_count,
+				 dev->timestr);
+			memcpy(vbuf[0] + pos, tmpbuf, wmax * 2);
+			pos += wmax*2;
+		}
 
-	dev->mv_count++;
+		dev->mv_count++;
 
-	kfree(tmpbuf);
+		kfree(tmpbuf);
+	}
 
 	/* Updates stream time */
 
@@ -708,8 +765,6 @@ static int vivi_start_thread(struct vivi_fh *fh)
 	dma_q->frame = 0;
 	dma_q->ini_jiffies = jiffies;
 
-	dprintk(dev, 1, "%s\n", __func__);
-
 	dma_q->kthread = kthread_run(vivi_thread, fh, "vivi");
 
 	if (IS_ERR(dma_q->kthread)) {
@@ -719,7 +774,6 @@ static int vivi_start_thread(struct vivi_fh *fh)
 	/* Wakes thread */
 	wake_up_interruptible(&dma_q->wq);
 
-	dprintk(dev, 1, "returning from %s\n", __func__);
 	return 0;
 }
 
@@ -738,22 +792,66 @@ static void vivi_stop_thread(struct vivi_dmaqueue  *dma_q)
 /* ------------------------------------------------------------------
 	Videobuf operations
    ------------------------------------------------------------------*/
-static int
-buffer_setup(struct videobuf_queue *vq, unsigned int *count, unsigned int *size)
+static unsigned long get_plane_size(struct vivi_fh *fh, unsigned int plane)
 {
-	struct vivi_fh  *fh = vq->priv_data;
-	struct vivi_dev *dev  = fh->dev;
+	unsigned long plane_size = 0;
+
+	if (plane >= fh->fmt->num_planes)
+		return 0;
+
+	if (1 == fh->fmt->num_planes) {
+		plane_size = fh->width * fh->height * 2;
+	} else {
+		if (0 == plane) {
+			plane_size = fh->width * fh->height;
+		} else {
+			plane_size = (fh->width >> 1) * fh->height;
+			if (2 == fh->fmt->num_planes)
+				plane_size *= 2;
+		}
+	}
+
+	return plane_size;
+}
+static int buffer_negotiate(struct videobuf_queue *vq, unsigned int *buf_count,
+			    unsigned int *plane_count)
+{
+	struct vivi_fh *fh	= vq->priv_data;
+	struct vivi_dev *dev	= fh->dev;
+	unsigned int buf_size	= 0;
+	unsigned int i;
+
+	*plane_count = fh->fmt->num_planes;
+
+	if (0 == *buf_count)
+		*buf_count = 32;
+
+	for (i = 0; i < fh->fmt->num_planes; ++i)
+		buf_size += get_plane_size(fh, i);
+
+	while (buf_size * *buf_count > vid_limit * 1024 * 1024)
+		(*buf_count)--;
+
+	dprintk(dev, 1, "%s, buffer count=%d, plane count=%d\n",
+			__func__, *buf_count, *plane_count);
 
-	*size = fh->width*fh->height*2;
+	return 0;
+}
 
-	if (0 == *count)
-		*count = 32;
+static int buffer_setup_plane(struct videobuf_queue *vq, unsigned int plane,
+			      unsigned int *plane_size)
+{
+	struct vivi_fh *fh	= vq->priv_data;
+	struct vivi_dev *dev	= fh->dev;
 
-	while (*size * *count > vid_limit * 1024 * 1024)
-		(*count)--;
+	if (plane >= fh->fmt->num_planes) {
+		dprintk(dev, 1, "%s, invalid plane=%d\n", __func__, plane);
+		return -EINVAL;
+	}
 
-	dprintk(dev, 1, "%s, count=%d, size=%d\n", __func__,
-		*count, *size);
+	*plane_size = get_plane_size(fh, plane);
+	dprintk(dev, 1, "%s, plane=%d, size=%d\n",
+		__func__, plane, *plane_size);
 
 	return 0;
 }
@@ -783,6 +881,7 @@ buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 	struct vivi_dev    *dev = fh->dev;
 	struct vivi_buffer *buf = container_of(vb, struct vivi_buffer, vb);
 	int rc;
+	unsigned int i;
 
 	dprintk(dev, 1, "%s, field=%d\n", __func__, field);
 
@@ -792,9 +891,17 @@ buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 	    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;
+	for (i = 0; i < fh->fmt->num_planes; ++i) {
+		buf->vb.planes[i].size = get_plane_size(fh, i);
+
+		if (0 != buf->vb.planes[i].baddr
+		    && buf->vb.planes[i].bsize < buf->vb.planes[i].size) {
+			dprintk(dev, 1, "%s, invalid plane %u size: (%d<%lu)\n",
+				__func__, i, buf->vb.planes[i].bsize,
+				buf->vb.planes[i].size);
+			return -EINVAL;
+		}
+	}
 
 	/* These properties only change when queue is idle, see s_fmt */
 	buf->fmt       = fh->fmt;
@@ -846,7 +953,8 @@ static void buffer_release(struct videobuf_queue *vq,
 }
 
 static struct videobuf_queue_ops vivi_video_qops = {
-	.buf_setup      = buffer_setup,
+	.buf_negotiate  = buffer_negotiate,
+	.buf_setup_plane = buffer_setup_plane,
 	.buf_prepare    = buffer_prepare,
 	.buf_queue      = buffer_queue,
 	.buf_release    = buffer_release,
@@ -948,8 +1056,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct vivi_fh *fh = priv;
 	struct videobuf_queue *q = &fh->vb_vidq;
+	int ret;
 
-	int ret = vidioc_try_fmt_vid_cap(file, fh, f);
+	ret = vidioc_try_fmt_vid_cap(file, fh, f);
 	if (ret < 0)
 		return ret;
 
diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
index bf3f33d..fbce9d7 100644
--- a/include/linux/videodev2.h
+++ b/include/linux/videodev2.h
@@ -314,6 +314,8 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_UYVY    v4l2_fourcc('U', 'Y', 'V', 'Y') /* 16  YUV 4:2:2     */
 #define V4L2_PIX_FMT_VYUY    v4l2_fourcc('V', 'Y', 'U', 'Y') /* 16  YUV 4:2:2     */
 #define V4L2_PIX_FMT_YUV422P v4l2_fourcc('4', '2', '2', 'P') /* 16  YVU422 planar */
+#define V4L2_PIX_FMT_YUV422PM v4l2_fourcc('4', '2', '2', 'M') /* 16 YUV422 multiplane */
+
 #define V4L2_PIX_FMT_YUV411P v4l2_fourcc('4', '1', '1', 'P') /* 16  YVU411 planar */
 #define V4L2_PIX_FMT_Y41P    v4l2_fourcc('Y', '4', '1', 'P') /* 12  YUV 4:1:1     */
 #define V4L2_PIX_FMT_YUV444  v4l2_fourcc('Y', '4', '4', '4') /* 16  xxxxyyyy uuuuvvvv */
@@ -329,6 +331,7 @@ struct v4l2_pix_format {
 #define V4L2_PIX_FMT_NV12    v4l2_fourcc('N', 'V', '1', '2') /* 12  Y/CbCr 4:2:0  */
 #define V4L2_PIX_FMT_NV21    v4l2_fourcc('N', 'V', '2', '1') /* 12  Y/CrCb 4:2:0  */
 #define V4L2_PIX_FMT_NV16    v4l2_fourcc('N', 'V', '1', '6') /* 16  Y/CbCr 4:2:2  */
+#define V4L2_PIX_FMT_NV16M   v4l2_fourcc('N', 'M', '1', '6') /* 16  Y/CbCr multiplane 4:2:2 */
 #define V4L2_PIX_FMT_NV61    v4l2_fourcc('N', 'V', '6', '1') /* 16  Y/CrCb 4:2:2  */
 
 /* Bayer formats - see http://www.siliconimaging.com/RGB%20Bayer.htm */
-- 
1.7.0.31.g1df487