// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright (C) 2025 Advanced Micro Devices, Inc.
 */

#include <media/v4l2-ioctl.h>
#include <media/v4l2-mc.h>
#include <media/videobuf2-vmalloc.h>

#include "isp4_interface.h"
#include "isp4_subdev.h"
#include "isp4_video.h"

#define ISP4VID_ISP_DRV_NAME "amd_isp_capture"
#define ISP4VID_MAX_PREVIEW_FPS 30
#define ISP4VID_DEFAULT_FMT V4L2_PIX_FMT_NV12

#define ISP4VID_PAD_VIDEO_OUTPUT 0

/* time perframe default */
#define ISP4VID_ISP_TPF_DEFAULT isp4vid_tpfs[0]

static const char *const isp4vid_video_dev_name = "Preview";

/* Sizes must be in increasing order */
static const struct v4l2_frmsize_discrete isp4vid_frmsize[] = {
        {640, 360},
        {640, 480},
        {1280, 720},
        {1280, 960},
        {1920, 1080},
        {1920, 1440},
        {2560, 1440},
        {2880, 1620},
        {2880, 1624},
        {2888, 1808},
};

static const u32 isp4vid_formats[] = {
        V4L2_PIX_FMT_NV12,
        V4L2_PIX_FMT_YUYV
};

/* time perframe list */
static const struct v4l2_fract isp4vid_tpfs[] = {
        { 1, ISP4VID_MAX_PREVIEW_FPS }
};

void isp4vid_handle_frame_done(struct isp4vid_dev *isp_vdev,
                               const struct isp4if_img_buf_info *img_buf)
{
        struct isp4vid_capture_buffer *isp4vid_buf;
        void *vbuf;

        scoped_guard(mutex, &isp_vdev->buf_list_lock) {
                isp4vid_buf = list_first_entry_or_null(&isp_vdev->buf_list,
                                                       typeof(*isp4vid_buf),
                                                       list);
                if (!isp4vid_buf)
                        return;

                vbuf = vb2_plane_vaddr(&isp4vid_buf->vb2.vb2_buf, 0);

                if (vbuf != img_buf->planes[0].sys_addr) {
                        dev_err(isp_vdev->dev, "Invalid vbuf\n");
                        return;
                }

                list_del(&isp4vid_buf->list);
        }

        /* Fill the buffer */
        isp4vid_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
        isp4vid_buf->vb2.sequence = isp_vdev->sequence++;
        isp4vid_buf->vb2.field = V4L2_FIELD_ANY;

        vb2_set_plane_payload(&isp4vid_buf->vb2.vb2_buf,
                              0, isp_vdev->format.sizeimage);

        vb2_buffer_done(&isp4vid_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);

        dev_dbg(isp_vdev->dev, "call vb2_buffer_done(size=%u)\n",
                isp_vdev->format.sizeimage);
}

static const struct v4l2_pix_format isp4vid_fmt_default = {
        .width = 1920,
        .height = 1080,
        .pixelformat = ISP4VID_DEFAULT_FMT,
        .field = V4L2_FIELD_NONE,
        .colorspace = V4L2_COLORSPACE_SRGB,
};

static void isp4vid_capture_return_all_buffers(struct isp4vid_dev *isp_vdev,
                                               enum vb2_buffer_state state)
{
        struct isp4vid_capture_buffer *vbuf, *node;

        scoped_guard(mutex, &isp_vdev->buf_list_lock) {
                list_for_each_entry_safe(vbuf, node, &isp_vdev->buf_list, list)
                        vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
                INIT_LIST_HEAD(&isp_vdev->buf_list);
        }

        dev_dbg(isp_vdev->dev, "call vb2_buffer_done(%d)\n", state);
}

static int isp4vid_vdev_link_validate(struct media_link *link)
{
        return 0;
}

static const struct media_entity_operations isp4vid_vdev_ent_ops = {
        .link_validate = isp4vid_vdev_link_validate,
};

static const struct v4l2_file_operations isp4vid_vdev_fops = {
        .owner = THIS_MODULE,
        .open = v4l2_fh_open,
        .release = vb2_fop_release,
        .read = vb2_fop_read,
        .poll = vb2_fop_poll,
        .unlocked_ioctl = video_ioctl2,
        .mmap = vb2_fop_mmap,
};

