[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-v4l2.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 {
	struct device			*dev;
	void				*vaddr;
	struct page			**pages;
	struct frame_vector		*vec;
	int				offset;
	enum dma_data_direction		dma_dir;
	struct sg_table			sg_table;
	/*
	 * This will point to sg_table when used with the MMAP or USERPTR
	 * memory model, and to the dma_buf sglist when used with the
	 * DMABUF memory model.
	 */
	struct sg_table			*dma_sgt;
	size_t				size;
	unsigned int			num_pages;
	atomic_t			refcount;
	struct vb2_vmarea_handler	handler;

	struct dma_buf_attachment	*db_attach;
};

static void vb2_dma_sg_put(void *buf_priv);

static int vb2_dma_sg_alloc_compacted(struct vb2_dma_sg_buf *buf,
		gfp_t gfp_flags)
{
	unsigned int last_page = 0;
	int size = buf->size;

	while (size > 0) {
		struct page *pages;
		int order;
		int i;

		order = get_order(size);
		/* Dont over allocate*/
		if ((PAGE_SIZE << order) > size)
			order--;

		pages = NULL;
		while (!pages) {
			pages = alloc_pages(GFP_KERNEL | __GFP_ZERO |
					__GFP_NOWARN | gfp_flags, order);
			if (pages)
				break;

			if (order == 0) {
				while (last_page--)
					__free_page(buf->pages[last_page]);
				return -ENOMEM;
			}
			order--;
		}

		split_page(pages, order);
		for (i = 0; i < (1 << order); i++)
			buf->pages[last_page++] = &pages[i];

		size -= PAGE_SIZE << order;
	}

	return 0;
}

static void *vb2_dma_sg_alloc(struct device *dev, unsigned long dma_attrs,
			      unsigned long size, enum dma_data_direction dma_dir,
			      gfp_t gfp_flags)
{
	struct vb2_dma_sg_buf *buf;
	struct sg_table *sgt;
	int ret;
	int num_pages;

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

	buf->vaddr = NULL;
	buf->dma_dir = dma_dir;
	buf->offset = 0;
	buf->size = size;
	/* size is already page aligned */
	buf->num_pages = size >> PAGE_SHIFT;
	buf->dma_sgt = &buf->sg_table;

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

	ret = vb2_dma_sg_alloc_compacted(buf, gfp_flags);
	if (ret)
		goto fail_pages_alloc;

	ret = sg_alloc_table_from_pages(buf->dma_sgt, buf->pages,
			buf->num_pages, 0, size, GFP_KERNEL);
	if (ret)
		goto fail_table_alloc;

	/* Prevent the device from being released while the buffer is used */
	buf->dev = get_device(dev);

	sgt = &buf->sg_table;
	/*
	 * No need to sync to the device, this will happen later when the
	 * prepare() memop is called.
	 */
	sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
				      buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC);
	if (!sgt->nents)
		goto fail_map;

	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->num_pages);
	return buf;

fail_map:
	put_device(buf->dev);
	sg_free_table(buf->dma_sgt);
fail_table_alloc:
	num_pages = buf->num_pages;
	while (num_pages--)
		__free_page(buf->pages[num_pages]);
fail_pages_alloc:
	kfree(buf->pages);
fail_pages_array_alloc:
	kfree(buf);
	return NULL;
}

static void vb2_dma_sg_put(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	struct sg_table *sgt = &buf->sg_table;
	int i = buf->num_pages;

	if (atomic_dec_and_test(&buf->refcount)) {
		dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
			buf->num_pages);
		dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
				   buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC);
		if (buf->vaddr)
			vm_unmap_ram(buf->vaddr, buf->num_pages);
		sg_free_table(buf->dma_sgt);
		while (--i >= 0)
			__free_page(buf->pages[i]);
		kfree(buf->pages);
		put_device(buf->dev);
		kfree(buf);
	}
}

static void vb2_dma_sg_prepare(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	struct sg_table *sgt = buf->dma_sgt;

	/* DMABUF exporter will flush the cache for us */
	if (buf->db_attach)
		return;

	dma_sync_sg_for_device(buf->dev, sgt->sgl, sgt->orig_nents,
			       buf->dma_dir);
}

static void vb2_dma_sg_finish(void *buf_priv)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	struct sg_table *sgt = buf->dma_sgt;

	/* DMABUF exporter will flush the cache for us */
	if (buf->db_attach)
		return;

	dma_sync_sg_for_cpu(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir);
}

static void *vb2_dma_sg_get_userptr(struct device *dev, unsigned long vaddr,
				    unsigned long size,
				    enum dma_data_direction dma_dir)
{
	struct vb2_dma_sg_buf *buf;
	struct sg_table *sgt;
	struct frame_vector *vec;

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

	buf->vaddr = NULL;
	buf->dev = dev;
	buf->dma_dir = dma_dir;
	buf->offset = vaddr & ~PAGE_MASK;
	buf->size = size;
	buf->dma_sgt = &buf->sg_table;
	vec = vb2_create_framevec(vaddr, size, buf->dma_dir == DMA_FROM_DEVICE);
	if (IS_ERR(vec))
		goto userptr_fail_pfnvec;
	buf->vec = vec;

	buf->pages = frame_vector_pages(vec);
	if (IS_ERR(buf->pages))
		goto userptr_fail_sgtable;
	buf->num_pages = frame_vector_count(vec);

