drivers/media/v4l2-core/videobuf2-dma-sg.c

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