static int isp4vid_ioctl_querycap(struct file *file, void *fh,
                                  struct v4l2_capability *cap)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);

        strscpy(cap->driver, ISP4VID_ISP_DRV_NAME, sizeof(cap->driver));
        snprintf(cap->card, sizeof(cap->card), "%s", ISP4VID_ISP_DRV_NAME);
        cap->capabilities |= V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;

        dev_dbg(isp_vdev->dev, "%s|capabilities=0x%X\n", isp_vdev->vdev.name,
                cap->capabilities);

        return 0;
}

static int isp4vid_g_fmt_vid_cap(struct file *file, void *priv,
                                 struct v4l2_format *f)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);

        f->fmt.pix = isp_vdev->format;

        return 0;
}

static int isp4vid_fill_buffer_size(struct v4l2_pix_format *fmt)
{
        int ret = 0;

        switch (fmt->pixelformat) {
        case V4L2_PIX_FMT_NV12:
                fmt->bytesperline = fmt->width;
                fmt->sizeimage = fmt->bytesperline * fmt->height * 3 / 2;
                break;
        case V4L2_PIX_FMT_YUYV:
                fmt->bytesperline = fmt->width * 2;
                fmt->sizeimage = fmt->bytesperline * fmt->height;
                break;
        default:
                ret = -EINVAL;
                break;
        }

        return ret;
}

static int isp4vid_try_fmt_vid_cap(struct file *file, void *priv,
                                   struct v4l2_format *f)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);
        struct v4l2_pix_format *format = &f->fmt.pix;
        const struct v4l2_frmsize_discrete *fsz;
        size_t i;

        /*
         * Check if the hardware supports the requested format, use the default
         * format otherwise.
         */
        for (i = 0; i < ARRAY_SIZE(isp4vid_formats); i++)
                if (isp4vid_formats[i] == format->pixelformat)
                        break;

        if (i == ARRAY_SIZE(isp4vid_formats))
                format->pixelformat = ISP4VID_DEFAULT_FMT;

        switch (format->pixelformat) {
        case V4L2_PIX_FMT_NV12:
        case V4L2_PIX_FMT_YUYV:
                fsz = v4l2_find_nearest_size(isp4vid_frmsize,
                                             ARRAY_SIZE(isp4vid_frmsize),
                                             width, height, format->width,
                                             format->height);
                format->width = fsz->width;
                format->height = fsz->height;
                break;
        default:
                dev_err(isp_vdev->dev, "%s|unsupported fmt=%u\n",
                        isp_vdev->vdev.name,
                        format->pixelformat);
                return -EINVAL;
        }

        /*
         * There is no need to check the return value, as failure will never
         * happen here
         */
        isp4vid_fill_buffer_size(format);

        if (format->field == V4L2_FIELD_ANY)
                format->field = isp4vid_fmt_default.field;

        if (format->colorspace == V4L2_COLORSPACE_DEFAULT)
                format->colorspace = isp4vid_fmt_default.colorspace;

        return 0;
}

static int isp4vid_set_fmt_2_isp(struct v4l2_subdev *sdev,
                                 struct v4l2_pix_format *pix_fmt)
{
        struct v4l2_subdev_format fmt = {};

        switch (pix_fmt->pixelformat) {
        case V4L2_PIX_FMT_NV12:
                fmt.format.code = MEDIA_BUS_FMT_YUYV8_1_5X8;
                break;
        case V4L2_PIX_FMT_YUYV:
                fmt.format.code = MEDIA_BUS_FMT_YUYV8_1X16;
                break;
        default:
                return -EINVAL;
        }
        fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
        fmt.pad = ISP4VID_PAD_VIDEO_OUTPUT;
        fmt.format.width = pix_fmt->width;
        fmt.format.height = pix_fmt->height;
        return v4l2_subdev_call(sdev, pad, set_fmt, NULL, &fmt);
}

