videobuf.c

#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt

#include <linux/spinlock.h>
#include <linux/vmalloc.h>
#include <media/videobuf2-core.h>
#include <media/videobuf2-vmalloc.h>

#include "videobuf.h"

static inline int init_vcam_in_buffer(struct vcam_in_buffer *buf, size_t size)
{
    buf->data = vmalloc(size);
    if (!buf->data) {
        pr_err("Failed to allocate input buffer\n");
        return -ENOMEM;
    }
    buf->filled = 0;
    return 0;
}

static inline void destroy_vcam_in_buffer(struct vcam_in_buffer *buf)
{
    vfree(buf->data);
}

void swap_in_queue_buffers(struct vcam_in_queue *q)
{
    struct vcam_in_buffer *tmp;
    if (!q)
        return;
    tmp = q->pending;
    q->pending = q->ready;
    q->ready = tmp;
    q->pending->filled = 0;
}

int vcam_in_queue_setup(struct vcam_in_queue *q, size_t size)
{
    int i;
    int ret = 0;

    /* Initialize buffers */
    for (i = 0; i < 2; i++) {
        ret = init_vcam_in_buffer(&q->buffers[i], size);
        if (ret)
            break;
    }

    if (ret) {
        pr_err("input queue alloc failure\n");
        for (; i > 0; i--)
            destroy_vcam_in_buffer(&q->buffers[i - 1]);
        return ret;
    }

    /* Initialize dummy buffer */
    memset(&q->dummy, 0x00, sizeof(struct vcam_in_buffer));

    /* Initialize pointers to buffers */
    q->pending = &q->buffers[0];
    q->ready = &q->buffers[1];

    return ret;
}

void vcam_in_queue_destroy(struct vcam_in_queue *q)
{
    int i;
    for (i = 0; i < 2; i++)
        destroy_vcam_in_buffer(&q->buffers[i]);
}

static int vcam_out_queue_setup(struct vb2_queue *vq,
                                unsigned int *nbuffers,
                                unsigned int *nplanes,
                                unsigned int sizes[],
                                struct device *alloc_ctxs[])
{
    int i;
    struct vcam_device *dev = vb2_get_drv_priv(vq);
    unsigned long size = dev->output_format.sizeimage;

    if (*nbuffers < 2)
        *nbuffers = 2;

    *nplanes = 1;

    sizes[0] = size;
    for (i = 1; i < VB2_MAX_PLANES; ++i)
        sizes[i] = 0;
    pr_debug("queue_setup completed\n");
    return 0;
}

static int vcam_out_buffer_prepare(struct vb2_buffer *vb)
{
    struct vcam_device *dev = vb2_get_drv_priv(vb->vb2_queue);
    unsigned long size = dev->output_format.sizeimage;
    if (vb2_plane_size(vb, 0) < size) {
        pr_err(KERN_ERR "data will not fit into buffer\n");
        return -EINVAL;
    }

    vb2_set_plane_payload(vb, 0, size);
    return 0;
}

static void vcam_out_buffer_queue(struct vb2_buffer *vb)
{
    unsigned long flags = 0;
    struct vcam_device *dev = vb2_get_drv_priv(vb->vb2_queue);
    struct vcam_out_buffer *buf = container_of(vb, struct vcam_out_buffer, vb);
    struct vcam_out_queue *q = &dev->vcam_out_vidq;
    buf->filled = 0;

    spin_lock_irqsave(&dev->out_q_slock, flags);
    list_add_tail(&buf->list, &q->active);
    spin_unlock_irqrestore(&dev->out_q_slock, flags);
}

static int vcam_start_streaming(struct vb2_queue *q, unsigned int count)
{
    struct vcam_device *dev = q->drv_priv;

    /* Try to start kernel thread */
    dev->sub_thr_id = kthread_create(submitter_thread, dev, "vcam_submitter");
    if (!dev->sub_thr_id) {
        pr_err("Failed to create kernel thread\n");
        return -ECANCELED;
    }

    wake_up_process(dev->sub_thr_id);

    return 0;
}

static void vcam_stop_streaming(struct vb2_queue *vb2_q)
{
    struct vcam_device *dev = vb2_q->drv_priv;
    struct vcam_out_queue *q = &dev->vcam_out_vidq;
    unsigned long flags = 0;

    /* Stop running threads */
    if (dev->sub_thr_id)
        kthread_stop(dev->sub_thr_id);

    dev->sub_thr_id = NULL;
    /* Empty buffer queue */
    spin_lock_irqsave(&dev->out_q_slock, flags);
    while (!list_empty(&q->active)) {
        struct vcam_out_buffer *buf =
            list_entry(q->active.next, struct vcam_out_buffer, list);
        list_del(&buf->list);
        vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
        pr_debug("Throwing out buffer\n");
    }
    spin_unlock_irqrestore(&dev->out_q_slock, flags);
}

static void vcam_outbuf_lock(struct vb2_queue *vq)
{
    struct vcam_device *dev = vb2_get_drv_priv(vq);
    mutex_lock(&dev->vcam_mutex);
}

static void vcam_outbuf_unlock(struct vb2_queue *vq)
{
    struct vcam_device *dev = vb2_get_drv_priv(vq);
    mutex_unlock(&dev->vcam_mutex);
}

static const struct vb2_ops vcam_vb2_ops = {
    .queue_setup = vcam_out_queue_setup,
    .buf_prepare = vcam_out_buffer_prepare,
    .buf_queue = vcam_out_buffer_queue,
    .start_streaming = vcam_start_streaming,
    .stop_streaming = vcam_stop_streaming,
    .wait_prepare = vcam_outbuf_unlock,
    .wait_finish = vcam_outbuf_lock,
};

int vcam_out_videobuf2_setup(struct vcam_device *dev)
{
    struct vb2_queue *q = &dev->vb_out_vidq;

    q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
    q->drv_priv = dev;
    q->buf_struct_size = sizeof(struct vcam_out_buffer);
    q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
    q->ops = &vcam_vb2_ops;
    q->mem_ops = &vb2_vmalloc_memops;
    q->min_buffers_needed = 2;
    q->lock = &dev->vcam_mutex;

    return vb2_queue_init(q);
}