1 /*
2  * helper functions for SG DMA video4linux capture buffers
3  *
4  * The functions expect the hardware being able to scatter gather
5  * (i.e. the buffers are not linear in physical memory, but fragmented
6  * into PAGE_SIZE chunks).  They also assume the driver does not need
7  * to touch the video data.
8  *
9  * (c) 2007 Mauro Carvalho Chehab, <mchehab@infradead.org>
10  *
11  * Highly based on video-buf written originally by:
12  * (c) 2001,02 Gerd Knorr <kraxel@bytesex.org>
13  * (c) 2006 Mauro Carvalho Chehab, <mchehab@infradead.org>
14  * (c) 2006 Ted Walther and John Sokol
15  *
16  * This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation; either version 2
19  */
20 
21 #include <linux/init.h>
22 #include <linux/module.h>
23 #include <linux/moduleparam.h>
24 #include <linux/sched.h>
25 #include <linux/slab.h>
26 #include <linux/interrupt.h>
27 
28 #include <linux/dma-mapping.h>
29 #include <linux/vmalloc.h>
30 #include <linux/pagemap.h>
31 #include <linux/scatterlist.h>
32 #include <asm/page.h>
33 #include <asm/pgtable.h>
34 
35 #include <media/videobuf-dma-sg.h>
36 
37 #define MAGIC_DMABUF 0x19721112
38 #define MAGIC_SG_MEM 0x17890714
39 
40 #define MAGIC_CHECK(is, should)						\
41 	if (unlikely((is) != (should))) {				\
42 		printk(KERN_ERR "magic mismatch: %x (expected %x)\n",	\
43 				is, should);				\
44 		BUG();							\
45 	}
46 
47 static int debug;
48 module_param(debug, int, 0644);
49 
50 MODULE_DESCRIPTION("helper module to manage video4linux dma sg buffers");
51 MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@infradead.org>");
52 MODULE_LICENSE("GPL");
53 
54 #define dprintk(level, fmt, arg...)					\
55 	if (debug >= level)						\
56 		printk(KERN_DEBUG "vbuf-sg: " fmt , ## arg)
57 
58 /* --------------------------------------------------------------------- */
59 
60 /*
61  * Return a scatterlist for some page-aligned vmalloc()'ed memory
62  * block (NULL on errors).  Memory for the scatterlist is allocated
63  * using kmalloc.  The caller must free the memory.
64  */
65 static struct scatterlist *videobuf_vmalloc_to_sg(unsigned char *virt,
66 						  int nr_pages)
67 {
68 	struct scatterlist *sglist;
69 	struct page *pg;
70 	int i;
71 
72 	sglist = vzalloc(nr_pages * sizeof(*sglist));
73 	if (NULL == sglist)
74 		return NULL;
75 	sg_init_table(sglist, nr_pages);
76 	for (i = 0; i < nr_pages; i++, virt += PAGE_SIZE) {
77 		pg = vmalloc_to_page(virt);
78 		if (NULL == pg)
79 			goto err;
80 		BUG_ON(PageHighMem(pg));
81 		sg_set_page(&sglist[i], pg, PAGE_SIZE, 0);
82 	}
83 	return sglist;
84 
85 err:
86 	vfree(sglist);
87 	return NULL;
88 }
89 
90 /*
91  * Return a scatterlist for a an array of userpages (NULL on errors).
92  * Memory for the scatterlist is allocated using kmalloc.  The caller
93  * must free the memory.
94  */
95 static struct scatterlist *videobuf_pages_to_sg(struct page **pages,
96 					int nr_pages, int offset, size_t size)
97 {
98 	struct scatterlist *sglist;
99 	int i;
100 
101 	if (NULL == pages[0])
102 		return NULL;
103 	sglist = vmalloc(nr_pages * sizeof(*sglist));
104 	if (NULL == sglist)
105 		return NULL;
106 	sg_init_table(sglist, nr_pages);
107 
108 	if (PageHighMem(pages[0]))
109 		/* DMA to highmem pages might not work */
110 		goto highmem;
111 	sg_set_page(&sglist[0], pages[0],
112 			min_t(size_t, PAGE_SIZE - offset, size), offset);
113 	size -= min_t(size_t, PAGE_SIZE - offset, size);
114 	for (i = 1; i < nr_pages; i++) {
115 		if (NULL == pages[i])
116 			goto nopage;
117 		if (PageHighMem(pages[i]))
118 			goto highmem;
119 		sg_set_page(&sglist[i], pages[i], min_t(size_t, PAGE_SIZE, size), 0);
120 		size -= min_t(size_t, PAGE_SIZE, size);
121 	}
122 	return sglist;
123 
124 nopage:
125 	dprintk(2, "sgl: oops - no page\n");
126 	vfree(sglist);
127 	return NULL;
128 
129 highmem:
130 	dprintk(2, "sgl: oops - highmem page\n");
131 	vfree(sglist);
132 	return NULL;
133 }
134 
135 /* --------------------------------------------------------------------- */
136 
137 struct videobuf_dmabuf *videobuf_to_dma(struct videobuf_buffer *buf)
138 {
139 	struct videobuf_dma_sg_memory *mem = buf->priv;
140 	BUG_ON(!mem);
141 
142 	MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
143 
144 	return &mem->dma;
145 }
146 EXPORT_SYMBOL_GPL(videobuf_to_dma);
147 
148 void videobuf_dma_init(struct videobuf_dmabuf *dma)
149 {
150 	memset(dma, 0, sizeof(*dma));
151 	dma->magic = MAGIC_DMABUF;
152 }
153 EXPORT_SYMBOL_GPL(videobuf_dma_init);
154 
155 static int videobuf_dma_init_user_locked(struct videobuf_dmabuf *dma,
156 			int direction, unsigned long data, unsigned long size)
157 {
158 	unsigned long first, last;
159 	int err, rw = 0;
160 
161 	dma->direction = direction;
162 	switch (dma->direction) {
163 	case DMA_FROM_DEVICE:
164 		rw = READ;
165 		break;
166 	case DMA_TO_DEVICE:
167 		rw = WRITE;
168 		break;
169 	default:
170 		BUG();
171 	}
172 
173 	first = (data          & PAGE_MASK) >> PAGE_SHIFT;
174 	last  = ((data+size-1) & PAGE_MASK) >> PAGE_SHIFT;
175 	dma->offset = data & ~PAGE_MASK;
176 	dma->size = size;
177 	dma->nr_pages = last-first+1;
178 	dma->pages = kmalloc(dma->nr_pages * sizeof(struct page *), GFP_KERNEL);
179 	if (NULL == dma->pages)
180 		return -ENOMEM;
181 
182 	dprintk(1, "init user [0x%lx+0x%lx => %d pages]\n",
183 		data, size, dma->nr_pages);
184 
185 	err = get_user_pages(current, current->mm,
186 			     data & PAGE_MASK, dma->nr_pages,
187 			     rw == READ, 1, /* force */
188 			     dma->pages, NULL);
189 
190 	if (err != dma->nr_pages) {
191 		dma->nr_pages = (err >= 0) ? err : 0;
192 		dprintk(1, "get_user_pages: err=%d [%d]\n", err, dma->nr_pages);
193 		return err < 0 ? err : -EINVAL;
194 	}
195 	return 0;
196 }
197 
198 int videobuf_dma_init_user(struct videobuf_dmabuf *dma, int direction,
199 			   unsigned long data, unsigned long size)
200 {
201 	int ret;
202 
203 	down_read(&current->mm->mmap_sem);
204 	ret = videobuf_dma_init_user_locked(dma, direction, data, size);
205 	up_read(&current->mm->mmap_sem);
206 
207 	return ret;
208 }
209 EXPORT_SYMBOL_GPL(videobuf_dma_init_user);
210 
211 int videobuf_dma_init_kernel(struct videobuf_dmabuf *dma, int direction,
212 			     int nr_pages)
213 {
214 	dprintk(1, "init kernel [%d pages]\n", nr_pages);
215 
216 	dma->direction = direction;
217 	dma->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
218 	if (NULL == dma->vaddr) {
219 		dprintk(1, "vmalloc_32(%d pages) failed\n", nr_pages);
220 		return -ENOMEM;
221 	}
222 
223 	dprintk(1, "vmalloc is at addr 0x%08lx, size=%d\n",
224 				(unsigned long)dma->vaddr,
225 				nr_pages << PAGE_SHIFT);
226 
227 	memset(dma->vaddr, 0, nr_pages << PAGE_SHIFT);
228 	dma->nr_pages = nr_pages;
229 
230 	return 0;
231 }
232 EXPORT_SYMBOL_GPL(videobuf_dma_init_kernel);
233 
234 int videobuf_dma_init_overlay(struct videobuf_dmabuf *dma, int direction,
235 			      dma_addr_t addr, int nr_pages)
236 {
237 	dprintk(1, "init overlay [%d pages @ bus 0x%lx]\n",
238 		nr_pages, (unsigned long)addr);
239 	dma->direction = direction;
240 
241 	if (0 == addr)
242 		return -EINVAL;
243 
244 	dma->bus_addr = addr;
245 	dma->nr_pages = nr_pages;
246 
247 	return 0;
248 }
249 EXPORT_SYMBOL_GPL(videobuf_dma_init_overlay);
250 
251 int videobuf_dma_map(struct device *dev, struct videobuf_dmabuf *dma)
252 {
253 	MAGIC_CHECK(dma->magic, MAGIC_DMABUF);
254 	BUG_ON(0 == dma->nr_pages);
255 
256 	if (dma->pages) {
257 		dma->sglist = videobuf_pages_to_sg(dma->pages, dma->nr_pages,
258 						   dma->offset, dma->size);
259 	}
260 	if (dma->vaddr) {
261 		dma->sglist = videobuf_vmalloc_to_sg(dma->vaddr,
262 						     dma->nr_pages);
263 	}
264 	if (dma->bus_addr) {
265 		dma->sglist = vmalloc(sizeof(*dma->sglist));
266 		if (NULL != dma->sglist) {
267 			dma->sglen = 1;
268 			sg_dma_address(&dma->sglist[0])	= dma->bus_addr
269 							& PAGE_MASK;
270 			dma->sglist[0].offset = dma->bus_addr & ~PAGE_MASK;
271 			sg_dma_len(&dma->sglist[0]) = dma->nr_pages * PAGE_SIZE;
272 		}
273 	}
274 	if (NULL == dma->sglist) {
275 		dprintk(1, "scatterlist is NULL\n");
276 		return -ENOMEM;
277 	}
278 	if (!dma->bus_addr) {
279 		dma->sglen = dma_map_sg(dev, dma->sglist,
280 					dma->nr_pages, dma->direction);
281 		if (0 == dma->sglen) {
282 			printk(KERN_WARNING
283 			       "%s: videobuf_map_sg failed\n", __func__);
284 			vfree(dma->sglist);
285 			dma->sglist = NULL;
286 			dma->sglen = 0;
287 			return -ENOMEM;
288 		}
289 	}
290 
291 	return 0;
292 }
293 EXPORT_SYMBOL_GPL(videobuf_dma_map);
294 
295 int videobuf_dma_unmap(struct device *dev, struct videobuf_dmabuf *dma)
296 {
297 	MAGIC_CHECK(dma->magic, MAGIC_DMABUF);
298 
299 	if (!dma->sglen)
300 		return 0;
301 
302 	dma_unmap_sg(dev, dma->sglist, dma->sglen, dma->direction);
303 
304 	vfree(dma->sglist);
305 	dma->sglist = NULL;
306 	dma->sglen = 0;
307 
308 	return 0;
309 }
310 EXPORT_SYMBOL_GPL(videobuf_dma_unmap);
311 
312 int videobuf_dma_free(struct videobuf_dmabuf *dma)
313 {
314 	int i;
315 	MAGIC_CHECK(dma->magic, MAGIC_DMABUF);
316 	BUG_ON(dma->sglen);
317 
318 	if (dma->pages) {
319 		for (i = 0; i < dma->nr_pages; i++)
320 			page_cache_release(dma->pages[i]);
321 		kfree(dma->pages);
322 		dma->pages = NULL;
323 	}
324 
325 	vfree(dma->vaddr);
326 	dma->vaddr = NULL;
327 
328 	if (dma->bus_addr)
329 		dma->bus_addr = 0;
330 	dma->direction = DMA_NONE;
331 
332 	return 0;
333 }
334 EXPORT_SYMBOL_GPL(videobuf_dma_free);
335 
336 /* --------------------------------------------------------------------- */
337 
338 static void videobuf_vm_open(struct vm_area_struct *vma)
339 {
340 	struct videobuf_mapping *map = vma->vm_private_data;
341 	struct videobuf_queue *q = map->q;
342 
343 	dprintk(2, "vm_open %p [count=%d,vma=%08lx-%08lx]\n", map,
344 		map->count, vma->vm_start, vma->vm_end);
345 
346 	videobuf_queue_lock(q);
347 	map->count++;
348 	videobuf_queue_unlock(q);
349 }
350 
351 static void videobuf_vm_close(struct vm_area_struct *vma)
352 {
353 	struct videobuf_mapping *map = vma->vm_private_data;
354 	struct videobuf_queue *q = map->q;
355 	struct videobuf_dma_sg_memory *mem;
356 	int i;
357 
358 	dprintk(2, "vm_close %p [count=%d,vma=%08lx-%08lx]\n", map,
359 		map->count, vma->vm_start, vma->vm_end);
360 
361 	videobuf_queue_lock(q);
362 	if (!--map->count) {
363 		dprintk(1, "munmap %p q=%p\n", map, q);
364 		for (i = 0; i < VIDEO_MAX_FRAME; i++) {
365 			if (NULL == q->bufs[i])
366 				continue;
367 			mem = q->bufs[i]->priv;
368 			if (!mem)
369 				continue;
370 
371 			MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
372 
373 			if (q->bufs[i]->map != map)
374 				continue;
375 			q->bufs[i]->map   = NULL;
376 			q->bufs[i]->baddr = 0;
377 			q->ops->buf_release(q, q->bufs[i]);
378 		}
379 		kfree(map);
380 	}
381 	videobuf_queue_unlock(q);
382 	return;
383 }
384 
385 /*
386  * Get a anonymous page for the mapping.  Make sure we can DMA to that
387  * memory location with 32bit PCI devices (i.e. don't use highmem for
388  * now ...).  Bounce buffers don't work very well for the data rates
389  * video capture has.
390  */
391 static int videobuf_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
392 {
393 	struct page *page;
394 
395 	dprintk(3, "fault: fault @ %08lx [vma %08lx-%08lx]\n",
396 		(unsigned long)vmf->virtual_address,
397 		vma->vm_start, vma->vm_end);
398 
399 	page = alloc_page(GFP_USER | __GFP_DMA32);
400 	if (!page)
401 		return VM_FAULT_OOM;
402 	clear_user_highpage(page, (unsigned long)vmf->virtual_address);
403 	vmf->page = page;
404 
405 	return 0;
406 }
407 
408 static const struct vm_operations_struct videobuf_vm_ops = {
409 	.open	= videobuf_vm_open,
410 	.close	= videobuf_vm_close,
411 	.fault	= videobuf_vm_fault,
412 };
413 
414 /* ---------------------------------------------------------------------
415  * SG handlers for the generic methods
416  */
417 
418 /* Allocated area consists on 3 parts:
419 	struct video_buffer
420 	struct <driver>_buffer (cx88_buffer, saa7134_buf, ...)
421 	struct videobuf_dma_sg_memory
422  */
423 
424 static struct videobuf_buffer *__videobuf_alloc_vb(size_t size)
425 {
426 	struct videobuf_dma_sg_memory *mem;
427 	struct videobuf_buffer *vb;
428 
429 	vb = kzalloc(size + sizeof(*mem), GFP_KERNEL);
430 	if (!vb)
431 		return vb;
432 
433 	mem = vb->priv = ((char *)vb) + size;
434 	mem->magic = MAGIC_SG_MEM;
435 
436 	videobuf_dma_init(&mem->dma);
437 
438 	dprintk(1, "%s: allocated at %p(%ld+%ld) & %p(%ld)\n",
439 		__func__, vb, (long)sizeof(*vb), (long)size - sizeof(*vb),
440 		mem, (long)sizeof(*mem));
441 
442 	return vb;
443 }
444 
445 static void *__videobuf_to_vaddr(struct videobuf_buffer *buf)
446 {
447 	struct videobuf_dma_sg_memory *mem = buf->priv;
448 	BUG_ON(!mem);
449 
450 	MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
451 
452 	return mem->dma.vaddr;
453 }
454 
455 static int __videobuf_iolock(struct videobuf_queue *q,
456 			     struct videobuf_buffer *vb,
457 			     struct v4l2_framebuffer *fbuf)
458 {
459 	int err, pages;
460 	dma_addr_t bus;
461 	struct videobuf_dma_sg_memory *mem = vb->priv;
462 	BUG_ON(!mem);
463 
464 	MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
465 
466 	switch (vb->memory) {
467 	case V4L2_MEMORY_MMAP:
468 	case V4L2_MEMORY_USERPTR:
469 		if (0 == vb->baddr) {
470 			/* no userspace addr -- kernel bounce buffer */
471 			pages = PAGE_ALIGN(vb->size) >> PAGE_SHIFT;
472 			err = videobuf_dma_init_kernel(&mem->dma,
473 						       DMA_FROM_DEVICE,
474 						       pages);
475 			if (0 != err)
476 				return err;
477 		} else if (vb->memory == V4L2_MEMORY_USERPTR) {
478 			/* dma directly to userspace */
479 			err = videobuf_dma_init_user(&mem->dma,
480 						     DMA_FROM_DEVICE,
481 						     vb->baddr, vb->bsize);
482 			if (0 != err)
483 				return err;
484 		} else {
485 			/* NOTE: HACK: videobuf_iolock on V4L2_MEMORY_MMAP
486 			buffers can only be called from videobuf_qbuf
487 			we take current->mm->mmap_sem there, to prevent
488 			locking inversion, so don't take it here */
489 
490 			err = videobuf_dma_init_user_locked(&mem->dma,
491 						      DMA_FROM_DEVICE,
492 						      vb->baddr, vb->bsize);
493 			if (0 != err)
494 				return err;
495 		}
496 		break;
497 	case V4L2_MEMORY_OVERLAY:
498 		if (NULL == fbuf)
499 			return -EINVAL;
500 		/* FIXME: need sanity checks for vb->boff */
501 		/*
502 		 * Using a double cast to avoid compiler warnings when
503 		 * building for PAE. Compiler doesn't like direct casting
504 		 * of a 32 bit ptr to 64 bit integer.
505 		 */
506 		bus   = (dma_addr_t)(unsigned long)fbuf->base + vb->boff;
507 		pages = PAGE_ALIGN(vb->size) >> PAGE_SHIFT;
508 		err = videobuf_dma_init_overlay(&mem->dma, DMA_FROM_DEVICE,
509 						bus, pages);
510 		if (0 != err)
511 			return err;
512 		break;
513 	default:
514 		BUG();
515 	}
516 	err = videobuf_dma_map(q->dev, &mem->dma);
517 	if (0 != err)
518 		return err;
519 
520 	return 0;
521 }
522 
523 static int __videobuf_sync(struct videobuf_queue *q,
524 			   struct videobuf_buffer *buf)
525 {
526 	struct videobuf_dma_sg_memory *mem = buf->priv;
527 	BUG_ON(!mem || !mem->dma.sglen);
528 
529 	MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
530 	MAGIC_CHECK(mem->dma.magic, MAGIC_DMABUF);
531 
532 	dma_sync_sg_for_cpu(q->dev, mem->dma.sglist,
533 			    mem->dma.sglen, mem->dma.direction);
534 
535 	return 0;
536 }
537 
538 static int __videobuf_mmap_mapper(struct videobuf_queue *q,
539 				  struct videobuf_buffer *buf,
540 				  struct vm_area_struct *vma)
541 {
542 	struct videobuf_dma_sg_memory *mem = buf->priv;
543 	struct videobuf_mapping *map;
544 	unsigned int first, last, size = 0, i;
545 	int retval;
546 
547 	retval = -EINVAL;
548 
549 	BUG_ON(!mem);
550 	MAGIC_CHECK(mem->magic, MAGIC_SG_MEM);
551 
552 	/* look for first buffer to map */
553 	for (first = 0; first < VIDEO_MAX_FRAME; first++) {
554 		if (buf == q->bufs[first]) {
555 			size = PAGE_ALIGN(q->bufs[first]->bsize);
556 			break;
557 		}
558 	}
559 
560 	/* paranoia, should never happen since buf is always valid. */
561 	if (!size) {
562 		dprintk(1, "mmap app bug: offset invalid [offset=0x%lx]\n",
563 				(vma->vm_pgoff << PAGE_SHIFT));
564 		goto done;
565 	}
566 
567 	last = first;
568 
569 	/* create mapping + update buffer list */
570 	retval = -ENOMEM;
571 	map = kmalloc(sizeof(struct videobuf_mapping), GFP_KERNEL);
572 	if (NULL == map)
573 		goto done;
574 
575 	size = 0;
576 	for (i = first; i <= last; i++) {
577 		if (NULL == q->bufs[i])
578 			continue;
579 		q->bufs[i]->map   = map;
580 		q->bufs[i]->baddr = vma->vm_start + size;
581 		size += PAGE_ALIGN(q->bufs[i]->bsize);
582 	}
583 
584 	map->count    = 1;
585 	map->q        = q;
586 	vma->vm_ops   = &videobuf_vm_ops;
587 	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
588 	vma->vm_flags &= ~VM_IO; /* using shared anonymous pages */
589 	vma->vm_private_data = map;
590 	dprintk(1, "mmap %p: q=%p %08lx-%08lx pgoff %08lx bufs %d-%d\n",
591 		map, q, vma->vm_start, vma->vm_end, vma->vm_pgoff, first, last);
592 	retval = 0;
593 
594 done:
595 	return retval;
596 }
597 
598 static struct videobuf_qtype_ops sg_ops = {
599 	.magic        = MAGIC_QTYPE_OPS,
600 
601 	.alloc_vb     = __videobuf_alloc_vb,
602 	.iolock       = __videobuf_iolock,
603 	.sync         = __videobuf_sync,
604 	.mmap_mapper  = __videobuf_mmap_mapper,
605 	.vaddr        = __videobuf_to_vaddr,
606 };
607 
608 void *videobuf_sg_alloc(size_t size)
609 {
610 	struct videobuf_queue q;
611 
612 	/* Required to make generic handler to call __videobuf_alloc */
613 	q.int_ops = &sg_ops;
614 
615 	q.msize = size;
616 
617 	return videobuf_alloc_vb(&q);
618 }
619 EXPORT_SYMBOL_GPL(videobuf_sg_alloc);
620 
621 void videobuf_queue_sg_init(struct videobuf_queue *q,
622 			 const struct videobuf_queue_ops *ops,
623 			 struct device *dev,
624 			 spinlock_t *irqlock,
625 			 enum v4l2_buf_type type,
626 			 enum v4l2_field field,
627 			 unsigned int msize,
628 			 void *priv,
629 			 struct mutex *ext_lock)
630 {
631 	videobuf_queue_core_init(q, ops, dev, irqlock, type, field, msize,
632 				 priv, &sg_ops, ext_lock);
633 }
634 EXPORT_SYMBOL_GPL(videobuf_queue_sg_init);
635 
636