static int isp4vid_s_fmt_vid_cap(struct file *file, void *priv,
                                 struct v4l2_format *f)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);
        int ret;

        /* Do not change the format while stream is on */
        if (vb2_is_busy(&isp_vdev->vbq))
                return -EBUSY;

        ret = isp4vid_try_fmt_vid_cap(file, priv, f);
        if (ret)
                return ret;

        dev_dbg(isp_vdev->dev, "%s|width height:%ux%u->%ux%u\n",
                isp_vdev->vdev.name,
                isp_vdev->format.width, isp_vdev->format.height,
                f->fmt.pix.width, f->fmt.pix.height);
        dev_dbg(isp_vdev->dev, "%s|pixelformat:0x%x-0x%x\n",
                isp_vdev->vdev.name, isp_vdev->format.pixelformat,
                f->fmt.pix.pixelformat);
        dev_dbg(isp_vdev->dev, "%s|bytesperline:%u->%u\n",
                isp_vdev->vdev.name, isp_vdev->format.bytesperline,
                f->fmt.pix.bytesperline);
        dev_dbg(isp_vdev->dev, "%s|sizeimage:%u->%u\n",
                isp_vdev->vdev.name, isp_vdev->format.sizeimage,
                f->fmt.pix.sizeimage);

        isp_vdev->format = f->fmt.pix;
        ret = isp4vid_set_fmt_2_isp(isp_vdev->isp_sdev, &isp_vdev->format);

        return ret;
}

static int isp4vid_enum_fmt_vid_cap(struct file *file, void *priv,
                                    struct v4l2_fmtdesc *f)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);

        switch (f->index) {
        case 0:
                f->pixelformat = V4L2_PIX_FMT_NV12;
                break;
        case 1:
                f->pixelformat = V4L2_PIX_FMT_YUYV;
                break;
        default:
                return -EINVAL;
        }

        dev_dbg(isp_vdev->dev, "%s|index=%d, pixelformat=0x%X\n",
                isp_vdev->vdev.name, f->index, f->pixelformat);

        return 0;
}

static int isp4vid_enum_framesizes(struct file *file, void *fh,
                                   struct v4l2_frmsizeenum *fsize)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);
        unsigned int i;

        for (i = 0; i < ARRAY_SIZE(isp4vid_formats); i++) {
                if (isp4vid_formats[i] == fsize->pixel_format)
                        break;
        }

        if (i == ARRAY_SIZE(isp4vid_formats))
                return -EINVAL;

        if (fsize->index < ARRAY_SIZE(isp4vid_frmsize)) {
                fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
                fsize->discrete = isp4vid_frmsize[fsize->index];
                dev_dbg(isp_vdev->dev, "%s|size[%d]=%dx%d\n",
                        isp_vdev->vdev.name, fsize->index,
                        fsize->discrete.width, fsize->discrete.height);
        } else {
                return -EINVAL;
        }

        return 0;
}

static int isp4vid_ioctl_enum_frameintervals(struct file *file, void *priv,
                                             struct v4l2_frmivalenum *fival)
{
        struct isp4vid_dev *isp_vdev = video_drvdata(file);
        size_t i;

        if (fival->index >= ARRAY_SIZE(isp4vid_tpfs))
                return -EINVAL;

        for (i = 0; i < ARRAY_SIZE(isp4vid_formats); i++)
                if (isp4vid_formats[i] == fival->pixel_format)
                        break;

        if (i == ARRAY_SIZE(isp4vid_formats))
                return -EINVAL;

        for (i = 0; i < ARRAY_SIZE(isp4vid_frmsize); i++)
                if (isp4vid_frmsize[i].width == fival->width &&
                    isp4vid_frmsize[i].height == fival->height)
                        break;

        if (i == ARRAY_SIZE(isp4vid_frmsize))
                return -EINVAL;

        fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
        fival->discrete = isp4vid_tpfs[fival->index];
        v4l2_simplify_fraction(&fival->discrete.numerator,
                               &fival->discrete.denominator, 8, 333);

        dev_dbg(isp_vdev->dev, "%s|interval[%d]=%d/%d\n",
                isp_vdev->vdev.name, fival->index,
                fival->discrete.numerator,
                fival->discrete.denominator);

        return 0;
}

