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