videobuf2-dma-sg.c

Go to the documentation of this file.

/*
 * videobuf2-dma-sg.c - dma scatter/gather memory allocator for videobuf2
 *
 * Copyright (C) 2010 Samsung Electronics
 *
 * Author: Andrzej Pietrasiewicz <[email protected]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation.
 */

#include <linux/module.h>
#include <linux/mm.h>
#include <linux/scatterlist.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/vmalloc.h>

#include <media/videobuf2-core.h>
#include <media/videobuf2-memops.h>
#include <media/videobuf2-dma-sg.h>

struct vb2_dma_sg_buf {
    void                *vaddr;
    struct page         **pages;
    int             write;
    int             offset;
    struct vb2_dma_sg_desc      sg_desc;
    atomic_t            refcount;
    struct vb2_vmarea_handler   handler;
};

static void vb2_dma_sg_put(void *buf_priv);

static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size)
{
    struct vb2_dma_sg_buf *buf;
    int i;

    buf = kzalloc(sizeof *buf, GFP_KERNEL);
    if (!buf)
        return NULL;

    buf->vaddr = NULL;
    buf->write = 0;
    buf->offset = 0;
    buf->sg_desc.size = size;
    buf->sg_desc.num_pages = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;

    buf->sg_desc.sglist = vzalloc(buf->sg_desc.num_pages *
                      sizeof(*buf->sg_desc.sglist));
    if (!buf->sg_desc.sglist)
        goto fail_sglist_alloc;
    sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);

    buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
                 GFP_KERNEL);
    if (!buf->pages)
        goto fail_pages_array_alloc;

    for (i = 0; i < buf->sg_desc.num_pages; ++i) {
        buf->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO | __GFP_NOWARN);
        if (NULL == buf->pages[i])
            goto fail_pages_alloc;
        sg_set_page(&buf->sg_desc.sglist[i],
                buf->pages[i], PAGE_SIZE, 0);
    }

    buf->handler.refcount = &buf->refcount;
    buf->handler.put = vb2_dma_sg_put;
    buf->handler.arg = buf;

    atomic_inc(&buf->refcount);

    printk(KERN_DEBUG "%s: Allocated buffer of %d pages\n",
        __func__, buf->sg_desc.num_pages);
    return buf;

fail_pages_alloc:
    while (--i >= 0)
        __free_page(buf->pages[i]);
    kfree(buf->pages);

fail_pages_array_alloc:
    vfree(buf->sg_desc.sglist);

fail_sglist_alloc:
    kfree(buf);
    return NULL;
}

static void vb2_dma_sg_put(void *buf_priv)
{
    struct vb2_dma_sg_buf *buf = buf_priv;
    int i = buf->sg_desc.num_pages;

    if (atomic_dec_and_test(&buf->refcount)) {
        printk(KERN_DEBUG "%s: Freeing buffer of %d pages\n", __func__,
            buf->sg_desc.num_pages);
        if (buf->vaddr)
            vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
        vfree(buf->sg_desc.sglist);
        while (--i >= 0)
            __free_page(buf->pages[i]);
        kfree(buf->pages);
        kfree(buf);
    }
}

static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
                    unsigned long size, int write)
{
    struct vb2_dma_sg_buf *buf;
    unsigned long first, last;
    int num_pages_from_user, i;

    buf = kzalloc(sizeof *buf, GFP_KERNEL);
    if (!buf)
        return NULL;

    buf->vaddr = NULL;
    buf->write = write;
    buf->offset = vaddr & ~PAGE_MASK;
    buf->sg_desc.size = size;

    first = (vaddr           & PAGE_MASK) >> PAGE_SHIFT;
    last  = ((vaddr + size - 1) & PAGE_MASK) >> PAGE_SHIFT;
    buf->sg_desc.num_pages = last - first + 1;

    buf->sg_desc.sglist = vzalloc(
        buf->sg_desc.num_pages * sizeof(*buf->sg_desc.sglist));
    if (!buf->sg_desc.sglist)
        goto userptr_fail_sglist_alloc;

    sg_init_table(buf->sg_desc.sglist, buf->sg_desc.num_pages);

    buf->pages = kzalloc(buf->sg_desc.num_pages * sizeof(struct page *),
                 GFP_KERNEL);
    if (!buf->pages)
        goto userptr_fail_pages_array_alloc;

    num_pages_from_user = get_user_pages(current, current->mm,
                         vaddr & PAGE_MASK,
                         buf->sg_desc.num_pages,
                         write,
                         1, /* force */
                         buf->pages,
                         NULL);

    if (num_pages_from_user != buf->sg_desc.num_pages)
        goto userptr_fail_get_user_pages;

    sg_set_page(&buf->sg_desc.sglist[0], buf->pages[0],
            PAGE_SIZE - buf->offset, buf->offset);
    size -= PAGE_SIZE - buf->offset;
    for (i = 1; i < buf->sg_desc.num_pages; ++i) {
        sg_set_page(&buf->sg_desc.sglist[i], buf->pages[i],
                min_t(size_t, PAGE_SIZE, size), 0);
        size -= min_t(size_t, PAGE_SIZE, size);
    }
    return buf;

userptr_fail_get_user_pages:
    printk(KERN_DEBUG "get_user_pages requested/got: %d/%d]\n",
           num_pages_from_user, buf->sg_desc.num_pages);
    while (--num_pages_from_user >= 0)
        put_page(buf->pages[num_pages_from_user]);
    kfree(buf->pages);

userptr_fail_pages_array_alloc:
    vfree(buf->sg_desc.sglist);

userptr_fail_sglist_alloc:
    kfree(buf);
    return NULL;
}