static int isp4vid_ioctl_g_param(struct file *file, void *priv,
                                 struct v4l2_streamparm *param)
{
        struct v4l2_captureparm *capture = &param->parm.capture;
        struct isp4vid_dev *isp_vdev = video_drvdata(file);

        if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
                return -EINVAL;

        capture->capability   = V4L2_CAP_TIMEPERFRAME;
        capture->timeperframe = isp_vdev->timeperframe;
        capture->readbuffers  = 0;

        dev_dbg(isp_vdev->dev, "%s|timeperframe=%d/%d\n", isp_vdev->vdev.name,
                capture->timeperframe.numerator,
                capture->timeperframe.denominator);

        return 0;
}

static const struct v4l2_ioctl_ops isp4vid_vdev_ioctl_ops = {
        .vidioc_querycap            = isp4vid_ioctl_querycap,
        .vidioc_enum_fmt_vid_cap    = isp4vid_enum_fmt_vid_cap,
        .vidioc_g_fmt_vid_cap       = isp4vid_g_fmt_vid_cap,
        .vidioc_s_fmt_vid_cap       = isp4vid_s_fmt_vid_cap,
        .vidioc_try_fmt_vid_cap     = isp4vid_try_fmt_vid_cap,
        .vidioc_reqbufs             = vb2_ioctl_reqbufs,
        .vidioc_querybuf            = vb2_ioctl_querybuf,
        .vidioc_qbuf                = vb2_ioctl_qbuf,
        .vidioc_expbuf              = vb2_ioctl_expbuf,
        .vidioc_dqbuf               = vb2_ioctl_dqbuf,
        .vidioc_create_bufs         = vb2_ioctl_create_bufs,
        .vidioc_prepare_buf         = vb2_ioctl_prepare_buf,
        .vidioc_streamon            = vb2_ioctl_streamon,
        .vidioc_streamoff           = vb2_ioctl_streamoff,
        .vidioc_g_parm              = isp4vid_ioctl_g_param,
        .vidioc_s_parm              = isp4vid_ioctl_g_param,
        .vidioc_enum_framesizes     = isp4vid_enum_framesizes,
        .vidioc_enum_frameintervals = isp4vid_ioctl_enum_frameintervals,
};

static unsigned int isp4vid_get_image_size(struct v4l2_pix_format *fmt)
{
        switch (fmt->pixelformat) {
        case V4L2_PIX_FMT_NV12:
                return fmt->width * fmt->height * 3 / 2;
        case V4L2_PIX_FMT_YUYV:
                return fmt->width * fmt->height * 2;
        default:
                return 0;
        }
}

static int isp4vid_qops_queue_setup(struct vb2_queue *vq,
                                    unsigned int *nbuffers,
                                    unsigned int *nplanes, unsigned int sizes[],
                                    struct device *alloc_devs[])
{
        struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vq);
        unsigned int q_num_bufs = vb2_get_num_buffers(vq);

        if (*nplanes > 1) {
                dev_err(isp_vdev->dev,
                        "fail to setup queue, no mplane supported %u\n",
                        *nplanes);
                return -EINVAL;
        }

        if (*nplanes == 1) {
                unsigned int size;

                size = isp4vid_get_image_size(&isp_vdev->format);
                if (sizes[0] < size) {
                        dev_err(isp_vdev->dev,
                                "fail for small plane size %u, %u expected\n",
                                sizes[0], size);
                        return -EINVAL;
                }
        }

        if (q_num_bufs + *nbuffers < ISP4IF_MAX_STREAM_BUF_COUNT)
                *nbuffers = ISP4IF_MAX_STREAM_BUF_COUNT - q_num_bufs;

        switch (isp_vdev->format.pixelformat) {
        case V4L2_PIX_FMT_NV12:
        case V4L2_PIX_FMT_YUYV: {
                *nplanes = 1;
                sizes[0] = max(sizes[0], isp_vdev->format.sizeimage);
                isp_vdev->format.sizeimage = sizes[0];
        }
        break;
        default:
                dev_err(isp_vdev->dev, "%s|unsupported fmt=%u\n",
                        isp_vdev->vdev.name, isp_vdev->format.pixelformat);
                return -EINVAL;
        }

        dev_dbg(isp_vdev->dev, "%s|*nbuffers=%u *nplanes=%u sizes[0]=%u\n",
                isp_vdev->vdev.name,
                *nbuffers, *nplanes, sizes[0]);

        return 0;
}

