1 /*
2  * helper functions for physically contiguous capture buffers
3  *
4  * The functions support hardware lacking scatter gather support
5  * (i.e. the buffers must be linear in physical memory)
6  *
7  * Copyright (c) 2008 Magnus Damm
8  *
9  * Based on videobuf-vmalloc.c,
10  * (c) 2007 Mauro Carvalho Chehab, <mchehab@infradead.org>
11  *
12  * This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2
15  */
16 
17 #include <linux/init.h>
18 #include <linux/module.h>
19 #include <linux/mm.h>
20 #include <linux/pagemap.h>
21 #include <linux/dma-mapping.h>
22 #include <linux/sched.h>
23 #include <linux/slab.h>
24 #include <media/videobuf-dma-contig.h>
25 
26 struct videobuf_dma_contig_memory {
27 	u32 magic;
28 	void *vaddr;
29 	dma_addr_t dma_handle;
30 	unsigned long size;
31 };
32 
33 #define MAGIC_DC_MEM 0x0733ac61
34 #define MAGIC_CHECK(is, should)						    \
35 	if (unlikely((is) != (should)))	{				    \
36 		pr_err("magic mismatch: %x expected %x\n", (is), (should)); \
37 		BUG();							    \
38 	}
39 
40 static int __videobuf_dc_alloc(struct device *dev,
41 			       struct videobuf_dma_contig_memory *mem,
42 			       unsigned long size, gfp_t flags)
43 {
44 	mem->size = size;
45 	mem->vaddr = dma_alloc_coherent(dev, mem->size,
46 					&mem->dma_handle, flags);
47 
48 	if (!mem->vaddr) {
49 		dev_err(dev, "memory alloc size %ld failed\n", mem->size);
50 		return -ENOMEM;
51 	}
52 
53 	dev_dbg(dev, "dma mapped data is at %p (%ld)\n", mem->vaddr, mem->size);
54 
55 	return 0;
56 }
57 
58 static void __videobuf_dc_free(struct device *dev,
59 			       struct videobuf_dma_contig_memory *mem)
60 {
61 	dma_free_coherent(dev, mem->size, mem->vaddr, mem->dma_handle);
62 
63 	mem->vaddr = NULL;
64 }
65 
66 static void videobuf_vm_open(struct vm_area_struct *vma)
67 {
68 	struct videobuf_mapping *map = vma->vm_private_data;
69 	struct videobuf_queue *q = map->q;
70 
71 	dev_dbg(q->dev, "vm_open %p [count=%u,vma=%08lx-%08lx]\n",
72 		map, map->count, vma->vm_start, vma->vm_end);
73 
74 	videobuf_queue_lock(q);
75 	map->count++;
76 	videobuf_queue_unlock(q);
77 }
78 
79 static void videobuf_vm_close(struct vm_area_struct *vma)
80 {
81 	struct videobuf_mapping *map = vma->vm_private_data;
82 	struct videobuf_queue *q = map->q;
83 	int i;
84 
85 	dev_dbg(q->dev, "vm_close %p [count=%u,vma=%08lx-%08lx]\n",
86 		map, map->count, vma->vm_start, vma->vm_end);
87 
88 	videobuf_queue_lock(q);
89 	if (!--map->count) {
90 		struct videobuf_dma_contig_memory *mem;
91 
92 		dev_dbg(q->dev, "munmap %p q=%p\n", map, q);
93 
94 		/* We need first to cancel streams, before unmapping */
95 		if (q->streaming)
96 			videobuf_queue_cancel(q);
97 
98 		for (i = 0; i < VIDEO_MAX_FRAME; i++) {
99 			if (NULL == q->bufs[i])
100 				continue;
101 
102 			if (q->bufs[i]->map != map)
103 				continue;
104 
105 			mem = q->bufs[i]->priv;
106 			if (mem) {
107 				/* This callback is called only if kernel has
108 				   allocated memory and this memory is mmapped.
109 				   In this case, memory should be freed,
110 				   in order to do memory unmap.
111 				 */
112 
113 				MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
114 
115 				/* vfree is not atomic - can't be
116 				   called with IRQ's disabled
117 				 */
118 				dev_dbg(q->dev, "buf[%d] freeing %p\n",
119 					i, mem->vaddr);
120 
121 				__videobuf_dc_free(q->dev, mem);
122 				mem->vaddr = NULL;
123 			}
124 
125 			q->bufs[i]->map = NULL;
126 			q->bufs[i]->baddr = 0;
127 		}
128 
129 		kfree(map);
130 
131 	}
132 	videobuf_queue_unlock(q);
133 }
134 
135 static const struct vm_operations_struct videobuf_vm_ops = {
136 	.open	= videobuf_vm_open,
137 	.close	= videobuf_vm_close,
138 };
139 
140 /**
141  * videobuf_dma_contig_user_put() - reset pointer to user space buffer
142  * @mem: per-buffer private videobuf-dma-contig data
143  *
144  * This function resets the user space pointer
145  */
146 static void videobuf_dma_contig_user_put(struct videobuf_dma_contig_memory *mem)
147 {
148 	mem->dma_handle = 0;
149 	mem->size = 0;
150 }
151 
152 /**
153  * videobuf_dma_contig_user_get() - setup user space memory pointer
154  * @mem: per-buffer private videobuf-dma-contig data
155  * @vb: video buffer to map
156  *
157  * This function validates and sets up a pointer to user space memory.
158  * Only physically contiguous pfn-mapped memory is accepted.
159  *
160  * Returns 0 if successful.
161  */
162 static int videobuf_dma_contig_user_get(struct videobuf_dma_contig_memory *mem,
163 					struct videobuf_buffer *vb)
164 {
165 	struct mm_struct *mm = current->mm;
166 	struct vm_area_struct *vma;
167 	unsigned long prev_pfn, this_pfn;
168 	unsigned long pages_done, user_address;
169 	unsigned int offset;
170 	int ret;
171 
172 	offset = vb->baddr & ~PAGE_MASK;
173 	mem->size = PAGE_ALIGN(vb->size + offset);
174 	ret = -EINVAL;
175 
176 	down_read(&mm->mmap_sem);
177 
178 	vma = find_vma(mm, vb->baddr);
179 	if (!vma)
180 		goto out_up;
181 
182 	if ((vb->baddr + mem->size) > vma->vm_end)
183 		goto out_up;
184 
185 	pages_done = 0;
186 	prev_pfn = 0; /* kill warning */
187 	user_address = vb->baddr;
188 
189 	while (pages_done < (mem->size >> PAGE_SHIFT)) {
190 		ret = follow_pfn(vma, user_address, &this_pfn);
191 		if (ret)
192 			break;
193 
194 		if (pages_done == 0)
195 			mem->dma_handle = (this_pfn << PAGE_SHIFT) + offset;
196 		else if (this_pfn != (prev_pfn + 1))
197 			ret = -EFAULT;
198 
199 		if (ret)
200 			break;
201 
202 		prev_pfn = this_pfn;
203 		user_address += PAGE_SIZE;
204 		pages_done++;
205 	}
206 
207 out_up:
208 	up_read(&current->mm->mmap_sem);
209 
210 	return ret;
211 }
212 
213 static struct videobuf_buffer *__videobuf_alloc(size_t size)
214 {
215 	struct videobuf_dma_contig_memory *mem;
216 	struct videobuf_buffer *vb;
217 
218 	vb = kzalloc(size + sizeof(*mem), GFP_KERNEL);
219 	if (vb) {
220 		vb->priv = ((char *)vb) + size;
221 		mem = vb->priv;
222 		mem->magic = MAGIC_DC_MEM;
223 	}
224 
225 	return vb;
226 }
227 
228 static void *__videobuf_to_vaddr(struct videobuf_buffer *buf)
229 {
230 	struct videobuf_dma_contig_memory *mem = buf->priv;
231 
232 	BUG_ON(!mem);
233 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
234 
235 	return mem->vaddr;
236 }
237 
238 static int __videobuf_iolock(struct videobuf_queue *q,
239 			     struct videobuf_buffer *vb,
240 			     struct v4l2_framebuffer *fbuf)
241 {
242 	struct videobuf_dma_contig_memory *mem = vb->priv;
243 
244 	BUG_ON(!mem);
245 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
246 
247 	switch (vb->memory) {
248 	case V4L2_MEMORY_MMAP:
249 		dev_dbg(q->dev, "%s memory method MMAP\n", __func__);
250 
251 		/* All handling should be done by __videobuf_mmap_mapper() */
252 		if (!mem->vaddr) {
253 			dev_err(q->dev, "memory is not alloced/mmapped.\n");
254 			return -EINVAL;
255 		}
256 		break;
257 	case V4L2_MEMORY_USERPTR:
258 		dev_dbg(q->dev, "%s memory method USERPTR\n", __func__);
259 
260 		/* handle pointer from user space */
261 		if (vb->baddr)
262 			return videobuf_dma_contig_user_get(mem, vb);
263 
264 		/* allocate memory for the read() method */
265 		if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(vb->size),
266 					GFP_KERNEL))
267 			return -ENOMEM;
268 		break;
269 	case V4L2_MEMORY_OVERLAY:
270 	default:
271 		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n", __func__);
272 		return -EINVAL;
273 	}
274 
275 	return 0;
276 }
277 
278 static int __videobuf_mmap_mapper(struct videobuf_queue *q,
279 				  struct videobuf_buffer *buf,
280 				  struct vm_area_struct *vma)
281 {
282 	struct videobuf_dma_contig_memory *mem;
283 	struct videobuf_mapping *map;
284 	int retval;
285 	unsigned long size;
286 
287 	dev_dbg(q->dev, "%s\n", __func__);
288 
289 	/* create mapping + update buffer list */
290 	map = kzalloc(sizeof(struct videobuf_mapping), GFP_KERNEL);
291 	if (!map)
292 		return -ENOMEM;
293 
294 	buf->map = map;
295 	map->q = q;
296 
297 	buf->baddr = vma->vm_start;
298 
299 	mem = buf->priv;
300 	BUG_ON(!mem);
301 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
302 
303 	if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(buf->bsize),
304 				GFP_KERNEL | __GFP_COMP))
305 		goto error;
306 
307 	/* Try to remap memory */
308 	size = vma->vm_end - vma->vm_start;
309 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
310 	retval = vm_iomap_memory(vma, vma->vm_start, size);
311 	if (retval) {
312 		dev_err(q->dev, "mmap: remap failed with error %d. ",
313 			retval);
314 		dma_free_coherent(q->dev, mem->size,
315 				  mem->vaddr, mem->dma_handle);
316 		goto error;
317 	}
318 
319 	vma->vm_ops = &videobuf_vm_ops;
320 	vma->vm_flags |= VM_DONTEXPAND;
321 	vma->vm_private_data = map;
322 
323 	dev_dbg(q->dev, "mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
324 		map, q, vma->vm_start, vma->vm_end,
325 		(long int)buf->bsize, vma->vm_pgoff, buf->i);
326 
327 	videobuf_vm_open(vma);
328 
329 	return 0;
330 
331 error:
332 	kfree(map);
333 	return -ENOMEM;
334 }
335 
336 static struct videobuf_qtype_ops qops = {
337 	.magic		= MAGIC_QTYPE_OPS,
338 	.alloc_vb	= __videobuf_alloc,
339 	.iolock		= __videobuf_iolock,
340 	.mmap_mapper	= __videobuf_mmap_mapper,
341 	.vaddr		= __videobuf_to_vaddr,
342 };
343 
344 void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
345 				    const struct videobuf_queue_ops *ops,
346 				    struct device *dev,
347 				    spinlock_t *irqlock,
348 				    enum v4l2_buf_type type,
349 				    enum v4l2_field field,
350 				    unsigned int msize,
351 				    void *priv,
352 				    struct mutex *ext_lock)
353 {
354 	videobuf_queue_core_init(q, ops, dev, irqlock, type, field, msize,
355 				 priv, &qops, ext_lock);
356 }
357 EXPORT_SYMBOL_GPL(videobuf_queue_dma_contig_init);
358 
359 dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf)
360 {
361 	struct videobuf_dma_contig_memory *mem = buf->priv;
362 
363 	BUG_ON(!mem);
364 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
365 
366 	return mem->dma_handle;
367 }
368 EXPORT_SYMBOL_GPL(videobuf_to_dma_contig);
369 
370 void videobuf_dma_contig_free(struct videobuf_queue *q,
371 			      struct videobuf_buffer *buf)
372 {
373 	struct videobuf_dma_contig_memory *mem = buf->priv;
374 
375 	/* mmapped memory can't be freed here, otherwise mmapped region
376 	   would be released, while still needed. In this case, the memory
377 	   release should happen inside videobuf_vm_close().
378 	   So, it should free memory only if the memory were allocated for
379 	   read() operation.
380 	 */
381 	if (buf->memory != V4L2_MEMORY_USERPTR)
382 		return;
383 
384 	if (!mem)
385 		return;
386 
387 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
388 
389 	/* handle user space pointer case */
390 	if (buf->baddr) {
391 		videobuf_dma_contig_user_put(mem);
392 		return;
393 	}
394 
395 	/* read() method */
396 	if (mem->vaddr) {
397 		__videobuf_dc_free(q->dev, mem);
398 		mem->vaddr = NULL;
399 	}
400 }
401 EXPORT_SYMBOL_GPL(videobuf_dma_contig_free);
402 
403 MODULE_DESCRIPTION("helper module to manage video4linux dma contig buffers");
404 MODULE_AUTHOR("Magnus Damm");
405 MODULE_LICENSE("GPL");
406