	if (sg_alloc_table_from_pages(buf->dma_sgt, buf->pages,
			buf->num_pages, buf->offset, size, 0))
		goto userptr_fail_sgtable;

	sgt = &buf->sg_table;
	/*
	 * No need to sync to the device, this will happen later when the
	 * prepare() memop is called.
	 */
	sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
				      buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC);
	if (!sgt->nents)
		goto userptr_fail_map;

	return buf;

userptr_fail_map:
	sg_free_table(&buf->sg_table);
userptr_fail_sgtable:
	vb2_destroy_framevec(vec);
userptr_fail_pfnvec:
	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;
	struct sg_table *sgt = &buf->sg_table;
	int i = buf->num_pages;

	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
	       __func__, buf->num_pages);
	dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir,
			   DMA_ATTR_SKIP_CPU_SYNC);
	if (buf->vaddr)
		vm_unmap_ram(buf->vaddr, buf->num_pages);
	sg_free_table(buf->dma_sgt);
	while (--i >= 0) {
		if (buf->dma_dir == DMA_FROM_DEVICE)
			set_page_dirty_lock(buf->pages[i]);
	}
	vb2_destroy_framevec(buf->vec);
	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) {
		if (buf->db_attach)
			buf->vaddr = dma_buf_vmap(buf->db_attach->dmabuf);
		else
			buf->vaddr = vm_map_ram(buf->pages,
					buf->num_pages, -1, PAGE_KERNEL);
	}

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

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;
}

/*********************************************/
/*         DMABUF ops for exporters          */
/*********************************************/

struct vb2_dma_sg_attachment {
	struct sg_table sgt;
	enum dma_data_direction dma_dir;
};

static int vb2_dma_sg_dmabuf_ops_attach(struct dma_buf *dbuf, struct device *dev,
	struct dma_buf_attachment *dbuf_attach)
{
	struct vb2_dma_sg_attachment *attach;
	unsigned int i;
	struct scatterlist *rd, *wr;
	struct sg_table *sgt;
	struct vb2_dma_sg_buf *buf = dbuf->priv;
	int ret;

	attach = kzalloc(sizeof(*attach), GFP_KERNEL);
	if (!attach)
		return -ENOMEM;

	sgt = &attach->sgt;
	/* Copy the buf->base_sgt scatter list to the attachment, as we can't
	 * map the same scatter list to multiple attachments at the same time.
	 */
	ret = sg_alloc_table(sgt, buf->dma_sgt->orig_nents, GFP_KERNEL);
	if (ret) {
		kfree(attach);
		return -ENOMEM;
	}

	rd = buf->dma_sgt->sgl;
	wr = sgt->sgl;
	for (i = 0; i < sgt->orig_nents; ++i) {
		sg_set_page(wr, sg_page(rd), rd->length, rd->offset);
		rd = sg_next(rd);
		wr = sg_next(wr);
	}

	attach->dma_dir = DMA_NONE;
	dbuf_attach->priv = attach;

	return 0;
}

static void vb2_dma_sg_dmabuf_ops_detach(struct dma_buf *dbuf,
	struct dma_buf_attachment *db_attach)
{
	struct vb2_dma_sg_attachment *attach = db_attach->priv;
	struct sg_table *sgt;

	if (!attach)
		return;

	sgt = &attach->sgt;

	/* release the scatterlist cache */
	if (attach->dma_dir != DMA_NONE)
		dma_unmap_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
			attach->dma_dir);
	sg_free_table(sgt);
	kfree(attach);
	db_attach->priv = NULL;
}

static struct sg_table *vb2_dma_sg_dmabuf_ops_map(
	struct dma_buf_attachment *db_attach, enum dma_data_direction dma_dir)
{
	struct vb2_dma_sg_attachment *attach = db_attach->priv;
	/* stealing dmabuf mutex to serialize map/unmap operations */
	struct mutex *lock = &db_attach->dmabuf->lock;
	struct sg_table *sgt;

	mutex_lock(lock);

	sgt = &attach->sgt;
	/* return previously mapped sg table */
	if (attach->dma_dir == dma_dir) {
		mutex_unlock(lock);
		return sgt;
	}

	/* release any previous cache */
	if (attach->dma_dir != DMA_NONE) {
		dma_unmap_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
			attach->dma_dir);
		attach->dma_dir = DMA_NONE;
	}

	/* mapping to the client with new direction */
	sgt->nents = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
				dma_dir);
	if (!sgt->nents) {
		pr_err("failed to map scatterlist\n");
		mutex_unlock(lock);
		return ERR_PTR(-EIO);
	}

	attach->dma_dir = dma_dir;

	mutex_unlock(lock);

	return sgt;
}

static void vb2_dma_sg_dmabuf_ops_unmap(struct dma_buf_attachment *db_attach,
	struct sg_table *sgt, enum dma_data_direction dma_dir)
{
	/* nothing to be done here */
}

static void vb2_dma_sg_dmabuf_ops_release(struct dma_buf *dbuf)
{
	/* drop reference obtained in vb2_dma_sg_get_dmabuf */
	vb2_dma_sg_put(dbuf->priv);
}

static void *vb2_dma_sg_dmabuf_ops_kmap(struct dma_buf *dbuf, unsigned long pgnum)
{
	struct vb2_dma_sg_buf *buf = dbuf->priv;

	return buf->vaddr ? buf->vaddr + pgnum * PAGE_SIZE : NULL;
}