static void isp4vid_qops_buffer_queue(struct vb2_buffer *vb)
{
        struct isp4vid_capture_buffer *buf =
                container_of(vb, struct isp4vid_capture_buffer, vb2.vb2_buf);
        struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vb->vb2_queue);
        struct isp4if_img_buf_info *img_buf = &buf->img_buf;
        void *vaddr = vb2_plane_vaddr(vb, 0);

        dev_dbg(isp_vdev->dev, "queue buf, vaddr %p, gpuva 0x%llx, size %u\n",
                vaddr, buf->gpu_addr, vb->planes[0].length);

        switch (isp_vdev->format.pixelformat) {
        case V4L2_PIX_FMT_NV12: {
                u32 y_size = isp_vdev->format.sizeimage / 3 * 2;
                u32 uv_size = isp_vdev->format.sizeimage / 3;

                img_buf->planes[0].len = y_size;
                img_buf->planes[0].sys_addr = vaddr;
                img_buf->planes[0].mc_addr = buf->gpu_addr;

                dev_dbg(isp_vdev->dev, "img_buf[0]: mc=0x%llx size=%u\n",
                        img_buf->planes[0].mc_addr,
                        img_buf->planes[0].len);

                img_buf->planes[1].len = uv_size;
                img_buf->planes[1].sys_addr = vaddr + y_size;
                img_buf->planes[1].mc_addr = buf->gpu_addr + y_size;

                dev_dbg(isp_vdev->dev, "img_buf[1]: mc=0x%llx size=%u\n",
                        img_buf->planes[1].mc_addr,
                        img_buf->planes[1].len);

                img_buf->planes[2].len = 0;
        }
        break;
        case V4L2_PIX_FMT_YUYV: {
                img_buf->planes[0].len = isp_vdev->format.sizeimage;
                img_buf->planes[0].sys_addr = vaddr;
                img_buf->planes[0].mc_addr = buf->gpu_addr;

                dev_dbg(isp_vdev->dev, "img_buf[0]: mc=0x%llx size=%u\n",
                        img_buf->planes[0].mc_addr,
                        img_buf->planes[0].len);

                img_buf->planes[1].len = 0;
                img_buf->planes[2].len = 0;
        }
        break;
        default:
                dev_err(isp_vdev->dev, "%s|unsupported fmt=%u\n",
                        isp_vdev->vdev.name, isp_vdev->format.pixelformat);
                return;
        }

        if (isp_vdev->stream_started)
                isp4sd_ioc_send_img_buf(isp_vdev->isp_sdev, img_buf);

        scoped_guard(mutex, &isp_vdev->buf_list_lock)
                list_add_tail(&buf->list, &isp_vdev->buf_list);
}

static int isp4vid_qops_start_streaming(struct vb2_queue *vq,
                                        unsigned int count)
{
        struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vq);
        struct isp4vid_capture_buffer *isp4vid_buf;
        struct media_entity *entity;
        struct v4l2_subdev *subdev;
        struct media_pad *pad;
        int ret = 0;

        isp_vdev->sequence = 0;

        ret = isp4sd_pwron_and_init(isp_vdev->isp_sdev);
        if (ret) {
                dev_err(isp_vdev->dev, "power up isp fail %d\n", ret);
                goto release_buffers;
        }

        entity = &isp_vdev->vdev.entity;
        while (1) {
                pad = &entity->pads[0];
                if (!(pad->flags & MEDIA_PAD_FL_SINK))
                        break;

                pad = media_pad_remote_pad_first(pad);
                if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
                        break;

                entity = pad->entity;
                subdev = media_entity_to_v4l2_subdev(entity);

                ret = v4l2_subdev_call(subdev, video, s_stream, 1);
                if (ret < 0 && ret != -ENOIOCTLCMD) {
                        dev_dbg(isp_vdev->dev, "fail start streaming: %s %d\n",
                                subdev->name, ret);
                        goto release_buffers;
                }
        }

        list_for_each_entry(isp4vid_buf, &isp_vdev->buf_list, list)
                isp4sd_ioc_send_img_buf(isp_vdev->isp_sdev,
                                        &isp4vid_buf->img_buf);

        isp_vdev->stream_started = true;

        return 0;

