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@kernel.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 
70 	dev_dbg(map->q->dev, "vm_open %p [count=%u,vma=%08lx-%08lx]\n",
71 		map, map->count, vma->vm_start, vma->vm_end);
72 
73 	map->count++;
74 }
75 
76 static void videobuf_vm_close(struct vm_area_struct *vma)
77 {
78 	struct videobuf_mapping *map = vma->vm_private_data;
79 	struct videobuf_queue *q = map->q;
80 	int i;
81 
82 	dev_dbg(q->dev, "vm_close %p [count=%u,vma=%08lx-%08lx]\n",
83 		map, map->count, vma->vm_start, vma->vm_end);
84 
85 	map->count--;
86 	if (0 == map->count) {
87 		struct videobuf_dma_contig_memory *mem;
88 
89 		dev_dbg(q->dev, "munmap %p q=%p\n", map, q);
90 		videobuf_queue_lock(q);
91 
92 		/* We need first to cancel streams, before unmapping */
93 		if (q->streaming)
94 			videobuf_queue_cancel(q);
95 
96 		for (i = 0; i < VIDEO_MAX_FRAME; i++) {
97 			if (NULL == q->bufs[i])
98 				continue;
99 
100 			if (q->bufs[i]->map != map)
101 				continue;
102 
103 			mem = q->bufs[i]->priv;
104 			if (mem) {
105 				/* This callback is called only if kernel has
106 				   allocated memory and this memory is mmapped.
107 				   In this case, memory should be freed,
108 				   in order to do memory unmap.
109 				 */
110 
111 				MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
112 
113 				/* vfree is not atomic - can't be
114 				   called with IRQ's disabled
115 				 */
116 				dev_dbg(q->dev, "buf[%d] freeing %p\n",
117 					i, mem->vaddr);
118 
119 				__videobuf_dc_free(q->dev, mem);
120 				mem->vaddr = NULL;
121 			}
122 
123 			q->bufs[i]->map = NULL;
124 			q->bufs[i]->baddr = 0;
125 		}
126 
127 		kfree(map);
128 
129 		videobuf_queue_unlock(q);
130 	}
131 }
132 
133 static const struct vm_operations_struct videobuf_vm_ops = {
134 	.open	= videobuf_vm_open,
135 	.close	= videobuf_vm_close,
136 };
137 
138 /**
139  * videobuf_dma_contig_user_put() - reset pointer to user space buffer
140  * @mem: per-buffer private videobuf-dma-contig data
141  *
142  * This function resets the user space pointer
143  */
144 static void videobuf_dma_contig_user_put(struct videobuf_dma_contig_memory *mem)
145 {
146 	mem->dma_handle = 0;
147 	mem->size = 0;
148 }
149 
150 /**
151  * videobuf_dma_contig_user_get() - setup user space memory pointer
152  * @mem: per-buffer private videobuf-dma-contig data
153  * @vb: video buffer to map
154  *
155  * This function validates and sets up a pointer to user space memory.
156  * Only physically contiguous pfn-mapped memory is accepted.
157  *
158  * Returns 0 if successful.
159  */
160 static int videobuf_dma_contig_user_get(struct videobuf_dma_contig_memory *mem,
161 					struct videobuf_buffer *vb)
162 {
163 	struct mm_struct *mm = current->mm;
164 	struct vm_area_struct *vma;
165 	unsigned long prev_pfn, this_pfn;
166 	unsigned long pages_done, user_address;
167 	unsigned int offset;
168 	int ret;
169 
170 	offset = vb->baddr & ~PAGE_MASK;
171 	mem->size = PAGE_ALIGN(vb->size + offset);
172 	ret = -EINVAL;
173 
174 	down_read(&mm->mmap_sem);
175 
176 	vma = find_vma(mm, vb->baddr);
177 	if (!vma)
178 		goto out_up;
179 
180 	if ((vb->baddr + mem->size) > vma->vm_end)
181 		goto out_up;
182 
183 	pages_done = 0;
184 	prev_pfn = 0; /* kill warning */
185 	user_address = vb->baddr;
186 
187 	while (pages_done < (mem->size >> PAGE_SHIFT)) {
188 		ret = follow_pfn(vma, user_address, &this_pfn);
189 		if (ret)
190 			break;
191 
192 		if (pages_done == 0)
193 			mem->dma_handle = (this_pfn << PAGE_SHIFT) + offset;
194 		else if (this_pfn != (prev_pfn + 1))
195 			ret = -EFAULT;
196 
197 		if (ret)
198 			break;
199 
200 		prev_pfn = this_pfn;
201 		user_address += PAGE_SIZE;
202 		pages_done++;
203 	}
204 
205 out_up:
206 	up_read(&current->mm->mmap_sem);
207 
208 	return ret;
209 }
210 
211 static struct videobuf_buffer *__videobuf_alloc(size_t size)
212 {
213 	struct videobuf_dma_contig_memory *mem;
214 	struct videobuf_buffer *vb;
215 
216 	vb = kzalloc(size + sizeof(*mem), GFP_KERNEL);
217 	if (vb) {
218 		vb->priv = ((char *)vb) + size;
219 		mem = vb->priv;
220 		mem->magic = MAGIC_DC_MEM;
221 	}
222 
223 	return vb;
224 }
225 
226 static void *__videobuf_to_vaddr(struct videobuf_buffer *buf)
227 {
228 	struct videobuf_dma_contig_memory *mem = buf->priv;
229 
230 	BUG_ON(!mem);
231 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
232 
233 	return mem->vaddr;
234 }
235 
236 static int __videobuf_iolock(struct videobuf_queue *q,
237 			     struct videobuf_buffer *vb,
238 			     struct v4l2_framebuffer *fbuf)
239 {
240 	struct videobuf_dma_contig_memory *mem = vb->priv;
241 
242 	BUG_ON(!mem);
243 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
244 
245 	switch (vb->memory) {
246 	case V4L2_MEMORY_MMAP:
247 		dev_dbg(q->dev, "%s memory method MMAP\n", __func__);
248 
249 		/* All handling should be done by __videobuf_mmap_mapper() */
250 		if (!mem->vaddr) {
251 			dev_err(q->dev, "memory is not allocated/mmapped.\n");
252 			return -EINVAL;
253 		}
254 		break;
255 	case V4L2_MEMORY_USERPTR:
256 		dev_dbg(q->dev, "%s memory method USERPTR\n", __func__);
257 
258 		/* handle pointer from user space */
259 		if (vb->baddr)
260 			return videobuf_dma_contig_user_get(mem, vb);
261 
262 		/* allocate memory for the read() method */
263 		if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(vb->size),
264 					GFP_KERNEL))
265 			return -ENOMEM;
266 		break;
267 	case V4L2_MEMORY_OVERLAY:
268 	default:
269 		dev_dbg(q->dev, "%s memory method OVERLAY/unknown\n", __func__);
270 		return -EINVAL;
271 	}
272 
273 	return 0;
274 }
275 
276 static int __videobuf_mmap_mapper(struct videobuf_queue *q,
277 				  struct videobuf_buffer *buf,
278 				  struct vm_area_struct *vma)
279 {
280 	struct videobuf_dma_contig_memory *mem;
281 	struct videobuf_mapping *map;
282 	int retval;
283 
284 	dev_dbg(q->dev, "%s\n", __func__);
285 
286 	/* create mapping + update buffer list */
287 	map = kzalloc(sizeof(struct videobuf_mapping), GFP_KERNEL);
288 	if (!map)
289 		return -ENOMEM;
290 
291 	buf->map = map;
292 	map->q = q;
293 
294 	buf->baddr = vma->vm_start;
295 
296 	mem = buf->priv;
297 	BUG_ON(!mem);
298 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
299 
300 	if (__videobuf_dc_alloc(q->dev, mem, PAGE_ALIGN(buf->bsize),
301 				GFP_KERNEL | __GFP_COMP))
302 		goto error;
303 
304 	/* Try to remap memory */
305 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
306 
307 	/* the "vm_pgoff" is just used in v4l2 to find the
308 	 * corresponding buffer data structure which is allocated
309 	 * earlier and it does not mean the offset from the physical
310 	 * buffer start address as usual. So set it to 0 to pass
311 	 * the sanity check in vm_iomap_memory().
312 	 */
313 	vma->vm_pgoff = 0;
314 
315 	retval = vm_iomap_memory(vma, mem->dma_handle, mem->size);
316 	if (retval) {
317 		dev_err(q->dev, "mmap: remap failed with error %d. ",
318 			retval);
319 		dma_free_coherent(q->dev, mem->size,
320 				  mem->vaddr, mem->dma_handle);
321 		goto error;
322 	}
323 
324 	vma->vm_ops = &videobuf_vm_ops;
325 	vma->vm_flags |= VM_DONTEXPAND;
326 	vma->vm_private_data = map;
327 
328 	dev_dbg(q->dev, "mmap %p: q=%p %08lx-%08lx (%lx) pgoff %08lx buf %d\n",
329 		map, q, vma->vm_start, vma->vm_end,
330 		(long int)buf->bsize, vma->vm_pgoff, buf->i);
331 
332 	videobuf_vm_open(vma);
333 
334 	return 0;
335 
336 error:
337 	kfree(map);
338 	return -ENOMEM;
339 }
340 
341 static struct videobuf_qtype_ops qops = {
342 	.magic		= MAGIC_QTYPE_OPS,
343 	.alloc_vb	= __videobuf_alloc,
344 	.iolock		= __videobuf_iolock,
345 	.mmap_mapper	= __videobuf_mmap_mapper,
346 	.vaddr		= __videobuf_to_vaddr,
347 };
348 
349 void videobuf_queue_dma_contig_init(struct videobuf_queue *q,
350 				    const struct videobuf_queue_ops *ops,
351 				    struct device *dev,
352 				    spinlock_t *irqlock,
353 				    enum v4l2_buf_type type,
354 				    enum v4l2_field field,
355 				    unsigned int msize,
356 				    void *priv,
357 				    struct mutex *ext_lock)
358 {
359 	videobuf_queue_core_init(q, ops, dev, irqlock, type, field, msize,
360 				 priv, &qops, ext_lock);
361 }
362 EXPORT_SYMBOL_GPL(videobuf_queue_dma_contig_init);
363 
364 dma_addr_t videobuf_to_dma_contig(struct videobuf_buffer *buf)
365 {
366 	struct videobuf_dma_contig_memory *mem = buf->priv;
367 
368 	BUG_ON(!mem);
369 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
370 
371 	return mem->dma_handle;
372 }
373 EXPORT_SYMBOL_GPL(videobuf_to_dma_contig);
374 
375 void videobuf_dma_contig_free(struct videobuf_queue *q,
376 			      struct videobuf_buffer *buf)
377 {
378 	struct videobuf_dma_contig_memory *mem = buf->priv;
379 
380 	/* mmapped memory can't be freed here, otherwise mmapped region
381 	   would be released, while still needed. In this case, the memory
382 	   release should happen inside videobuf_vm_close().
383 	   So, it should free memory only if the memory were allocated for
384 	   read() operation.
385 	 */
386 	if (buf->memory != V4L2_MEMORY_USERPTR)
387 		return;
388 
389 	if (!mem)
390 		return;
391 
392 	MAGIC_CHECK(mem->magic, MAGIC_DC_MEM);
393 
394 	/* handle user space pointer case */
395 	if (buf->baddr) {
396 		videobuf_dma_contig_user_put(mem);
397 		return;
398 	}
399 
400 	/* read() method */
401 	if (mem->vaddr) {
402 		__videobuf_dc_free(q->dev, mem);
403 		mem->vaddr = NULL;
404 	}
405 }
406 EXPORT_SYMBOL_GPL(videobuf_dma_contig_free);
407 
408 MODULE_DESCRIPTION("helper module to manage video4linux dma contig buffers");
409 MODULE_AUTHOR("Magnus Damm");
410 MODULE_LICENSE("GPL");
411