static void *vb2_dma_sg_dmabuf_ops_vmap(struct dma_buf *dbuf)
{
	struct vb2_dma_sg_buf *buf = dbuf->priv;

	return vb2_dma_sg_vaddr(buf);
}

static int vb2_dma_sg_dmabuf_ops_mmap(struct dma_buf *dbuf,
	struct vm_area_struct *vma)
{
	return vb2_dma_sg_mmap(dbuf->priv, vma);
}

static struct dma_buf_ops vb2_dma_sg_dmabuf_ops = {
	.attach = vb2_dma_sg_dmabuf_ops_attach,
	.detach = vb2_dma_sg_dmabuf_ops_detach,
	.map_dma_buf = vb2_dma_sg_dmabuf_ops_map,
	.unmap_dma_buf = vb2_dma_sg_dmabuf_ops_unmap,
	.kmap = vb2_dma_sg_dmabuf_ops_kmap,
	.kmap_atomic = vb2_dma_sg_dmabuf_ops_kmap,
	.vmap = vb2_dma_sg_dmabuf_ops_vmap,
	.mmap = vb2_dma_sg_dmabuf_ops_mmap,
	.release = vb2_dma_sg_dmabuf_ops_release,
};

static struct dma_buf *vb2_dma_sg_get_dmabuf(void *buf_priv, unsigned long flags)
{
	struct vb2_dma_sg_buf *buf = buf_priv;
	struct dma_buf *dbuf;
	DEFINE_DMA_BUF_EXPORT_INFO(exp_info);

	exp_info.ops = &vb2_dma_sg_dmabuf_ops;
	exp_info.size = buf->size;
	exp_info.flags = flags;
	exp_info.priv = buf;

	if (WARN_ON(!buf->dma_sgt))
		return NULL;

	dbuf = dma_buf_export(&exp_info);
	if (IS_ERR(dbuf))
		return NULL;

	/* dmabuf keeps reference to vb2 buffer */
	atomic_inc(&buf->refcount);

	return dbuf;
}

/*********************************************/
/*       callbacks for DMABUF buffers        */
/*********************************************/

static int vb2_dma_sg_map_dmabuf(void *mem_priv)
{
	struct vb2_dma_sg_buf *buf = mem_priv;
	struct sg_table *sgt;

	if (WARN_ON(!buf->db_attach)) {
		pr_err("trying to pin a non attached buffer\n");
		return -EINVAL;
	}

	if (WARN_ON(buf->dma_sgt)) {
		pr_err("dmabuf buffer is already pinned\n");
		return 0;
	}

	/* get the associated scatterlist for this buffer */
	sgt = dma_buf_map_attachment(buf->db_attach, buf->dma_dir);
	if (IS_ERR(sgt)) {
		pr_err("Error getting dmabuf scatterlist\n");
		return -EINVAL;
	}

	buf->dma_sgt = sgt;
	buf->vaddr = NULL;

	return 0;
}

static void vb2_dma_sg_unmap_dmabuf(void *mem_priv)
{
	struct vb2_dma_sg_buf *buf = mem_priv;
	struct sg_table *sgt = buf->dma_sgt;

	if (WARN_ON(!buf->db_attach)) {
		pr_err("trying to unpin a not attached buffer\n");
		return;
	}

	if (WARN_ON(!sgt)) {
		pr_err("dmabuf buffer is already unpinned\n");
		return;
	}

	if (buf->vaddr) {
		dma_buf_vunmap(buf->db_attach->dmabuf, buf->vaddr);
		buf->vaddr = NULL;
	}
	dma_buf_unmap_attachment(buf->db_attach, sgt, buf->dma_dir);

	buf->dma_sgt = NULL;
}

static void vb2_dma_sg_detach_dmabuf(void *mem_priv)
{
	struct vb2_dma_sg_buf *buf = mem_priv;

	/* if vb2 works correctly you should never detach mapped buffer */
	if (WARN_ON(buf->dma_sgt))
		vb2_dma_sg_unmap_dmabuf(buf);

	/* detach this attachment */
	dma_buf_detach(buf->db_attach->dmabuf, buf->db_attach);
	kfree(buf);
}

static void *vb2_dma_sg_attach_dmabuf(struct device *dev, struct dma_buf *dbuf,
	unsigned long size, enum dma_data_direction dma_dir)
{
	struct vb2_dma_sg_buf *buf;
	struct dma_buf_attachment *dba;

	if (dbuf->size < size)
		return ERR_PTR(-EFAULT);

	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
	if (!buf)
		return ERR_PTR(-ENOMEM);

	buf->dev = dev;
	/* create attachment for the dmabuf with the user device */
	dba = dma_buf_attach(dbuf, buf->dev);
	if (IS_ERR(dba)) {
		pr_err("failed to attach dmabuf\n");
		kfree(buf);
		return dba;
	}

	buf->dma_dir = dma_dir;
	buf->size = size;
	buf->db_attach = dba;

	return buf;
}

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

	return buf->dma_sgt;
}

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,
	.prepare	= vb2_dma_sg_prepare,
	.finish		= vb2_dma_sg_finish,
	.vaddr		= vb2_dma_sg_vaddr,
	.mmap		= vb2_dma_sg_mmap,
	.num_users	= vb2_dma_sg_num_users,
	.get_dmabuf	= vb2_dma_sg_get_dmabuf,
	.map_dmabuf	= vb2_dma_sg_map_dmabuf,
	.unmap_dmabuf	= vb2_dma_sg_unmap_dmabuf,
	.attach_dmabuf	= vb2_dma_sg_attach_dmabuf,
	.detach_dmabuf	= vb2_dma_sg_detach_dmabuf,
	.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