release_buffers:
        isp4vid_capture_return_all_buffers(isp_vdev, VB2_BUF_STATE_QUEUED);
        return ret;
}

static void isp4vid_qops_stop_streaming(struct vb2_queue *vq)
{
        struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vq);
        struct media_entity *entity;
        struct v4l2_subdev *subdev;
        struct media_pad *pad;
        int ret;

        entity = &isp_vdev->vdev.entity;
        while (1) {
                pad = &entity->pads[0];
                if (!(pad->flags & MEDIA_PAD_FL_SINK))
                        break;

                pad = media_pad_remote_pad_first(pad);
                if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
                        break;

                entity = pad->entity;
                subdev = media_entity_to_v4l2_subdev(entity);

                ret = v4l2_subdev_call(subdev, video, s_stream, 0);

                if (ret < 0 && ret != -ENOIOCTLCMD)
                        dev_dbg(isp_vdev->dev, "fail stop streaming: %s %d\n",
                                subdev->name, ret);
        }

        isp_vdev->stream_started = false;
        isp4sd_pwroff_and_deinit(isp_vdev->isp_sdev);

        /* Release all active buffers */
        isp4vid_capture_return_all_buffers(isp_vdev, VB2_BUF_STATE_ERROR);
}

static int isp4vid_qops_buf_init(struct vb2_buffer *vb)
{
        struct isp4vid_capture_buffer *buf =
                container_of(vb, struct isp4vid_capture_buffer, vb2.vb2_buf);
        struct isp4vid_dev *isp_vdev = vb2_get_drv_priv(vb->vb2_queue);
        void *mem_priv = vb->planes[0].mem_priv;
        struct device *dev = isp_vdev->dev;
        u64 gpu_addr;
        void *bo;
        int ret;

        if (vb->planes[0].dbuf) {
                buf->dbuf = vb->planes[0].dbuf;
        } else {
                /*
                 * HAS_DMA is a Kconfig dependency so CONFIG_HAS_DMA is always
                 * defined when this driver is compiled. The #else branch is
                 * kept as a safeguard in case the dependency is ever removed.
                 */
#ifdef CONFIG_HAS_DMA
                buf->dbuf = vb2_vmalloc_memops.get_dmabuf(vb, mem_priv, 0);
                if (IS_ERR_OR_NULL(buf->dbuf)) {
                        dev_err(dev, "fail to get dma buf\n");
                        return -EINVAL;
                }
#else
                dev_err(dev, "get dmabuf fail -- CONFIG_HAS_DMA not defined\n");
                buf->dbuf = NULL;
                return -EINVAL;
#endif
        }

        /* create isp user BO and obtain gpu_addr */
        ret = isp_user_buffer_alloc(dev, buf->dbuf, &bo, &gpu_addr);
        if (ret) {
                dev_err(dev, "fail to create isp user BO\n");
                if (!vb->planes[0].dbuf) {
                        dma_buf_put(buf->dbuf);
                        buf->dbuf = NULL;
                }

                return ret;
        }

        buf->bo = bo;
        buf->gpu_addr = gpu_addr;
        return 0;
}

static void isp4vid_qops_buf_cleanup(struct vb2_buffer *vb)
{
        struct isp4vid_capture_buffer *buf =
                container_of(vb, struct isp4vid_capture_buffer, vb2.vb2_buf);

        if (buf->bo) {
                isp_user_buffer_free(buf->bo);
                buf->bo = NULL;
        }

        /*
         * Only put dmabufs we obtained ourselves via get_dmabuf, not ones
         * provided by the framework for DMABUF import
         */
        if (buf->dbuf && buf->dbuf != vb->planes[0].dbuf)
                dma_buf_put(buf->dbuf);

        buf->dbuf = NULL;
}

static const struct vb2_ops isp4vid_qops = {
        .queue_setup = isp4vid_qops_queue_setup,
        .buf_init = isp4vid_qops_buf_init,
        .buf_cleanup = isp4vid_qops_buf_cleanup,
        .start_streaming = isp4vid_qops_start_streaming,
        .stop_streaming = isp4vid_qops_stop_streaming,
        .buf_queue = isp4vid_qops_buffer_queue,
};

