[deliverable/linux.git] / drivers / media / v4l2-core / videobuf2-dma-sg.c

/*
 * videobuf2-dma-sg.c - dma scatter/gather memory allocator for videobuf2
 *
 * Copyright (C) 2010 Samsung Electronics
 *
 * Author: Andrzej Pietrasiewicz <andrzej.p@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.
 */

#include <linux/module.h>
#include <linux/mm.h>
#include <linux/scatterlist.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/vmalloc.h>

#include <media/videobuf2-core.h>
#include <media/videobuf2-memops.h>
#include <media/videobuf2-dma-sg.h>

static int debug;
module_param(debug, int, 0644);

#define dprintk(level, fmt, arg...)					\
	do {								\
		if (debug >= level)					\
			printk(KERN_DEBUG "vb2-dma-sg: " fmt, ## arg);	\
	} while (0)

struct vb2_dma_sg_buf {
	void				*vaddr;
	struct page			**pages;
	int				write;
	int				offset;
	struct vb2_dma_sg_desc		sg_desc;
	atomic_t			refcount;
	struct vb2_vmarea_handler	handler;
};

static void vb2_dma_sg_put(void *buf_priv);

static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_flags)
{
	struct vb2_dma_sg_buf *buf;
	int i;

	buf = kzalloc(sizeof *buf, GFP_KERNEL);
	if (!buf)
		return NULL;

	buf->vaddr = NULL;
	buf->write = 0;
	buf->offset = 0;
	buf->sg_desc.size = size;
	/* size is already page aligned */
	buf->sg_desc.num_pages = size >> PAGE_SHIFT;

	buf->sg_desc.sglist = vzalloc(buf->sg_desc.num_pages *
				      sizeof(*buf->sg_desc.sglist));
	if (!buf->sg_desc.sglist)
		goto fail_sglist_alloc;
	sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);

	buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
			     GFP_KERNEL);
	if (!buf->pages)
		goto fail_pages_array_alloc;

	for (i = 0; i < buf->sg_desc.num_pages; ++i) {
		buf->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO |
					   __GFP_NOWARN | gfp_flags);
		if (NULL == buf->pages[i])
			goto fail_pages_alloc;
		sg_set_page(&buf->sg_desc.sglist[i],
			    buf->pages[i], PAGE_SIZE, 0);
	}

	buf->handler.refcount = &buf->refcount;
	buf->handler.put = vb2_dma_sg_put;
	buf->handler.arg = buf;

	atomic_inc(&buf->refcount);

	dprintk(1, "%s: Allocated buffer of %d pages\n",
		__func__, buf->sg_desc.num_pages);
	return buf;

fail_pages_alloc:
	while (--i >= 0)
		__free_page(buf->pages[i]);
	kfree(buf->pages);

fail_pages_array_alloc:
	vfree(buf->sg_desc.sglist);

fail_sglist_alloc:
	kfree(buf);
	return NULL;
}

static void vb2_dma_sg_put(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	int i = buf->sg_desc.num_pages;

	if (atomic_dec_and_test(&buf->refcount)) {
		dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
			buf->sg_desc.num_pages);
		if (buf->vaddr)
			vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
		vfree(buf->sg_desc.sglist);
		while (--i >= 0)
			__free_page(buf->pages[i]);
		kfree(buf->pages);
		kfree(buf);
	}
}