c139990e	20	#include <media/videobuf2-v4l2.h>
5ba3f757 AP	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	33	struct vb2_dma_sg_buf {
0c3a14c1	34	struct device *dev;
5ba3f757 AP	35	void *vaddr;
5ba3f757 AP	36	struct page **pages;
3336c24f	37	struct frame_vector *vec;
5ba3f757	38	int offset;
cd474037	39	enum dma_data_direction dma_dir;
22301247	40	struct sg_table sg_table;
e078b79d HV	41	/*
	42	* This will point to sg_table when used with the MMAP or USERPTR
	43	* memory model, and to the dma_buf sglist when used with the
	44	* DMABUF memory model.
	45	*/
	46	struct sg_table *dma_sgt;
22301247 RR	47	size_t size;
22301247 RR	48	unsigned int num_pages;
5ba3f757 AP	49	atomic_t refcount;
5ba3f757 AP	50	struct vb2_vmarea_handler handler;
e078b79d HV	51
e078b79d HV	52	struct dma_buf_attachment *db_attach;
5ba3f757 AP	53	};
	54
	55	static void vb2_dma_sg_put(void *buf_priv);
	56
df237281 RR	57	static int vb2_dma_sg_alloc_compacted(struct vb2_dma_sg_buf *buf,
	58	gfp_t gfp_flags)
	59	{
	60	unsigned int last_page = 0;
22301247	61	int size = buf->size;
df237281 RR	62
	63	while (size > 0) {
	64	struct page *pages;
	65	int order;
	66	int i;
	67
	68	order = get_order(size);
	69	/* Dont over allocate*/
	70	if ((PAGE_SIZE << order) > size)
	71	order--;
	72
	73	pages = NULL;
	74	while (!pages) {
	75	pages = alloc_pages(GFP_KERNEL \| __GFP_ZERO \|
	76	__GFP_NOWARN \| gfp_flags, order);
	77	if (pages)
	78	break;
	79
	80	if (order == 0) {
	81	while (last_page--)
	82	__free_page(buf->pages[last_page]);
	83	return -ENOMEM;
	84	}
	85	order--;
	86	}
	87
	88	split_page(pages, order);
22301247 RR	89	for (i = 0; i < (1 << order); i++)
22301247 RR	90	buf->pages[last_page++] = &pages[i];
df237281 RR	91
	92	size -= PAGE_SIZE << order;
	93	}
	94
	95	return 0;
	96	}
	97
00085f1e	98	static void vb2_dma_sg_alloc(struct device dev, unsigned long dma_attrs,
d16e832d HV	99	unsigned long size, enum dma_data_direction dma_dir,
d16e832d HV	100	gfp_t gfp_flags)
5ba3f757 AP	101	{
5ba3f757 AP	102	struct vb2_dma_sg_buf *buf;
d790b7ed	103	struct sg_table *sgt;
df237281	104	int ret;
22301247	105	int num_pages;
5ba3f757	106
36c0f8b3	107	if (WARN_ON(dev == NULL))
0c3a14c1	108	return NULL;
5ba3f757 AP	109	buf = kzalloc(sizeof *buf, GFP_KERNEL);
	110	if (!buf)
	111	return NULL;
	112
	113	buf->vaddr = NULL;
d935c57e	114	buf->dma_dir = dma_dir;
5ba3f757	115	buf->offset = 0;
22301247	116	buf->size = size;
7f841459	117	/* size is already page aligned */
22301247	118	buf->num_pages = size >> PAGE_SHIFT;
e078b79d	119	buf->dma_sgt = &buf->sg_table;
5ba3f757	120
22301247	121	buf->pages = kzalloc(buf->num_pages * sizeof(struct page *),
5ba3f757 AP	122	GFP_KERNEL);
	123	if (!buf->pages)
	124	goto fail_pages_array_alloc;
	125
df237281 RR	126	ret = vb2_dma_sg_alloc_compacted(buf, gfp_flags);
	127	if (ret)
	128	goto fail_pages_alloc;
5ba3f757	129
e078b79d	130	ret = sg_alloc_table_from_pages(buf->dma_sgt, buf->pages,
47bc59c5	131	buf->num_pages, 0, size, GFP_KERNEL);
22301247 RR	132	if (ret)
	133	goto fail_table_alloc;
	134
0c3a14c1	135	/* Prevent the device from being released while the buffer is used */
36c0f8b3	136	buf->dev = get_device(dev);
d790b7ed HV	137
d790b7ed HV	138	sgt = &buf->sg_table;
251a79f8 HV	139	/*
	140	* No need to sync to the device, this will happen later when the
	141	* prepare() memop is called.
	142	*/
6a5d77cb	143	sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
00085f1e	144	buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC);
6a5d77cb	145	if (!sgt->nents)
d790b7ed	146	goto fail_map;
d790b7ed	147
5ba3f757 AP	148	buf->handler.refcount = &buf->refcount;
	149	buf->handler.put = vb2_dma_sg_put;
	150	buf->handler.arg = buf;
	151
	152	atomic_inc(&buf->refcount);
	153
