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