static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
				    unsigned long size, int write)
{
	struct vb2_dma_sg_buf *buf;
	unsigned long first, last;
	int num_pages_from_user, i;

	buf = kzalloc(sizeof *buf, GFP_KERNEL);
	if (!buf)
		return NULL;

	buf->vaddr = NULL;
	buf->write = write;
	buf->offset = vaddr & ~PAGE_MASK;
	buf->sg_desc.size = size;

	first = (vaddr           & PAGE_MASK) >> PAGE_SHIFT;
	last  = ((vaddr + size - 1) & PAGE_MASK) >> PAGE_SHIFT;
	buf->sg_desc.num_pages = last - first + 1;

	buf->sg_desc.sglist = vzalloc(
		buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
	if (!buf->sg_desc.sglist)
		goto userptr_fail_sglist_alloc;

	sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);

	buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
			     GFP_KERNEL);
	if (!buf->pages)
		goto userptr_fail_pages_array_alloc;

	num_pages_from_user = get_user_pages(current, current->mm,
					     vaddr & PAGE_MASK,
					     buf->sg_desc.num_pages,
					     write,
					     1, /* force */
					     buf->pages,
					     NULL);

	if (num_pages_from_user != buf->sg_desc.num_pages)
		goto userptr_fail_get_user_pages;

	sg_set_page(&buf->sg_desc.sglist[0], buf->pages[0],
		    PAGE_SIZE - buf->offset, buf->offset);
	size -= PAGE_SIZE - buf->offset;
	for (i = 1; i < buf->sg_desc.num_pages; ++i) {
		sg_set_page(&buf->sg_desc.sglist[i], buf->pages[i],
			    min_t(size_t, PAGE_SIZE, size), 0);
		size -= min_t(size_t, PAGE_SIZE, size);
	}
	return buf;

userptr_fail_get_user_pages:
	dprintk(1, "get_user_pages requested/got: %d/%d]\n",
	       num_pages_from_user, buf->sg_desc.num_pages);
	while (--num_pages_from_user >= 0)
		put_page(buf->pages[num_pages_from_user]);
	kfree(buf->pages);

userptr_fail_pages_array_alloc:
	vfree(buf->sg_desc.sglist);

userptr_fail_sglist_alloc:
	kfree(buf);
	return NULL;
}

/*
 * @put_userptr: inform the allocator that a USERPTR buffer will no longer
 *		 be used
 */
static void vb2_dma_sg_put_userptr(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	int i = buf->sg_desc.num_pages;

	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
	       __func__, buf->sg_desc.num_pages);
	if (buf->vaddr)
		vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
	while (--i >= 0) {
		if (buf->write)
			set_page_dirty_lock(buf->pages[i]);
		put_page(buf->pages[i]);
	}
	vfree(buf->sg_desc.sglist);
	kfree(buf->pages);
	kfree(buf);
}

static void *vb2_dma_sg_vaddr(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;

	BUG_ON(!buf);

	if (!buf->vaddr)
		buf->vaddr = vm_map_ram(buf->pages,
					buf->sg_desc.num_pages,
					-1,
					PAGE_KERNEL);

	/* add offset in case userptr is not page-aligned */
	return buf->vaddr + buf->offset;
}

static unsigned int vb2_dma_sg_num_users(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;

	return atomic_read(&buf->refcount);
}

static int vb2_dma_sg_mmap(void *buf_priv, struct vm_area_struct *vma)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	unsigned long uaddr = vma->vm_start;
	unsigned long usize = vma->vm_end - vma->vm_start;
	int i = 0;

	if (!buf) {
		printk(KERN_ERR "No memory to map\n");
		return -EINVAL;
	}

	do {
		int ret;

		ret = vm_insert_page(vma, uaddr, buf->pages[i++]);
		if (ret) {
			printk(KERN_ERR "Remapping memory, error: %d\n", ret);
			return ret;
		}

		uaddr += PAGE_SIZE;
		usize -= PAGE_SIZE;
	} while (usize > 0);


	/*
	 * Use common vm_area operations to track buffer refcount.
	 */
	vma->vm_private_data	= &buf->handler;
	vma->vm_ops		= &vb2_common_vm_ops;

	vma->vm_ops->open(vma);

	return 0;
}

static void *vb2_dma_sg_cookie(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;

	return &buf->sg_desc;
}