ffdc78ef	154	dprintk(1, "%s: Allocated buffer of %d pages\n",
22301247	155	__func__, buf->num_pages);
5ba3f757 AP	156	return buf;
5ba3f757 AP	157
d790b7ed HV	158	fail_map:
d790b7ed HV	159	put_device(buf->dev);
e078b79d	160	sg_free_table(buf->dma_sgt);
22301247 RR	161	fail_table_alloc:
	162	num_pages = buf->num_pages;
	163	while (num_pages--)
	164	__free_page(buf->pages[num_pages]);
5ba3f757	165	fail_pages_alloc:
a9bb36aa	166	kfree(buf->pages);
5ba3f757	167	fail_pages_array_alloc:
5ba3f757 AP	168	kfree(buf);
	169	return NULL;
	170	}
	171
	172	static void vb2_dma_sg_put(void *buf_priv)
	173	{
	174	struct vb2_dma_sg_buf *buf = buf_priv;
d790b7ed	175	struct sg_table *sgt = &buf->sg_table;
22301247	176	int i = buf->num_pages;
5ba3f757 AP	177
5ba3f757 AP	178	if (atomic_dec_and_test(&buf->refcount)) {
ffdc78ef	179	dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
22301247	180	buf->num_pages);
6a5d77cb	181	dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
00085f1e	182	buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC);
5ba3f757	183	if (buf->vaddr)
22301247	184	vm_unmap_ram(buf->vaddr, buf->num_pages);
e078b79d	185	sg_free_table(buf->dma_sgt);
5ba3f757 AP	186	while (--i >= 0)
	187	__free_page(buf->pages[i]);
	188	kfree(buf->pages);
0c3a14c1	189	put_device(buf->dev);
5ba3f757 AP	190	kfree(buf);
	191	}
	192	}
	193
d790b7ed HV	194	static void vb2_dma_sg_prepare(void *buf_priv)
	195	{
	196	struct vb2_dma_sg_buf *buf = buf_priv;
e078b79d HV	197	struct sg_table *sgt = buf->dma_sgt;
	198
	199	/* DMABUF exporter will flush the cache for us */
	200	if (buf->db_attach)
	201	return;
d790b7ed	202
418dae22 TL	203	dma_sync_sg_for_device(buf->dev, sgt->sgl, sgt->orig_nents,
418dae22 TL	204	buf->dma_dir);
d790b7ed HV	205	}
	206
	207	static void vb2_dma_sg_finish(void *buf_priv)
	208	{
	209	struct vb2_dma_sg_buf *buf = buf_priv;
e078b79d HV	210	struct sg_table *sgt = buf->dma_sgt;
	211
	212	/* DMABUF exporter will flush the cache for us */
	213	if (buf->db_attach)
	214	return;
d790b7ed	215
418dae22	216	dma_sync_sg_for_cpu(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir);
d790b7ed HV	217	}
d790b7ed HV	218
36c0f8b3	219	static void vb2_dma_sg_get_userptr(struct device dev, unsigned long vaddr,
cd474037 HV	220	unsigned long size,
cd474037 HV	221	enum dma_data_direction dma_dir)
5ba3f757 AP	222	{
5ba3f757 AP	223	struct vb2_dma_sg_buf *buf;
d790b7ed	224	struct sg_table *sgt;
3336c24f	225	struct frame_vector *vec;
251a79f8	226
5ba3f757 AP	227	buf = kzalloc(sizeof *buf, GFP_KERNEL);
	228	if (!buf)
	229	return NULL;
	230
	231	buf->vaddr = NULL;
36c0f8b3	232	buf->dev = dev;
cd474037	233	buf->dma_dir = dma_dir;
5ba3f757	234	buf->offset = vaddr & ~PAGE_MASK;
22301247	235	buf->size = size;
e078b79d	236	buf->dma_sgt = &buf->sg_table;
3336c24f JK	237	vec = vb2_create_framevec(vaddr, size, buf->dma_dir == DMA_FROM_DEVICE);
	238	if (IS_ERR(vec))
	239	goto userptr_fail_pfnvec;
	240	buf->vec = vec;
5ba3f757	241
3336c24f JK	242	buf->pages = frame_vector_pages(vec);
	243	if (IS_ERR(buf->pages))
	244	goto userptr_fail_sgtable;
	245	buf->num_pages = frame_vector_count(vec);
5ba3f757	246
e078b79d	247	if (sg_alloc_table_from_pages(buf->dma_sgt, buf->pages,
22301247	248	buf->num_pages, buf->offset, size, 0))
3336c24f	249	goto userptr_fail_sgtable;
22301247	250
d790b7ed	251	sgt = &buf->sg_table;
251a79f8 HV	252	/*
	253	* No need to sync to the device, this will happen later when the
	254	* prepare() memop is called.
	255	*/
6a5d77cb	256	sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
00085f1e	257	buf->dma_dir, DMA_ATTR_SKIP_CPU_SYNC);
6a5d77cb	258	if (!sgt->nents)
d790b7ed	259	goto userptr_fail_map;
6a5d77cb	260
5ba3f757 AP	261	return buf;
5ba3f757 AP	262
d790b7ed HV	263	userptr_fail_map:
d790b7ed HV	264	sg_free_table(&buf->sg_table);
3336c24f JK	265	userptr_fail_sgtable:
	266	vb2_destroy_framevec(vec);
	267	userptr_fail_pfnvec:
