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