const struct vb2_mem_ops vb2_dma_sg_memops = {
	.alloc		= vb2_dma_sg_alloc,
	.put		= vb2_dma_sg_put,
	.get_userptr	= vb2_dma_sg_get_userptr,
	.put_userptr	= vb2_dma_sg_put_userptr,
	.vaddr		= vb2_dma_sg_vaddr,
	.mmap		= vb2_dma_sg_mmap,
	.num_users	= vb2_dma_sg_num_users,
	.cookie		= vb2_dma_sg_cookie,
};
EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);

MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
MODULE_AUTHOR("Andrzej Pietrasiewicz");
MODULE_LICENSE("GPL");
Commit	Line	Data
5ba3f757 AP	1	/*
	2	* videobuf2-dma-sg.c - dma scatter/gather memory allocator for videobuf2
	3	*
	4	* Copyright (C) 2010 Samsung Electronics
	5	*
	6	* Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
	7	*
	8	* This program is free software; you can redistribute it and/or modify
	9	* it under the terms of the GNU General Public License as published by
	10	* the Free Software Foundation.
	11	*/
	12
	13	#include <linux/module.h>
	14	#include <linux/mm.h>
	15	#include <linux/scatterlist.h>
	16	#include <linux/sched.h>
	17	#include <linux/slab.h>
	18	#include <linux/vmalloc.h>
	19
	20	#include <media/videobuf2-core.h>
	21	#include <media/videobuf2-memops.h>
	22	#include <media/videobuf2-dma-sg.h>
	23
