videobuf2-dma-sg.c 7.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292
  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 <andrzej.p@samsung.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. #include <linux/module.h>
  13. #include <linux/mm.h>
  14. #include <linux/scatterlist.h>
  15. #include <linux/sched.h>
  16. #include <linux/slab.h>
  17. #include <linux/vmalloc.h>
  18. #include <media/videobuf2-core.h>
  19. #include <media/videobuf2-memops.h>
  20. #include <media/videobuf2-dma-sg.h>
  21. struct vb2_dma_sg_buf {
  22. void *vaddr;
  23. struct page **pages;
  24. int write;
  25. int offset;
  26. struct vb2_dma_sg_desc sg_desc;
  27. atomic_t refcount;
  28. struct vb2_vmarea_handler handler;
  29. };
  30. static void vb2_dma_sg_put(void *buf_priv);
  31. static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
  32. {
  33. struct vb2_dma_sg_buf *buf;
  34. int i;
  35. buf = kzalloc(sizeof *buf, GFP_KERNEL);
  36. if (!buf)
  37. return NULL;
  38. buf->vaddr = NULL;
  39. buf->write = 0;
  40. buf->offset = 0;
  41. buf->sg_desc.size = size;
  42. buf->sg_desc.num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
  43. buf->sg_desc.sglist = vmalloc(buf->sg_desc.num_pages *
  44. sizeof(*buf->sg_desc.sglist));
  45. if (!buf->sg_desc.sglist)
  46. goto fail_sglist_alloc;
  47. memset(buf->sg_desc.sglist, 0, buf->sg_desc.num_pages *
  48. sizeof(*buf->sg_desc.sglist));
  49. sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
  50. buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
  51. GFP_KERNEL);
  52. if (!buf->pages)
  53. goto fail_pages_array_alloc;
  54. for (i = 0; i < buf->sg_desc.num_pages; ++i) {
  55. buf->pages[i] = alloc_page(GFP_KERNEL);
  56. if (NULL == buf->pages[i])
  57. goto fail_pages_alloc;
  58. sg_set_page(&buf->sg_desc.sglist[i],
  59. buf->pages[i], PAGE_SIZE, 0);
  60. }
  61. buf->handler.refcount = &buf->refcount;
  62. buf->handler.put = vb2_dma_sg_put;
  63. buf->handler.arg = buf;
  64. atomic_inc(&buf->refcount);
  65. printk(KERN_DEBUG "%s: Allocated buffer of %d pages\n",
  66. __func__, buf->sg_desc.num_pages);
  67. if (!buf->vaddr)
  68. buf->vaddr = vm_map_ram(buf->pages,
  69. buf->sg_desc.num_pages,
  70. -1,
  71. PAGE_KERNEL);
  72. return buf;
  73. fail_pages_alloc:
  74. while (--i >= 0)
  75. __free_page(buf->pages[i]);
  76. fail_pages_array_alloc:
  77. vfree(buf->sg_desc.sglist);
  78. fail_sglist_alloc:
  79. kfree(buf);
  80. return NULL;
  81. }
  82. static void vb2_dma_sg_put(void *buf_priv)
  83. {
  84. struct vb2_dma_sg_buf *buf = buf_priv;
  85. int i = buf->sg_desc.num_pages;
  86. if (atomic_dec_and_test(&buf->refcount)) {
  87. printk(KERN_DEBUG "%s: Freeing buffer of %d pages\n", __func__,
  88. buf->sg_desc.num_pages);
  89. if (buf->vaddr)
  90. vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
  91. vfree(buf->sg_desc.sglist);
  92. while (--i >= 0)
  93. __free_page(buf->pages[i]);
  94. kfree(buf->pages);
  95. kfree(buf);
  96. }
  97. }
  98. static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
  99. unsigned long size, int write)
  100. {
  101. struct vb2_dma_sg_buf *buf;
  102. unsigned long first, last;
  103. int num_pages_from_user, i;
  104. buf = kzalloc(sizeof *buf, GFP_KERNEL);
  105. if (!buf)
  106. return NULL;
  107. buf->vaddr = NULL;
  108. buf->write = write;
  109. buf->offset = vaddr & ~PAGE_MASK;
  110. buf->sg_desc.size = size;
  111. first = (vaddr & PAGE_MASK) >> PAGE_SHIFT;
  112. last = ((vaddr + size - 1) & PAGE_MASK) >> PAGE_SHIFT;
  113. buf->sg_desc.num_pages = last - first + 1;
  114. buf->sg_desc.sglist = vmalloc(
  115. buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
  116. if (!buf->sg_desc.sglist)
  117. goto userptr_fail_sglist_alloc;
  118. memset(buf->sg_desc.sglist, 0,
  119. buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
  120. sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);
  121. buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
  122. GFP_KERNEL);
  123. if (!buf->pages)
  124. goto userptr_fail_pages_array_alloc;
  125. down_read(&current->mm->mmap_sem);
  126. num_pages_from_user = get_user_pages(current, current->mm,
  127. vaddr & PAGE_MASK,
  128. buf->sg_desc.num_pages,
  129. write,
  130. 1, /* force */
  131. buf->pages,
  132. NULL);
  133. up_read(&current->mm->mmap_sem);
  134. if (num_pages_from_user != buf->sg_desc.num_pages)
  135. goto userptr_fail_get_user_pages;
  136. sg_set_page(&buf->sg_desc.sglist[0], buf->pages[0],
  137. PAGE_SIZE - buf->offset, buf->offset);
  138. size -= PAGE_SIZE - buf->offset;
  139. for (i = 1; i < buf->sg_desc.num_pages; ++i) {
  140. sg_set_page(&buf->sg_desc.sglist[i], buf->pages[i],
  141. min_t(size_t, PAGE_SIZE, size), 0);
  142. size -= min_t(size_t, PAGE_SIZE, size);
  143. }
  144. return buf;
  145. userptr_fail_get_user_pages:
  146. printk(KERN_DEBUG "get_user_pages requested/got: %d/%d]\n",
  147. num_pages_from_user, buf->sg_desc.num_pages);
  148. while (--num_pages_from_user >= 0)
  149. put_page(buf->pages[num_pages_from_user]);
  150. userptr_fail_pages_array_alloc:
  151. vfree(buf->sg_desc.sglist);
  152. userptr_fail_sglist_alloc:
  153. kfree(buf);
  154. return NULL;
  155. }
  156. /*
  157. * @put_userptr: inform the allocator that a USERPTR buffer will no longer
  158. * be used
  159. */
  160. static void vb2_dma_sg_put_userptr(void *buf_priv)
  161. {
  162. struct vb2_dma_sg_buf *buf = buf_priv;
  163. int i = buf->sg_desc.num_pages;
  164. printk(KERN_DEBUG "%s: Releasing userspace buffer of %d pages\n",
  165. __func__, buf->sg_desc.num_pages);
  166. if (buf->vaddr)
  167. vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
  168. while (--i >= 0) {
  169. if (buf->write)
  170. set_page_dirty_lock(buf->pages[i]);
  171. put_page(buf->pages[i]);
  172. }
  173. vfree(buf->sg_desc.sglist);
  174. kfree(buf->pages);
  175. kfree(buf);
  176. }
  177. static void *vb2_dma_sg_vaddr(void *buf_priv)
  178. {
  179. struct vb2_dma_sg_buf *buf = buf_priv;
  180. BUG_ON(!buf);
  181. if (!buf->vaddr)
  182. buf->vaddr = vm_map_ram(buf->pages,
  183. buf->sg_desc.num_pages,
  184. -1,
  185. PAGE_KERNEL);
  186. /* add offset in case userptr is not page-aligned */
  187. return buf->vaddr + buf->offset;
  188. }
  189. static unsigned int vb2_dma_sg_num_users(void *buf_priv)
  190. {
  191. struct vb2_dma_sg_buf *buf = buf_priv;
  192. return atomic_read(&buf->refcount);
  193. }
  194. static int vb2_dma_sg_mmap(void *buf_priv, struct vm_area_struct *vma)
  195. {
  196. struct vb2_dma_sg_buf *buf = buf_priv;
  197. unsigned long uaddr = vma->vm_start;
  198. unsigned long usize = vma->vm_end - vma->vm_start;
  199. int i = 0;
  200. if (!buf) {
  201. printk(KERN_ERR "No memory to map\n");
  202. return -EINVAL;
  203. }
  204. do {
  205. int ret;
  206. ret = vm_insert_page(vma, uaddr, buf->pages[i++]);
  207. if (ret) {
  208. printk(KERN_ERR "Remapping memory, error: %d\n", ret);
  209. return ret;
  210. }
  211. uaddr += PAGE_SIZE;
  212. usize -= PAGE_SIZE;
  213. } while (usize > 0);
  214. /*
  215. * Use common vm_area operations to track buffer refcount.
  216. */
  217. vma->vm_private_data = &buf->handler;
  218. vma->vm_ops = &vb2_common_vm_ops;
  219. vma->vm_ops->open(vma);
  220. return 0;
  221. }
  222. static void *vb2_dma_sg_cookie(void *buf_priv)
  223. {
  224. struct vb2_dma_sg_buf *buf = buf_priv;
  225. return &buf->sg_desc;
  226. }
  227. const struct vb2_mem_ops vb2_dma_sg_memops = {
  228. .alloc = vb2_dma_sg_alloc,
  229. .put = vb2_dma_sg_put,
  230. .get_userptr = vb2_dma_sg_get_userptr,
  231. .put_userptr = vb2_dma_sg_put_userptr,
  232. .vaddr = vb2_dma_sg_vaddr,
  233. .mmap = vb2_dma_sg_mmap,
  234. .num_users = vb2_dma_sg_num_users,
  235. .cookie = vb2_dma_sg_cookie,
  236. };
  237. EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);
  238. MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
  239. MODULE_AUTHOR("Andrzej Pietrasiewicz");
  240. MODULE_LICENSE("GPL");