int isp4vid_dev_init(struct isp4vid_dev *isp_vdev, struct v4l2_subdev *isp_sd)
{
        const char *vdev_name = isp4vid_video_dev_name;
        struct v4l2_device *v4l2_dev;
        struct video_device *vdev;
        struct vb2_queue *q;
        int ret;

        if (!isp_vdev || !isp_sd || !isp_sd->v4l2_dev)
                return -EINVAL;

        v4l2_dev = isp_sd->v4l2_dev;
        vdev = &isp_vdev->vdev;

        isp_vdev->isp_sdev = isp_sd;
        isp_vdev->dev = v4l2_dev->dev;

        /* Initialize the vb2_queue struct */
        mutex_init(&isp_vdev->vbq_lock);
        q = &isp_vdev->vbq;
        q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        q->io_modes = VB2_MMAP | VB2_DMABUF;
        q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
        q->buf_struct_size = sizeof(struct isp4vid_capture_buffer);
        q->min_queued_buffers = 2;
        q->ops = &isp4vid_qops;
        q->drv_priv = isp_vdev;
        q->mem_ops = &vb2_vmalloc_memops;
        q->lock = &isp_vdev->vbq_lock;
        q->dev = v4l2_dev->dev;
        ret = vb2_queue_init(q);
        if (ret) {
                dev_err(v4l2_dev->dev, "vb2_queue_init error:%d\n", ret);
                return ret;
        }

        /* Initialize buffer list and its lock */
        mutex_init(&isp_vdev->buf_list_lock);
        INIT_LIST_HEAD(&isp_vdev->buf_list);

        /* Set default frame format */
        isp_vdev->format = isp4vid_fmt_default;
        isp_vdev->timeperframe = ISP4VID_ISP_TPF_DEFAULT;
        v4l2_simplify_fraction(&isp_vdev->timeperframe.numerator,
                               &isp_vdev->timeperframe.denominator, 8, 333);

        ret = isp4vid_fill_buffer_size(&isp_vdev->format);
        if (ret) {
                dev_err(v4l2_dev->dev, "fail to fill buffer size: %d\n", ret);
                goto err_release_vb2_queue;
        }

        ret = isp4vid_set_fmt_2_isp(isp_sd, &isp_vdev->format);
        if (ret) {
                dev_err(v4l2_dev->dev, "fail init format :%d\n", ret);
                goto err_release_vb2_queue;
        }

        /* Initialize the video_device struct */
        isp_vdev->vdev.entity.name = vdev_name;
        isp_vdev->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
        isp_vdev->vdev_pad.flags = MEDIA_PAD_FL_SINK;
        ret = media_entity_pads_init(&isp_vdev->vdev.entity, 1,
                                     &isp_vdev->vdev_pad);

        if (ret) {
                dev_err(v4l2_dev->dev, "init media entity pad fail:%d\n", ret);
                goto err_release_vb2_queue;
        }

        vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE |
                            V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
        vdev->entity.ops = &isp4vid_vdev_ent_ops;
        vdev->release = video_device_release_empty;
        vdev->fops = &isp4vid_vdev_fops;
        vdev->ioctl_ops = &isp4vid_vdev_ioctl_ops;
        vdev->lock = NULL;
        vdev->queue = q;
        vdev->v4l2_dev = v4l2_dev;
        vdev->vfl_dir = VFL_DIR_RX;
        strscpy(vdev->name, vdev_name, sizeof(vdev->name));
        video_set_drvdata(vdev, isp_vdev);

        ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
        if (ret) {
                dev_err(v4l2_dev->dev, "register video device fail:%d\n", ret);
                goto err_entity_cleanup;
        }

        return 0;

err_entity_cleanup:
        media_entity_cleanup(&isp_vdev->vdev.entity);
err_release_vb2_queue:
        vb2_queue_release(q);
        return ret;
}

void isp4vid_dev_deinit(struct isp4vid_dev *isp_vdev)
{
        vb2_video_unregister_device(&isp_vdev->vdev);
}