ffdc78ef HV	24	static int debug;
	25	module_param(debug, int, 0644);
	26
	27	#define dprintk(level, fmt, arg...) \
	28	do { \
	29	if (debug >= level) \
	30	printk(KERN_DEBUG "vb2-dma-sg: " fmt, ## arg); \
	31	} while (0)
	32
5ba3f757 AP	33	struct vb2_dma_sg_buf {
	34	void *vaddr;
	35	struct page **pages;
	36	int write;
	37	int offset;
	38	struct vb2_dma_sg_desc sg_desc;
	39	atomic_t refcount;
	40	struct vb2_vmarea_handler handler;
	41	};
	42
	43	static void vb2_dma_sg_put(void *buf_priv);
	44
b6ba2057	45	static void vb2_dma_sg_alloc(void alloc_ctx, unsigned long size, gfp_t gfp_flags)
5ba3f757 AP	46	{
	47	struct vb2_dma_sg_buf *buf;
	48	int i;
	49
	50	buf = kzalloc(sizeof *buf, GFP_KERNEL);
	51	if (!buf)
	52	return NULL;
	53
	54	buf->vaddr = NULL;
	55	buf->write = 0;
	56	buf->offset = 0;
	57	buf->sg_desc.size = size;
7f841459 MCC	58	/* size is already page aligned */
7f841459 MCC	59	buf->sg_desc.num_pages = size >> PAGE_SHIFT;
5ba3f757	60
fabc6b85	61	buf->sg_desc.sglist = vzalloc(buf->sg_desc.num_pages *
5ba3f757 AP	62	sizeof(*buf->sg_desc.sglist));
	63	if (!buf->sg_desc.sglist)
	64	goto fail_sglist_alloc;
5ba3f757 AP	65	sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
	66
	67	buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
	68	GFP_KERNEL);
	69	if (!buf->pages)
	70	goto fail_pages_array_alloc;
	71
	72	for (i = 0; i < buf->sg_desc.num_pages; ++i) {
b6ba2057 HV	73	buf->pages[i] = alloc_page(GFP_KERNEL \| __GFP_ZERO \|
b6ba2057 HV	74	__GFP_NOWARN \| gfp_flags);
5ba3f757 AP	75	if (NULL == buf->pages[i])
	76	goto fail_pages_alloc;
	77	sg_set_page(&buf->sg_desc.sglist[i],
	78	buf->pages[i], PAGE_SIZE, 0);
	79	}
	80
	81	buf->handler.refcount = &buf->refcount;
	82	buf->handler.put = vb2_dma_sg_put;
	83	buf->handler.arg = buf;
	84
	85	atomic_inc(&buf->refcount);
	86
ffdc78ef	87	dprintk(1, "%s: Allocated buffer of %d pages\n",
5ba3f757	88	__func__, buf->sg_desc.num_pages);
5ba3f757 AP	89	return buf;
	90
	91	fail_pages_alloc:
	92	while (--i >= 0)
	93	__free_page(buf->pages[i]);
a9bb36aa	94	kfree(buf->pages);
5ba3f757 AP	95
	96	fail_pages_array_alloc:
	97	vfree(buf->sg_desc.sglist);
	98
	99	fail_sglist_alloc:
	100	kfree(buf);
	101	return NULL;
	102	}
	103
	104	static void vb2_dma_sg_put(void *buf_priv)
	105	{
	106	struct vb2_dma_sg_buf *buf = buf_priv;
	107	int i = buf->sg_desc.num_pages;
	108
	109	if (atomic_dec_and_test(&buf->refcount)) {
ffdc78ef	110	dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
5ba3f757 AP	111	buf->sg_desc.num_pages);
	112	if (buf->vaddr)
	113	vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
	114	vfree(buf->sg_desc.sglist);
	115	while (--i >= 0)
	116	__free_page(buf->pages[i]);
	117	kfree(buf->pages);
	118	kfree(buf);
	119	}
	120	}
	121
	122	static void vb2_dma_sg_get_userptr(void alloc_ctx, unsigned long vaddr,
	123	unsigned long size, int write)
	124	{
	125	struct vb2_dma_sg_buf *buf;
	126	unsigned long first, last;
	127	int num_pages_from_user, i;
	128
	129	buf = kzalloc(sizeof *buf, GFP_KERNEL);
	130	if (!buf)
	131	return NULL;
	132
	133	buf->vaddr = NULL;
	134	buf->write = write;
	135	buf->offset = vaddr & ~PAGE_MASK;
	136	buf->sg_desc.size = size;
	137
	138	first = (vaddr & PAGE_MASK) >> PAGE_SHIFT;
	139	last = ((vaddr + size - 1) & PAGE_MASK) >> PAGE_SHIFT;
	140	buf->sg_desc.num_pages = last - first + 1;
	141
fabc6b85	142	buf->sg_desc.sglist = vzalloc(
5ba3f757 AP	143	buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
	144	if (!buf->sg_desc.sglist)
	145	goto userptr_fail_sglist_alloc;
	146
5ba3f757 AP	147	sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
	148
	149	buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
	150	GFP_KERNEL);
	151	if (!buf->pages)
	152	goto userptr_fail_pages_array_alloc;
	153
5ba3f757 AP	154	num_pages_from_user = get_user_pages(current, current->mm,
	155	vaddr & PAGE_MASK,
	156	buf->sg_desc.num_pages,
	157	write,
	158	1, /* force */
	159	buf->pages,
	160	NULL);
b037c0fd	161
5ba3f757 AP	162	if (num_pages_from_user != buf->sg_desc.num_pages)
	163	goto userptr_fail_get_user_pages;
	164
	165	sg_set_page(&buf->sg_desc.sglist[0], buf->pages[0],
	166	PAGE_SIZE - buf->offset, buf->offset);
	167	size -= PAGE_SIZE - buf->offset;
	168	for (i = 1; i < buf->sg_desc.num_pages; ++i) {
	169	sg_set_page(&buf->sg_desc.sglist[i], buf->pages[i],
	170	min_t(size_t, PAGE_SIZE, size), 0);
	171	size -= min_t(size_t, PAGE_SIZE, size);
	172	}
	173	return buf;
	174
	175	userptr_fail_get_user_pages:
ffdc78ef	176	dprintk(1, "get_user_pages requested/got: %d/%d]\n",
5ba3f757 AP	177	num_pages_from_user, buf->sg_desc.num_pages);
	178	while (--num_pages_from_user >= 0)
	179	put_page(buf->pages[num_pages_from_user]);
a9bb36aa	180	kfree(buf->pages);
5ba3f757 AP	181
	182	userptr_fail_pages_array_alloc:
	183	vfree(buf->sg_desc.sglist);
	184
	185	userptr_fail_sglist_alloc:
	186	kfree(buf);
	187	return NULL;
	188	}
	189
	190	/*
	191	* @put_userptr: inform the allocator that a USERPTR buffer will no longer
	192	* be used
	193	*/
	194	static void vb2_dma_sg_put_userptr(void *buf_priv)
	195	{
	196	struct vb2_dma_sg_buf *buf = buf_priv;
	197	int i = buf->sg_desc.num_pages;
	198
ffdc78ef	199	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
5ba3f757 AP	200	__func__, buf->sg_desc.num_pages);
	201	if (buf->vaddr)
	202	vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
	203	while (--i >= 0) {
	204	if (buf->write)
	205	set_page_dirty_lock(buf->pages[i]);
	206	put_page(buf->pages[i]);
	207	}
	208	vfree(buf->sg_desc.sglist);
	209	kfree(buf->pages);
	210	kfree(buf);
	211	}
	212
	213	static void vb2_dma_sg_vaddr(void buf_priv)
	214	{
	215	struct vb2_dma_sg_buf *buf = buf_priv;
	216
	217	BUG_ON(!buf);
	218
	219	if (!buf->vaddr)
	220	buf->vaddr = vm_map_ram(buf->pages,
	221	buf->sg_desc.num_pages,
	222	-1,
	223	PAGE_KERNEL);
	224
	225	/* add offset in case userptr is not page-aligned */
	226	return buf->vaddr + buf->offset;
	227	}
	228
	229	static unsigned int vb2_dma_sg_num_users(void *buf_priv)
	230	{
	231	struct vb2_dma_sg_buf *buf = buf_priv;
	232
	233	return atomic_read(&buf->refcount);
	234	}
	235
	236	static int vb2_dma_sg_mmap(void buf_priv, struct vm_area_struct vma)
	237	{
	238	struct vb2_dma_sg_buf *buf = buf_priv;
	239	unsigned long uaddr = vma->vm_start;
	240	unsigned long usize = vma->vm_end - vma->vm_start;
	241	int i = 0;
	242
	243	if (!buf) {
	244	printk(KERN_ERR "No memory to map\n");
	245	return -EINVAL;
	246	}
	247
	248	do {
	249	int ret;
	250
	251	ret = vm_insert_page(vma, uaddr, buf->pages[i++]);
	252	if (ret) {
	253	printk(KERN_ERR "Remapping memory, error: %d\n", ret);
	254	return ret;
	255	}
	256
	257	uaddr += PAGE_SIZE;
	258	usize -= PAGE_SIZE;
	259	} while (usize > 0);
	260
	261
	262	/*
	263	* Use common vm_area operations to track buffer refcount.
264	*/
265	vma->vm_private_data = &buf->handler;
266	vma->vm_ops = &vb2_common_vm_ops;
267
268	vma->vm_ops->open(vma);
269
270	return 0;
271	}
272
273	static void vb2_dma_sg_cookie(void buf_priv)
274	{
275	struct vb2_dma_sg_buf *buf = buf_priv;
276
277	return &buf->sg_desc;
278	}
279
280	const struct vb2_mem_ops vb2_dma_sg_memops = {
281	.alloc = vb2_dma_sg_alloc,
282	.put = vb2_dma_sg_put,
283	.get_userptr = vb2_dma_sg_get_userptr,
284	.put_userptr = vb2_dma_sg_put_userptr,
285	.vaddr = vb2_dma_sg_vaddr,
286	.mmap = vb2_dma_sg_mmap,
287	.num_users = vb2_dma_sg_num_users,
288	.cookie = vb2_dma_sg_cookie,
289	};
290	EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);
291
292	MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
293	MODULE_AUTHOR("Andrzej Pietrasiewicz");
294	MODULE_LICENSE("GPL");