5ba3f757 AP	268	kfree(buf);
	269	return NULL;
	270	}
	271
	272	/*
	273	* @put_userptr: inform the allocator that a USERPTR buffer will no longer
	274	* be used
	275	*/
	276	static void vb2_dma_sg_put_userptr(void *buf_priv)
	277	{
	278	struct vb2_dma_sg_buf *buf = buf_priv;
d790b7ed	279	struct sg_table *sgt = &buf->sg_table;
22301247	280	int i = buf->num_pages;
5ba3f757	281
ffdc78ef	282	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
22301247	283	__func__, buf->num_pages);
6a5d77cb	284	dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir,
00085f1e	285	DMA_ATTR_SKIP_CPU_SYNC);
5ba3f757	286	if (buf->vaddr)
22301247	287	vm_unmap_ram(buf->vaddr, buf->num_pages);
e078b79d	288	sg_free_table(buf->dma_sgt);
5ba3f757	289	while (--i >= 0) {
cd474037	290	if (buf->dma_dir == DMA_FROM_DEVICE)
5ba3f757	291	set_page_dirty_lock(buf->pages[i]);
5ba3f757	292	}
3336c24f	293	vb2_destroy_framevec(buf->vec);
5ba3f757 AP	294	kfree(buf);
	295	}
	296
	297	static void vb2_dma_sg_vaddr(void buf_priv)
	298	{
	299	struct vb2_dma_sg_buf *buf = buf_priv;
	300
	301	BUG_ON(!buf);
	302
e078b79d HV	303	if (!buf->vaddr) {
	304	if (buf->db_attach)
	305	buf->vaddr = dma_buf_vmap(buf->db_attach->dmabuf);
	306	else
	307	buf->vaddr = vm_map_ram(buf->pages,
	308	buf->num_pages, -1, PAGE_KERNEL);
	309	}
5ba3f757 AP	310
5ba3f757 AP	311	/* add offset in case userptr is not page-aligned */
e078b79d	312	return buf->vaddr ? buf->vaddr + buf->offset : NULL;
5ba3f757 AP	313	}
	314
	315	static unsigned int vb2_dma_sg_num_users(void *buf_priv)
	316	{
	317	struct vb2_dma_sg_buf *buf = buf_priv;
	318
	319	return atomic_read(&buf->refcount);
	320	}
	321
	322	static int vb2_dma_sg_mmap(void buf_priv, struct vm_area_struct vma)
	323	{
	324	struct vb2_dma_sg_buf *buf = buf_priv;
	325	unsigned long uaddr = vma->vm_start;
	326	unsigned long usize = vma->vm_end - vma->vm_start;
	327	int i = 0;
	328
	329	if (!buf) {
	330	printk(KERN_ERR "No memory to map\n");
	331	return -EINVAL;
	332	}
	333
	334	do {
	335	int ret;
	336
	337	ret = vm_insert_page(vma, uaddr, buf->pages[i++]);
	338	if (ret) {
	339	printk(KERN_ERR "Remapping memory, error: %d\n", ret);
	340	return ret;
	341	}
	342
	343	uaddr += PAGE_SIZE;
	344	usize -= PAGE_SIZE;
	345	} while (usize > 0);
	346
	347
	348	/*
	349	* Use common vm_area operations to track buffer refcount.
	350	*/
	351	vma->vm_private_data = &buf->handler;
	352	vma->vm_ops = &vb2_common_vm_ops;
	353
	354	vma->vm_ops->open(vma);
	355
	356	return 0;
	357	}
	358
041c7b6a HV	359	/*********************************************/
	360	/* DMABUF ops for exporters */
	361	/*********************************************/
	362
	363	struct vb2_dma_sg_attachment {
	364	struct sg_table sgt;
	365	enum dma_data_direction dma_dir;
	366	};
	367
	368	static int vb2_dma_sg_dmabuf_ops_attach(struct dma_buf dbuf, struct device dev,
	369	struct dma_buf_attachment *dbuf_attach)
	370	{
	371	struct vb2_dma_sg_attachment *attach;
	372	unsigned int i;
	373	struct scatterlist rd, wr;
	374	struct sg_table *sgt;
	375	struct vb2_dma_sg_buf *buf = dbuf->priv;
	376	int ret;
	377
	378	attach = kzalloc(sizeof(*attach), GFP_KERNEL);
	379	if (!attach)
	380	return -ENOMEM;
	381
	382	sgt = &attach->sgt;
	383	/* Copy the buf->base_sgt scatter list to the attachment, as we can't
	384	* map the same scatter list to multiple attachments at the same time.
	385	*/
	386	ret = sg_alloc_table(sgt, buf->dma_sgt->orig_nents, GFP_KERNEL);
	387	if (ret) {
	388	kfree(attach);
	389	return -ENOMEM;
	390	}
	391
	392	rd = buf->dma_sgt->sgl;
	393	wr = sgt->sgl;
	394	for (i = 0; i < sgt->orig_nents; ++i) {
	395	sg_set_page(wr, sg_page(rd), rd->length, rd->offset);
	396	rd = sg_next(rd);
	397	wr = sg_next(wr);
	398	}
	399
	400	attach->dma_dir = DMA_NONE;
	401	dbuf_attach->priv = attach;
	402
	403	return 0;
	404	}
	405
	406	static void vb2_dma_sg_dmabuf_ops_detach(struct dma_buf *dbuf,
	407	struct dma_buf_attachment *db_attach)
	408	{
	409	struct vb2_dma_sg_attachment *attach = db_attach->priv;
	410	struct sg_table *sgt;
	411
	412	if (!attach)
	413	return;
	414
	415	sgt = &attach->sgt;
	416
	417	/* release the scatterlist cache */
	418	if (attach->dma_dir != DMA_NONE)
	419	dma_unmap_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
	420	attach->dma_dir);
	421	sg_free_table(sgt);
	422	kfree(attach);