/*
 * @put_userptr: inform the allocator that a USERPTR buffer will no longer
 *       be used
 */
static void vb2_dma_sg_put_userptr(void *buf_priv)
{
    struct vb2_dma_sg_buf *buf = buf_priv;
    int i = buf->sg_desc.num_pages;

    printk(KERN_DEBUG "%s: Releasing userspace buffer of %d pages\n",
           __func__, buf->sg_desc.num_pages);
    if (buf->vaddr)
        vm_unmap_ram(buf->vaddr, buf->sg_desc.num_pages);
    while (--i >= 0) {
        if (buf->write)
            set_page_dirty_lock(buf->pages[i]);
        put_page(buf->pages[i]);
    }
    vfree(buf->sg_desc.sglist);
    kfree(buf->pages);
    kfree(buf);
}

static void *vb2_dma_sg_vaddr(void *buf_priv)
{
    struct vb2_dma_sg_buf *buf = buf_priv;

    BUG_ON(!buf);

    if (!buf->vaddr)
        buf->vaddr = vm_map_ram(buf->pages,
                    buf->sg_desc.num_pages,
                    -1,
                    PAGE_KERNEL);

    /* add offset in case userptr is not page-aligned */
    return buf->vaddr + buf->offset;
}

static unsigned int vb2_dma_sg_num_users(void *buf_priv)
{
    struct vb2_dma_sg_buf *buf = buf_priv;

    return atomic_read(&buf->refcount);
}

static int vb2_dma_sg_mmap(void *buf_priv, struct vm_area_struct *vma)
{
    struct vb2_dma_sg_buf *buf = buf_priv;
    unsigned long uaddr = vma->vm_start;
    unsigned long usize = vma->vm_end - vma->vm_start;
    int i = 0;

    if (!buf) {
        printk(KERN_ERR "No memory to map\n");
        return -EINVAL;
    }

    do {
        int ret;

        ret = vm_insert_page(vma, uaddr, buf->pages[i++]);
        if (ret) {
            printk(KERN_ERR "Remapping memory, error: %d\n", ret);
            return ret;
        }

        uaddr += PAGE_SIZE;
        usize -= PAGE_SIZE;
    } while (usize > 0);


    /*
     * Use common vm_area operations to track buffer refcount.
     */
    vma->vm_private_data    = &buf->handler;
    vma->vm_ops     = &vb2_common_vm_ops;

    vma->vm_ops->open(vma);

    return 0;
}

static void *vb2_dma_sg_cookie(void *buf_priv)
{
    struct vb2_dma_sg_buf *buf = buf_priv;

    return &buf->sg_desc;
}

const struct vb2_mem_ops vb2_dma_sg_memops = {
    .alloc      = vb2_dma_sg_alloc,
    .put        = vb2_dma_sg_put,
    .get_userptr    = vb2_dma_sg_get_userptr,
    .put_userptr    = vb2_dma_sg_put_userptr,
    .vaddr      = vb2_dma_sg_vaddr,
    .mmap       = vb2_dma_sg_mmap,
    .num_users  = vb2_dma_sg_num_users,
    .cookie     = vb2_dma_sg_cookie,
};
EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);

MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
MODULE_AUTHOR("Andrzej Pietrasiewicz");
MODULE_LICENSE("GPL");