423	db_attach->priv = NULL;
424	}
425
426	static struct sg_table *vb2_dma_sg_dmabuf_ops_map(
427	struct dma_buf_attachment *db_attach, enum dma_data_direction dma_dir)
428	{
429	struct vb2_dma_sg_attachment *attach = db_attach->priv;
430	/* stealing dmabuf mutex to serialize map/unmap operations */
431	struct mutex *lock = &db_attach->dmabuf->lock;
432	struct sg_table *sgt;
041c7b6a HV	433
	434	mutex_lock(lock);
	435
	436	sgt = &attach->sgt;
	437	/* return previously mapped sg table */
	438	if (attach->dma_dir == dma_dir) {
	439	mutex_unlock(lock);
	440	return sgt;
	441	}
	442
	443	/* release any previous cache */
	444	if (attach->dma_dir != DMA_NONE) {
	445	dma_unmap_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
	446	attach->dma_dir);
	447	attach->dma_dir = DMA_NONE;
	448	}
	449
	450	/* mapping to the client with new direction */
6a5d77cb RR	451	sgt->nents = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
	452	dma_dir);
	453	if (!sgt->nents) {
041c7b6a HV	454	pr_err("failed to map scatterlist\n");
	455	mutex_unlock(lock);
	456	return ERR_PTR(-EIO);
	457	}
	458
	459	attach->dma_dir = dma_dir;
	460
	461	mutex_unlock(lock);
	462
	463	return sgt;
	464	}
	465
	466	static void vb2_dma_sg_dmabuf_ops_unmap(struct dma_buf_attachment *db_attach,
	467	struct sg_table *sgt, enum dma_data_direction dma_dir)
	468	{
	469	/* nothing to be done here */
	470	}
	471
	472	static void vb2_dma_sg_dmabuf_ops_release(struct dma_buf *dbuf)
	473	{
	474	/* drop reference obtained in vb2_dma_sg_get_dmabuf */
	475	vb2_dma_sg_put(dbuf->priv);
	476	}
	477
	478	static void vb2_dma_sg_dmabuf_ops_kmap(struct dma_buf dbuf, unsigned long pgnum)
	479	{
	480	struct vb2_dma_sg_buf *buf = dbuf->priv;
	481
	482	return buf->vaddr ? buf->vaddr + pgnum * PAGE_SIZE : NULL;
	483	}
	484
	485	static void vb2_dma_sg_dmabuf_ops_vmap(struct dma_buf dbuf)
	486	{
	487	struct vb2_dma_sg_buf *buf = dbuf->priv;
	488
	489	return vb2_dma_sg_vaddr(buf);
	490	}
	491
	492	static int vb2_dma_sg_dmabuf_ops_mmap(struct dma_buf *dbuf,
	493	struct vm_area_struct *vma)
	494	{
	495	return vb2_dma_sg_mmap(dbuf->priv, vma);
	496	}
	497
	498	static struct dma_buf_ops vb2_dma_sg_dmabuf_ops = {
	499	.attach = vb2_dma_sg_dmabuf_ops_attach,
	500	.detach = vb2_dma_sg_dmabuf_ops_detach,
	501	.map_dma_buf = vb2_dma_sg_dmabuf_ops_map,
	502	.unmap_dma_buf = vb2_dma_sg_dmabuf_ops_unmap,
	503	.kmap = vb2_dma_sg_dmabuf_ops_kmap,
	504	.kmap_atomic = vb2_dma_sg_dmabuf_ops_kmap,
	505	.vmap = vb2_dma_sg_dmabuf_ops_vmap,
	506	.mmap = vb2_dma_sg_dmabuf_ops_mmap,
	507	.release = vb2_dma_sg_dmabuf_ops_release,
	508	};
	509
	510	static struct dma_buf vb2_dma_sg_get_dmabuf(void buf_priv, unsigned long flags)
	511	{
	512	struct vb2_dma_sg_buf *buf = buf_priv;
	513	struct dma_buf *dbuf;
d8fbe341 SS	514	DEFINE_DMA_BUF_EXPORT_INFO(exp_info);
	515
	516	exp_info.ops = &vb2_dma_sg_dmabuf_ops;
	517	exp_info.size = buf->size;
	518	exp_info.flags = flags;
	519	exp_info.priv = buf;
041c7b6a HV	520
	521	if (WARN_ON(!buf->dma_sgt))
	522	return NULL;
	523
d8fbe341	524	dbuf = dma_buf_export(&exp_info);
041c7b6a HV	525	if (IS_ERR(dbuf))
	526	return NULL;
	527
	528	/* dmabuf keeps reference to vb2 buffer */
	529	atomic_inc(&buf->refcount);
	530
	531	return dbuf;
	532	}
	533
e078b79d HV	534	/*********************************************/
	535	/* callbacks for DMABUF buffers */
	536	/*********************************************/
	537
	538	static int vb2_dma_sg_map_dmabuf(void *mem_priv)
	539	{
	540	struct vb2_dma_sg_buf *buf = mem_priv;
	541	struct sg_table *sgt;
	542
	543	if (WARN_ON(!buf->db_attach)) {
	544	pr_err("trying to pin a non attached buffer\n");
	545	return -EINVAL;
	546	}
	547
	548	if (WARN_ON(buf->dma_sgt)) {
	549	pr_err("dmabuf buffer is already pinned\n");
	550	return 0;
	551	}
	552
	553	/* get the associated scatterlist for this buffer */
	554	sgt = dma_buf_map_attachment(buf->db_attach, buf->dma_dir);
	555	if (IS_ERR(sgt)) {
	556	pr_err("Error getting dmabuf scatterlist\n");
	557	return -EINVAL;
	558	}
	559
	560	buf->dma_sgt = sgt;
	561	buf->vaddr = NULL;
	562
	563	return 0;
	564	}
	565
	566	static void vb2_dma_sg_unmap_dmabuf(void *mem_priv)
	567	{
	568	struct vb2_dma_sg_buf *buf = mem_priv;
	569	struct sg_table *sgt = buf->dma_sgt;
	570
	571	if (WARN_ON(!buf->db_attach)) {
	572	pr_err("trying to unpin a not attached buffer\n");
	573	return;
	574	}
	575
	576	if (WARN_ON(!sgt)) {
	577	pr_err("dmabuf buffer is already unpinned\n");
	578	return;
	579	}
	580
	581	if (buf->vaddr) {
	582	dma_buf_vunmap(buf->db_attach->dmabuf, buf->vaddr);
	583	buf->vaddr = NULL;
	584	}
	585	dma_buf_unmap_attachment(buf->db_attach, sgt, buf->dma_dir);
	586
	587	buf->dma_sgt = NULL;
	588	}
	589
	590	static void vb2_dma_sg_detach_dmabuf(void *mem_priv)
	591	{
	592	struct vb2_dma_sg_buf *buf = mem_priv;
	593
	594	/* if vb2 works correctly you should never detach mapped buffer */
	595	if (WARN_ON(buf->dma_sgt))
	596	vb2_dma_sg_unmap_dmabuf(buf);
	597
598	/* detach this attachment */
599	dma_buf_detach(buf->db_attach->dmabuf, buf->db_attach);
600	kfree(buf);
601	}
602
36c0f8b3	603	static void vb2_dma_sg_attach_dmabuf(struct device dev, struct dma_buf *dbuf,
e078b79d HV	604	unsigned long size, enum dma_data_direction dma_dir)
e078b79d HV	605	{
e078b79d HV	606	struct vb2_dma_sg_buf *buf;
	607	struct dma_buf_attachment *dba;
	608
	609	if (dbuf->size < size)
	610	return ERR_PTR(-EFAULT);
	611
	612	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
	613	if (!buf)
	614	return ERR_PTR(-ENOMEM);
	615
36c0f8b3	616	buf->dev = dev;
e078b79d HV	617	/* create attachment for the dmabuf with the user device */
	618	dba = dma_buf_attach(dbuf, buf->dev);
	619	if (IS_ERR(dba)) {
	620	pr_err("failed to attach dmabuf\n");
	621	kfree(buf);
	622	return dba;
	623	}
	624
	625	buf->dma_dir = dma_dir;
	626	buf->size = size;
	627	buf->db_attach = dba;
	628
	629	return buf;
	630	}
	631
5ba3f757 AP	632	static void vb2_dma_sg_cookie(void buf_priv)
	633	{
	634	struct vb2_dma_sg_buf *buf = buf_priv;
	635
e078b79d	636	return buf->dma_sgt;
5ba3f757 AP	637	}
	638
	639	const struct vb2_mem_ops vb2_dma_sg_memops = {
	640	.alloc = vb2_dma_sg_alloc,
	641	.put = vb2_dma_sg_put,
	642	.get_userptr = vb2_dma_sg_get_userptr,
	643	.put_userptr = vb2_dma_sg_put_userptr,
d790b7ed HV	644	.prepare = vb2_dma_sg_prepare,
d790b7ed HV	645	.finish = vb2_dma_sg_finish,
5ba3f757 AP	646	.vaddr = vb2_dma_sg_vaddr,
	647	.mmap = vb2_dma_sg_mmap,
	648	.num_users = vb2_dma_sg_num_users,
041c7b6a	649	.get_dmabuf = vb2_dma_sg_get_dmabuf,
e078b79d HV	650	.map_dmabuf = vb2_dma_sg_map_dmabuf,
	651	.unmap_dmabuf = vb2_dma_sg_unmap_dmabuf,
	652	.attach_dmabuf = vb2_dma_sg_attach_dmabuf,
	653	.detach_dmabuf = vb2_dma_sg_detach_dmabuf,
5ba3f757 AP	654	.cookie = vb2_dma_sg_cookie,
	655	};
	656	EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);
	657
	658	MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
	659	MODULE_AUTHOR("Andrzej Pietrasiewicz");
	660	MODULE_LICENSE("GPL");